Algorithm Complete Tri-Axis Magnetometer Calibration with a Gyro Auxiliary |
function [K,b,Mint]=MagCal_YD(MGdata,n,dA,dt,ht) |
% K: total error coefficient matrix |
% b: total bias error vector |
% Mint: tri-axis magnetometer interpolation outputs |
% MGdata: j x l x k matrix of tri-axis gyro and magnetometer outputs |
% j is the j-th sample point |
% l=1~6, represent the x-, y-, z-axis gyro outputs with unit of degree/s, and x-, y-, z-axis magnetometer outputs, respectively. |
% k=1~6, represent the sensor direction of z-axis upward, z-axis downward, y-axis upward, y-axis downward, x-axis upward, x-axis downward, respectively. |
% n: number of rotations, n>=1. |
% dA: angular interval with unit of degree. 360/dA must be an integer. |
% dt: sampling period, with unit of s. |
% ht: the geomagnetic field component perpendicular to the table. If ht is unknow, set ht=0. |
%% step 1: Magnetometer Interpolation Output Calculation |
AR=360*n; |
ni= AR/dA; |
Mint=zeros(AR/dA,3,6); |
num=length(MGdata(:,1,1)); |
angle=zeros(3*num,6); |
for k=1:6 |
i=1;j=1;l=1; |
while abs(angle(j,k))<AR |
angle(j+1,k)=angle(j,k)+MGdata(j,4-floor(0.5*k+0.5),k)*dt; |
j=j+1; |
end |
if angle(j,k)>0 |
angle(1:j,k)=angle(1:j,k); |
else |
angle(1:j,k)=-angle(1:j,k); |
end |
Mint(1,:,k)=MGdata(1,4:6,k); |
while i<(AR/dA) |
if angle(l,k)<(i)*dA && angle(l+1,k)>=(i)*dA |
Mint(i+1,:,k)=(i*dA-angle(l,k))/(angle(l+1,k)-angle(l,k))*MGdata(l+1,4:6,k)+(angle(l+1,k)-i*dA)/(angle(l+1,k)-angle(l,k))*MGdata(l,4:6,k); |
i=i+1; |
else |
l=l+1; |
end |
end |
end |
%% step 2: Error Coefficient Calculation |
if ht==0 |
ht=1*(sum(Mint(:,3,1))-sum(Mint(:,3,2))+sum(Mint(:,2,3))-sum(Mint(:,2,4))+sum(Mint(:,1,5))-sum(Mint(:,1,6)))/(ni*6); |
end |
bx=sum(sum(Mint(:,1,:)))/(ni*6); |
by=sum(sum(Mint(:,2,:)))/(ni*6); |
b=sum(sum(Mint(:,3,:)))/(ni*6); |
b=[bx;by;bz]; |
K=zeros(3,3); |
K(:,3)=(sum(Mint(:,1:3,1))-sum(Mint(:,1:3,2)))'/(ni*2*ht); |
K(:,2)=(sum(Mint(:,1:3,3))-sum(Mint(:,1:3,4)))'/(ni*2*ht); |
K(:,1)=(sum(Mint(:,1:3,5))-sum(Mint(:,1:3,6)))'/(ni*2*ht); |
end |