九轴传感器姿态解算方法互补滤波和梯度下降法MATLAB

九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）

function [Tm2aBmVmmag_strength]=Cal_mag4acc_frame（rotationfix_pointTgKga0）
% author  Zhang Xin

if nargin==4
   a0=[111111110000];
end
n=size（rotation1）;
rotation{n+1}=Tg;
rotation{n+2}=Kg;
 options=optimset（‘TolX‘1e-6‘Algorithm‘‘Levenberg-Marquardt‘...
 ‘Display‘‘iter‘）;
[a]=lsqnonlin（@mag_in_diff_gesturea0[][]optionsrotation）;

Tm2a=[a（1）    a（2）  a（3）;...
    a（4）   a（5）    a（6）;...
    a（7）   a（8）   -1];

Bm=[a（9）;a（10）;a（11）];

mag_strength=a（12）;

Ba=rotation{end-3};
Ka=rotation{end-4};
Ta=rotation{end-5};
for i=1:size（fix_point1）
    
    Acc（:i）=Ta*Ka*（fix_point（i1:3）‘+Ba）;
    Mag（:i）=Tm2a*（fix_point（i7:9）‘+Bm）;     
    SS（i）=Acc（:i）‘*Mag（:i）/（norm（Acc（:i））*norm（Mag（:i）））;
end

Mag_z=mean（SS）;
Mag_y=sqrt（1-Mag_z^2）;
Vm=[0Mag_yMag_z];

end


function E=mag_in_diff_gesture（arotation）

Tm2a=[a（1）    a（2）  a（3）;...
    a（4）   a（5）    a（6）;...
    a（7）   a（8）   -1];

Bm=[a（9）;a（10）;a（11）];

mag_strength=a（12）;

Kg=rotation{end};
Tg=rotation{end-1};
Bg=rotation{end-2};
% Ba=rotation{end-3};
% Ka=rotation{end-4};
% Ta=rotation{end-5};

for i=1:size（rotation1）-6
    data=rotation{i};
    mag_start=Tm2a*（data（18:10）‘+Bm）;
    mag_end=Tm2a*（data（end8:10）‘+Bm）;
    Q（:1）=[1;0;0;0]; 
    for j=2:size（data1）
        gyro0=Tg*Kg*（data（j-15:7）‘+Bg）;
        gyro1=Tg*Kg*（data（j5:7）‘+Bg）;
        dt=（data（j1）-data（j-11））;
        Q（:j）=attitude_update_RK4（Q（:j-1）dtgyro0gyro1）; 
    end
    R = quatern2rotMat（Q（:j）‘）;
    mag_end_compute=R*mag_start;
    E（（i-1）*4+11）=mag_end（1）-mag_end_compute（1）;
    E（（i-1）*4+21）=mag_end（2）-mag_end_compute（2）;
    E（（i-1）*4+31）=mag_end（3）-mag_end_compute（3）;
    E（（i-1）*4+41）=mag_strength-norm（mag_start）;

end

end


function [Qk_plus1]=attitude_update_RK4（Qkdtgyro0gyro1）
% RK4
% conference: A Robust and Easy to implement method for imu
% calibration without External Equipments

q_1=Qk;
k1=（1/2）*omegaMatrix（gyro0）*q_1;
q_2=Qk+dt*（1/2）*k1;
k2=（1/2）*omegaMatrix（（1/2）*（gyro0+gyro1））*q_2;
q_3=Qk+dt*（1/2）*k2;
k3=（1/2）*omegaMatrix（（1/2）*（gyro0+gyro1））*q_3;
q_4=Qk+dt*k3;
k4=（1/2）*omegaMatrix（gyro1）*q_4;
Qk_plus1=Qk+dt*（k1/6+k2/3+k3/3+k4/6）;
Qk_plus1=Qk_plus1/norm（Qk_plus1）;

end

function [omega]=omegaMatrix（data）

% wx=data（1）*pi/180;
% wy=data（2）*pi/180;
% wz=data（3）*pi/180;
wx=data（1）;
wy=data（2）;
wz=data（3）;

omega=[0   -wx  -wy  -wz ;...
       wx   0    wz  -wy ;...
       wy  -wz   0    wx ;...
       wz   wy  -wx   0   ];

end

function R = quatern2rotMat（q）
    [rows cols] = size（q）;
    R = zeros（33 rows）;
    R（11:） = 2.*q（:1）.^2-1+2.*q（:2）.^2;
    R（12:） = 2.*（q（:2）.*q（:3）+q（:1）.*q（:4））;
    R（13:） = 2.*（q（:2）.*q（:4）-q（:1）.*q（:3））;
    R（21:） = 2.*（q（:2）.*q（:3）-q（:1）.*q（:4））;
    R（22:） = 2.*q（:1）.^2-1+2.*q（:3）.^2;
    R（23:） = 2.*（q（:3）.*q（:4）+q（:1）.*q（:2））;
    R（31:） = 2.*（q（:2）.*q（:4）+q（:1）.*q（:3））;
    R（32:） = 2.*（q（:3）.*q（:4）-q（:1）.*q（:2））;
    R（33:） = 2.*q（:1）.^2-1+2.*q（

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-06-06 04:11  IMUCalibration-Gesture-master\
     文件        3012  2018-06-06 04:11  IMUCalibration-Gesture-master\Cal_mag4acc_frame.m
     文件        2386  2018-06-06 04:11  IMUCalibration-Gesture-master\EkfFilter.asv
     文件        2381  2018-06-06 04:11  IMUCalibration-Gesture-master\EkfFilter.m
     文件         967  2018-06-06 04:11  IMUCalibration-Gesture-master\FindFixData.m
     文件        2593  2018-06-06 04:11  IMUCalibration-Gesture-master\HighLowPassFilter.m
     文件        1287  2018-06-06 04:11  IMUCalibration-Gesture-master\ICRA2014_acc.m
     文件        2958  2018-06-06 04:11  IMUCalibration-Gesture-master\ICRA_2014_gyro.m
     文件         742  2018-06-06 04:11  IMUCalibration-Gesture-master\ImuCalibration_Gesture.m
     文件        1767  2018-06-06 04:11  IMUCalibration-Gesture-master\MahonyFilter.m
     文件        1582  2018-06-06 04:11  IMUCalibration-Gesture-master\README.md
     文件        9026  2018-06-06 04:11  IMUCalibration-Gesture-master\See_Gesture.m
     文件      595455  2018-06-06 04:11  IMUCalibration-Gesture-master\caldata.mat
     文件        1439  2018-06-06 04:11  IMUCalibration-Gesture-master\mag2acc_matrix.m