EKF: Update magnetometer fusion derivations

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

@@ -1,32 +1,57 @@
 /* 
-Autocode for fusion of a yaw error measurement where the innovation is given by: 
+Observation jacobian for fusion of the horizontal components of magnetometer measurements.
 
-innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
+innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained
+by rotating the measured magnetometer readings from body into earth axes.
 
-magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
-declinationAngle is the estimated declination as that location
+This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use
 
-Protection against /0 and covariance matrix errors will need to be added.
+Divide by zero protection hs been added
 */
 
-// intermediate variables
+// calculate intermediate variables for observation jacobian
 float t2 = q0*q0;
 float t3 = q1*q1;
 float t4 = q2*q2;
 float t5 = q3*q3;
-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;
+float t6 = q0*q3*2.0f;
+float t8 = t2-t3+t4-t5;
+float t9 = q0*q1*2.0f;
+float t10 = q2*q3*2.0f;
+float t11 = t9-t10;
+float t14 = q1*q2*2.0f;
+float t21 = magY*t8;
+float t22 = t6+t14;
+float t23 = magX*t22;
+float t24 = magZ*t11;
+float t7 = t21+t23-t24;
+float t12 = t2+t3-t4-t5;
+float t13 = magX*t12;
+float t15 = q0*q2*2.0f;
+float t16 = q1*q3*2.0f;
+float t17 = t15+t16;
+float t18 = magZ*t17;
+float t19 = t6-t14;
+float t25 = magY*t19;
+float t20 = t13+t18-t25;
+if (fabsf(t20) < 1e-6f) {
+    return;
+}
+float t26 = 1.0f/(t20*t20);
+float t27 = t7*t7;
+float t28 = t26*t27;
+float t29 = t28+1.0;
+if (fabsf(t29) < 1e-12f) {
+    return;
+}
+float t30 = 1.0f/t29;
+if (fabsf(t20) < 1e-12f) {
+    return;
+}
+float t31 = 1.0f/t20;
 
-// 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_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));
+// calculate observation jacobian
+float H_DECL[3] = {};
+H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
+H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
+H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));

matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt

@@ -0,0 +1,27 @@
+/* 
+Autocode for fusion of an Euler yaw measurement.
+
+Protection against /0 and covariance matrix errors will need to be added.
+*/
+
+// intermediate variables
+float t2 = q0*q0;
+float t3 = q1*q1;
+float t4 = q2*q2;
+float t5 = q3*q3;
+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_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));