EKF derivations: Correct error in direct yaw and declination angle fusion

The atan function is now being used correctly instead of the tan function. This fixes problems with large heading errors or declination values.
The simple heading fusion has been decoupled from the magnetic field measurements. This enables external yaw measurements to be used in the future.

matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt

@@ -9,62 +9,56 @@ declinationAngle is the estimated declination as that location
 This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements 
 (e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors
 can cause the magnetic declination of the earth field estimates to slowly rotate.
+
+Divide by zero protection and protection against a badly conditioned covariance matrix must be included.
 */
 
 // Calculate intermediate variable
-float t2 = 1.0f/magN;
-float t4 = magE*t2;
-float t3 = tanf(t4);
-float t5 = t3*t3;
-float t6 = t5+1.0f;
-float t25 = t2*t6;
-float t7 = 1.0f/(magN*magN);
-float t26 = magE*t6*t7;
-float t8 = P[17][17]*t25;
-float t15 = P[16][17]*t26;
-float t9 = t8-t15;
-float t10 = t25*t9;
-float t11 = P[17][16]*t25;
-float t16 = P[16][16]*t26;
-float t12 = t11-t16;
-float t17 = t26*t12;
-float t13 = R_DECL+t10-t17;
-float t14 = 1.0f/t13;
-float t18 = magE;
-float t19 = magN;
-float t21 = 1.0f/t19;
-float t22 = t18*t21;
-float t20 = tanf(t22);
-float t23 = t20*t20;
-float t24 = t23+1.0f;
+float t2 = magE*magE;
+float t3 = magN*magN;
+float t4 = t2+t3;
+float t5 = 1.0f/t4;
+float t22 = magE*t5;
+float t23 = magN*t5;
+float t6 = P[16][16]*t22;
+float t13 = P[17][16]*t23;
+float t7 = t6-t13;
+float t8 = t22*t7;
+float t9 = P[16][17]*t22;
+float t14 = P[17][17]*t23;
+float t10 = t9-t14;
+float t15 = t23*t10;
+float t11 = R_DECL+t8-t15; // innovation variance
+float t12 = 1.0f/t11;
 
 // Calculate the observation Jacobian
 // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
-H_MAG[16] = -t18*1.0f/(t19*t19)*t24;
-H_MAG[17] = t21*t24;
+H_DECL[16] = -magE*t5;
+H_DECL[17] = magN*t5;
 
 // Calculate the Kalman gains
-Kfusion[0] = t14*(P[0][17]*t25-P[0][16]*t26);
-Kfusion[1] = t14*(P[1][17]*t25-P[1][16]*t26);
-Kfusion[2] = t14*(P[2][17]*t25-P[2][16]*t26);
-Kfusion[3] = t14*(P[3][17]*t25-P[3][16]*t26);
-Kfusion[4] = t14*(P[4][17]*t25-P[4][16]*t26);
-Kfusion[5] = t14*(P[5][17]*t25-P[5][16]*t26);
-Kfusion[6] = t14*(P[6][17]*t25-P[6][16]*t26);
-Kfusion[7] = t14*(P[7][17]*t25-P[7][16]*t26);
-Kfusion[8] = t14*(P[8][17]*t25-P[8][16]*t26);
-Kfusion[9] = t14*(P[9][17]*t25-P[9][16]*t26);
-Kfusion[10] = t14*(P[10][17]*t25-P[10][16]*t26);
-Kfusion[11] = t14*(P[11][17]*t25-P[11][16]*t26);
-Kfusion[12] = t14*(P[12][17]*t25-P[12][16]*t26);
-Kfusion[13] = t14*(P[13][17]*t25-P[13][16]*t26);
-Kfusion[14] = t14*(P[14][17]*t25-P[14][16]*t26);
-Kfusion[15] = t14*(P[15][17]*t25-P[15][16]*t26);
-Kfusion[16] = -t14*(t16-P[16][17]*t25);
-Kfusion[17] = t14*(t8-P[17][16]*t26);
-Kfusion[18] = t14*(P[18][17]*t25-P[18][16]*t26);
-Kfusion[19] = t14*(P[19][17]*t25-P[19][16]*t26);
-Kfusion[20] = t14*(P[20][17]*t25-P[20][16]*t26);
-Kfusion[21] = t14*(P[21][17]*t25-P[21][16]*t26);
-Kfusion[22] = t14*(P[22][17]*t25-P[22][16]*t26);
-Kfusion[23] = t14*(P[23][17]*t25-P[23][16]*t26);
+Kfusion[0] = -t12*(P[0][16]*t22-P[0][17]*t23);
+Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23);
+Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23);
+Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23);
+Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23);
+Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23);
+Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23);
+Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23);
+Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23);
+Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23);
+Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23);
+Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23);
+Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23);
+Kfusion[13] = -t12*(P[13][16]*t22-P[13][17]*t23);
+Kfusion[14] = -t12*(P[14][16]*t22-P[14][17]*t23);
+Kfusion[15] = -t12*(P[15][16]*t22-P[15][17]*t23);
+Kfusion[16] = -t12*(t6-P[16][17]*t23);
+Kfusion[17] = t12*(t14-P[17][16]*t22);
+Kfusion[18] = -t12*(P[18][16]*t22-P[18][17]*t23);
+Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23);
+Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23);
+Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23);
+Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23);
+Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23);
+

matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt

@@ -5,6 +5,8 @@ innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationA
 
 magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
 declinationAngle is the estimated declination as that location
+
+Protection against /0 and covariance matrix errors will need to be added.
 */
 
 // intermediate variables
@@ -12,36 +14,19 @@ float t2 = q0*q0;
 float t3 = q1*q1;
 float t4 = q2*q2;
 float t5 = q3*q3;
-float t6 = q0*q2*2.0f;
-float t7 = q1*q3*2.0f;
-float t8 = t6+t7;
-float t9 = q0*q3*2.0f;
-float t13 = q1*q2*2.0f;
-float t10 = t9-t13;
-float t11 = t2+t3-t4-t5;
-float t12 = magX*t11;
-float t14 = magZ*t8;
-float t19 = magY*t10;
-float t15 = t12+t14-t19;
-float t16 = t2-t3+t4-t5;
-float t17 = q0*q1*2.0f;
-float t24 = q2*q3*2.0f;
-float t18 = t17-t24;
-float t20 = 1.0f/t15;
-float t21 = magY*t16;
-float t22 = t9+t13;
-float t23 = magX*t22;
-float t28 = magZ*t18;
-float t25 = t21+t23-t28;
-float t29 = t20*t25;
-float t26 = tan(t29);
-float t27 = 1.0f/(t15*t15);
-float t30 = t26*t26;
-float t31 = t30+1.0f;
+float t6 = t2+t3-t4-t5;
+float t7 = q0*q3*2.0f;
+float t8 = q1*q2*2.0f;
+float t9 = t7+t8;
+float t10 = 1.0f/(t6*t6);
+float t11 = t9*t9;
+float t12 = t10*t11;
+float t13 = t12+1.0f;
+float t14 = 1.0f/t13;
+float t15 = 1.0f/t6;
 
 // Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
 // Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
 float H_MAG[3];
-H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10));
-H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11));
-H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11));
+H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
+H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));

matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m

@@ -68,7 +68,7 @@ syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
 syms ptd real % location of terrain in D axis
 syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame
 syms decl real; % earth magnetic field declination from true north
-syms R_MAGS real; % variance for magnetic deviation measurement
+syms R_YAW real; % variance for magnetic deviation measurement
 syms R_DECL real; % variance of supplied declination
 syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y  body axes
 syms rho real % air density (kg/m^3)
@@ -269,19 +269,22 @@ ccode(H_LOS,'file','H_LOS.txt');
 ccode(K_LOSX,'file','K_LOSX.txt');
 ccode(K_LOSY,'file','K_LOSY.txt');
 
-%% derive equations for fusion of magnetic heading measurement
+%% derive equations for fusion of direct yaw measurement
 
-% rotate magnetic field into earth axes
-magMeasNED = Tbn*[magX;magY;magZ];
-% the predicted measurement is the angle wrt magnetic north of the horizontal
-% component of the measured field
-angMeas = tan(magMeasNED(2)/magMeasNED(1)) - decl;
-H_MAGS = jacobian(angMeas,errRotVec); % measurement Jacobian
+% rotate X body axis into earth axes
+yawVec = Tbn*[1;0;0];
+% Calculate the yaw angle of the projection
+angMeas = atan(yawVec(2)/yawVec(1));
+H_YAW = jacobian(angMeas,stateVector); % measurement Jacobian
 %H_MAGS = H_MAGS(1:3);
-H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
-H_MAGS = simplify(H_MAGS);
+H_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+H_YAW = simplify(H_YAW);
 %[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
-ccode(H_MAGS,'file','calcH_MAGS.c');
+ccode(H_YAW,'file','calcH_YAW.c');
+% Calculate Kalman gain vector
+K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW);
+%K_MAGS = simplify(K_MAGS);
+ccode(K_YAW,'file','calcK_YAW.c');
 
 %% derive equations for fusion of synthetic deviation measurement
 % used to keep correct heading when operating without absolute position or
@@ -290,7 +293,7 @@ ccode(H_MAGS,'file','calcH_MAGS.c');
 magMeasNED = [magN;magE;magD];
 % the predicted measurement is the angle wrt magnetic north of the horizontal
 % component of the measured field
-angMeas = tan(magMeasNED(2)/magMeasNED(1));
+angMeas = atan(magMeasNED(2)/magMeasNED(1));
 H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
 H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 H_MAGD = simplify(H_MAGD);