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

/* 
Autocode for fusion of a magnetic declination estimate where the innovation is given by 

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

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 = 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_DECL[16] = -magE*t5;
H_DECL[17] = magN*t5;

// Calculate the Kalman gains
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);