Orientation estimation using accelerometer and gyroscope

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);

So it is my code. I uploaded the raw data too. You can download it here. https://ufile.io/sua7vu0j

Thanks