Orientation estimation using accelerometer and gyroscope
18 vues (au cours des 30 derniers jours)
Afficher commentaires plus anciens
Hi!
I have some problem with orientation estimation, I have to calculate orientation on some other ways and compare them to each other. I want to calculate it with strapdown integration, with complementary filter and using Kalman filter. The magnetometer datas should be ignored.
Can maybe anybody help me?
function HupleIMU_postproc
%% User specified parameters
Fs = 100; % Sampling frequency [Hz]
% Sensor LSM9DS1
% Linear acceleration range: +-4g sensitivity --> 0.122 mg/LSB <- Not used now
% Magnetic range: +-4gauss sensitivity --> 0.14 mgauss/LSB
% Angular rate range: +-500dps sensitivity --> 17.50 mdps/LSB
accmax = 4;
magnmax = 4;
gyromax = 500;
accSensitivity = 0.122*10^-3;
magSensitivity = 0.14*10^-3;
gyroSensitivity = 17.50*10^-3;
% Linear acceleration range: +-2g sensitivity --> 0.061 mg/LSB
% Magnetic range: +-4gauss sensitivity --> 0.14 mgauss/LSB
% Angular rate range: +-245dps sensitivity --> 8.75 mdps/LSB
% accmax = 2;
% magnmax = 4;
% gyromax = 245;
% accSensitivity = 0.061*10^-3;
% magSensitivity = 0.14*10^-3;
% gyroSensitivity = 8.75*10^-3;
[name,path]=uigetfile('*.txt','Válaszd ki a fájlt');
file=[path,name];
T= readtable(file);
time = T.SampleTime_s_;
Gyro_1_X_dps_ =T.Gyro_1_X_dps_;
Gyro_1_Y_dps_ =T.Gyro_1_Y_dps_;
Gyro_1_Z_dps_ =T.Gyro_1_Z_dps_;
Acc_1_X_g_ =T.Acc_1_X_g_;
Acc_1_Y_g_ =T.Acc_1_Y_g_;
Acc_1_Z_g_ =T.Acc_1_Z_g_;
Gyro_2_X_dps_ =T.Gyro_2_X_dps_;
Gyro_2_Y_dps_ =T.Gyro_2_Y_dps_;
Gyro_2_Z_dps_ =T.Gyro_2_Z_dps_;
Acc_2_X_g_ =T.Acc_2_X_g_;
Acc_2_Y_g_ =T.Acc_2_Y_g_;
Acc_2_Z_g_ =T.Acc_2_Z_g_;
Gyro_3_X_dps_ =T.Gyro_3_X_dps_;
Gyro_3_Y_dps_ =T.Gyro_3_Y_dps_;
Gyro_3_Z_dps_ =T.Gyro_3_Z_dps_;
Acc_3_X_g_ =T.Acc_3_X_g_;
Acc_3_Y_g_ =T.Acc_3_Y_g_;
Acc_3_Z_g_ =T.Acc_3_Z_g_;
Magn_1_X_gauss_=T.Magn_1_X_gauss_;
Magn_1_Y_gauss_=T.Magn_1_Y_gauss_;
Magn_1_Z_gauss_=T.Magn_1_Z_gauss_;
Magn_2_X_gauss_=T.Magn_2_X_gauss_;
Magn_2_Y_gauss_=T.Magn_2_Y_gauss_;
Magn_2_Z_gauss_=T.Magn_2_Z_gauss_;
Magn_3_X_gauss_=T.Magn_3_X_gauss_;
Magn_3_Y_gauss_=T.Magn_3_Y_gauss_;
Magn_3_Z_gauss_=T.Magn_3_Z_gauss_;
%plot data
figure('Name', ['Accelerations_', file], 'color','w', 'NumberTitle', 'off')
subplot(3,1,1)
title('Accelerometers X direction', 'FontSize', 12)
hold on
plot(time,Acc_1_X_g_,'r-*','MarkerSize',3);
plot(time,Acc_2_X_g_,'g-*','MarkerSize',3);
plot(time,Acc_3_X_g_,'b-*','MarkerSize',3);
ylim([-1 1]*accmax)
xlabel('t(s)')
ylabel('g_x')
legend('1','2','3')
grid on
axis([0 60 -1.5 1.5])
set(gcf,'position',[300,100,800,600])
subplot(3,1,2)
title('Accelerometers Y direction', 'FontSize', 12)
hold on
plot(time,Acc_1_Y_g_,'r-*','MarkerSize',3);
plot(time,Acc_2_Y_g_,'g-*','MarkerSize',3);
plot(time,Acc_3_Y_g_,'b-*','MarkerSize',3);
ylim([-1 1]*accmax)
xlabel('t(s)')
ylabel('g_y')
legend('1','2','3')
grid on
axis([0 60 -2 2])
subplot(3,1,3)
title('Accelerometers Z direction', 'FontSize', 12)
hold on
plot(time,Acc_1_Z_g_,'r-*','MarkerSize',3);
plot(time,Acc_2_Z_g_,'g-*','MarkerSize',3);
plot(time,Acc_3_Z_g_,'b-*','MarkerSize',3);
ylim([-1 1]*accmax)
xlabel('t(s)')
ylabel('g_z')
legend('1','2','3')
grid on
axis([0 60 -3 1])
%
% try
% savefig(gcf, [file, '_Accelerations', '.fig']);
% saveas(gcf,[file, '_Accelerations', '.png']);
% catch Eexception
% msgbox('Figure saving not finished, wait a few seconds before closing it.', 'Warning', 'warn');
% end
%
figure('Name', ['Angular Rates_', file], 'color','w', 'NumberTitle', 'off')
subplot(3,1,1)
title('Gyroscopes', 'FontSize', 12)
hold on
plot(time,Gyro_1_X_dps_,'r-*','MarkerSize',3);
plot(time,Gyro_2_X_dps_,'g-*','MarkerSize',3);
plot(time,Gyro_3_X_dps_,'b-*','MarkerSize',3);
ylim([-1 1]*gyromax)
xlabel('t(s)')
ylabel('dps_x')
legend('1','2','3')
grid on
set(gcf,'position',[300,100,800,600])
axis([0 60 -150 150])
subplot(3,1,2)
hold on
plot(time,Gyro_1_Y_dps_,'r-*','MarkerSize',3);
plot(time,Gyro_2_Y_dps_,'g-*','MarkerSize',3);
plot(time,Gyro_3_Y_dps_,'b-*','MarkerSize',3);
ylim([-1 1]*gyromax)
xlabel('t(s)')
ylabel('dps_y')
legend('1','2','3')
grid on
axis([0 60 -100 100])
subplot(3,1,3)
hold on
plot(time,Gyro_1_Z_dps_,'r-*','MarkerSize',3);
plot(time,Gyro_2_Z_dps_,'g-*','MarkerSize',3);
plot(time,Gyro_3_Z_dps_,'b-*','MarkerSize',3);
ylim([-1 1]*gyromax)
xlabel('t(s)')
ylabel('dps_z')
legend('1','2','3')
grid on
axis([0 60 -100 100])
% try
% savefig(gcf, [file, '_Angular Rates', '.fig']);
% saveas(gcf, [file, '_Angular Rates', '.png']);
% catch Eexception
% msgbox('Figure saving not finished, wait a few seconds before closing it.', 'Warning', 'warn');
% end
figure('Name', ['Magnetic field strengths_', file], 'color','w', 'NumberTitle', 'off')
subplot(3,1,1)
title('Magnetometers', 'FontSize', 12)
hold on
plot(time,Magn_1_X_gauss_,'r-*','MarkerSize',3);
plot(time,Magn_2_X_gauss_,'g-*','MarkerSize',3);
plot(time,Magn_3_X_gauss_,'b-*','MarkerSize',3);
ylim([-1 1]*magnmax)
xlabel('t(s)')
ylabel('gauss_x')
legend('1','2','3')
grid on
set(gcf,'position',[300,100,800,600])
axis([0 60 -1 1])
subplot(3,1,2)
title('Magnetometers', 'FontSize', 12)
hold on
plot(time,Magn_1_Y_gauss_,'r-*','MarkerSize',3);
plot(time,Magn_2_Y_gauss_,'g-*','MarkerSize',3);
plot(time,Magn_3_Y_gauss_,'b-*','MarkerSize',3);
ylim([-1 1]*magnmax)
xlabel('t(s)')
ylabel('gauss_y')
legend('1','2','3')
grid on
axis([0 60 -1 1])
subplot(3,1,3)
title('Magnetometers', 'FontSize', 12)
hold on
plot(time,Magn_1_Z_gauss_,'r-*','MarkerSize',3);
plot(time,Magn_2_Z_gauss_,'g-*','MarkerSize',3);
plot(time,Magn_3_Z_gauss_,'b-*','MarkerSize',3);
ylim([-1 1]*magnmax)
xlabel('t(s)')
ylabel('gauss_z')
legend('1','2','3')
grid on
axis([0 60 -1 1])
% try
% savefig(gcf, [file, '_Magnetic field strengths', '.fig']);
% saveas(gcf,[file, '_Magnetic field strengths', '.png']);
% catch Eexception
% msgbox('Figure saving not finished, wait a few seconds before closing it.', 'Warning', 'warn');
% end
tsensor = time;
dtsensor = tsensor(2)-tsensor(1);
fsSens = 1/dtsensor; % sampling frequency
Fn=fsSens/2; %Nyquist freq
senssize = size(tsensor);
g = 9.81;
Rhup = 0.589; % radius of Huple
sensdiff = 0.02;
%Change Acc's to SI
Acc_1_X=Acc_1_X_g_*9.81;
Acc_2_X=Acc_2_X_g_*9.81;
Acc_3_X=Acc_3_X_g_*9.81;
Acc_1_Y=Acc_1_Y_g_*9.81;
Acc_2_Y=Acc_2_Y_g_*9.81;
Acc_3_Y=Acc_3_Y_g_*9.81;
Acc_1_Z=Acc_1_Z_g_*9.81;
Acc_2_Z=Acc_2_Z_g_*9.81;
Acc_3_Z=Acc_3_Z_g_*9.81;
%Change Gyroscopes to SI
Gyro_1_X=Gyro_1_X_dps_*(pi/180);
Gyro_1_Y=Gyro_1_Y_dps_*(pi/180);
Gyro_1_Z=Gyro_1_Z_dps_*(pi/180);
Gyro_2_X=Gyro_2_X_dps_*(pi/180);
Gyro_2_Y=Gyro_2_Y_dps_*(pi/180);
Gyro_2_Z=Gyro_2_Z_dps_*(pi/180);
Gyro_3_X=Gyro_3_X_dps_*(pi/180);
Gyro_3_Y=Gyro_3_Y_dps_*(pi/180);
Gyro_3_Z=Gyro_3_Z_dps_*(pi/180);
%applicate the right hand rule!
Acc_1_Y=-Acc_1_Y;
Acc_2_Y=-Acc_2_Y;
Acc_3_Y=-Acc_3_Y;
Gyro_1_Y=-Gyro_1_Y;
Gyro_2_Y=-Gyro_2_Y;
Gyro_3_Y=-Gyro_3_Y;
%gyros
gyro1=[Gyro_1_X,Gyro_1_Y,Gyro_1_Z];
gyro2=[Gyro_2_X,Gyro_2_Y,Gyro_2_Z];
gyro3=[Gyro_3_X,Gyro_3_Y,Gyro_3_Z];
%declarate Acc4
acc1=[Acc_1_X,Acc_1_Y,Acc_1_Z];
acc2=[Acc_2_X,Acc_2_Y,Acc_2_Z];
acc3=[Acc_3_X,Acc_3_Y,Acc_3_Z];
global acc4
acc4 = (acc1+acc3)/2;
global gyro4
gyro4 = (gyro1+gyro3)/2;
decim=1;
fuse=imufilter('SampleRate',Fs,'DecimationFactor',decim,'GyroscopeNoise',8.75*10^-6,'AccelerometerNoise',0.122*10^-6);
q4=fuse(acc4,gyro4);
figure('Name',['Orientation estimation',file]);
hold on;
title('Orientation estimation')
xlabel('time[s]')
ylabel('Rotation(degrees)')
time=(0:decim:size(acc4,1)-1)/Fs;
plot(time,unwrap(eulerd(q4,'ZYX','frame')))
legend('Z-axis','Y-axis','X-axis')
% %2nd solution
% fpass_gyro=0.5;%[Hz]
% fpass_acc=0.5;%[Hz]
% gyro_highpass=highpass(gyro4,fpass_gyro,fsSens);
% acc_lowpass=lowpass(acc4,fpass_acc,fsSens);
% figure('Name',['Filtered',file]);
% hold on;
% subplot(2,1,1);
% plot(time,gyro_highpass);
% title('Gyros')
% xlabel('time[s]')
% ylabel('Highpassed gyroscope')
% subplot(2,1,2);
% plot(time,acc_lowpass);
% title('Accs')
% xlabel('time[s]')
% ylabel('Lowpassed accs')
%
% velocity=cumtrapz(time,acc4);
% displacement=cumtrapz(time,velocity);
%
% figure('Name',['Integration',file]);
% hold on;
% plot(time,displacement)
% xlabel('time[s]')
% ylabel('Orientation')
%2nd solution
pv=gyro4(:,1);
qv=gyro4(:,2);
figure
plot(time,pv)
grid
[pn,tn]=resample(pv,time,100);
[qn,tn]=resample(qv,time,100);
L=numel(tn);
FTpn=fft(pn)/L
Fv=linspace(0, 1, fix(L/2+1))*Fn;
Iv=1:numel(Fv);
figure
plot(Fv,abs(FTpn(Iv))*2)
title('Fv')
grid
Wp = 0.20/Fn;
Ws=0.50/Fn;
Rp=1;
Rs=60;
[n,Wp]=ellipord(Wp,Ws,Rp,Rs);
[z,p,k]=ellip(n,Rp,Rs,Wp,'high');
[sos,g]=zp2sos(z,p,k);
figure
freqz(sos,2^16,Fs)
title('Freq')
pn_filt=filtfilt(sos,g,pn);
qn_filt=filtfilt(sos,g,qn);
figure
plot(tn, pn_filt)
hold on
plot(tn, pv)
hold off
grid
roll = cumtrapz(tn,pn_filt);
pitch = cumtrapz(tn,qn_filt);
figure
plot(tn,pn_filt)
hold on
plot(tn,roll)
legend('p','roll')
figure
plot(tn,qn_filt)
hold on
plot(tn,pitch)
legend('q','pitch')
% Complementary filter
compFilt = complementaryFilter('SampleRate', Fs,'HasMagnetometer',false);
Thanks
0 commentaires
Réponses (0)
Voir également
Catégories
En savoir plus sur Plasma Physics dans Help Center et File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!