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（

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????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