Skip to main content
. 2017 May 26;17(6):1223. doi: 10.3390/s17061223
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