EKF: Add missing Kalman gain to Declination fusion autocode

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

@@ -1,7 +1,7 @@
 /* 
 Autocode for fusion of a magnetic declination estimate where the innovation is given by 
 
-innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
+innovation = atanf(magMeasEarthFrameEast/magMeasEarthFrameNorth) - declinationAngle;
 
 magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to earth frame.
 declinationAngle is the estimated declination as that location
@@ -11,5 +11,60 @@ This fusion method is used to constrain the rotation of the earth field vector w
 can cause the magnetic declination of the earth field estimates to slowly rotate.
 */
 
-// 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 t7 = 1.0f/(magN*magN);
float t8 = P[17][17]*t2*t6;
float t15 = P[16][17]*magE*t6*t7;
float t9 = t8-t15;
float t10 = t2*t6*t9;
float t11 = P[17][16]*t2*t6;
float t16 = P[16][16]*magE*t6*t7;
float t12 = t11-t16;
float t17 = magE*t6*t7*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;

// 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
float H_MAG[24];
H_MAG[16] = -t18*1.0f/(t19*t19)*t24;
H_MAG[17] = t21*t24;
+// 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;
+
+// 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;
+
+// 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);