Derivations: Fix bug in derivation of direct heading fusion

derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m

@@ -69,7 +69,7 @@
 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_DECL real; % variance of supplied declination
+syms R_DECL R_YAW real; % variance of supplied declination or yaw angle
 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)
 syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
@@ -275,7 +275,7 @@
 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;
+angMeas = atan(magMeasNED(2)/magMeasNED(1)) - decl;
 H_MAGS = jacobian(angMeas,stateVector); % measurement Jacobian
 %H_MAGS = H_MAGS(1:3);
 H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
@@ -287,19 +287,36 @@
 %K_MAGS = simplify(K_MAGS);
 ccode(K_MAGS,'file','calcK_MAGS.c');
 
+%% derive equations for fusion of direct yaw measurement
+
+% 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_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_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
 % velocity measurements - eg when using optical flow
 % rotate magnetic field into earth axes
 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);
 %[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
-%ccode(H_MAGD,'file','calcH_MAGD.c');
+ccode(H_MAGD,'file','calcH_MAGD.c');
 % Calculate Kalman gain vector
 K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
 ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');

derivations/RotationVectorAttitudeParameterisation/calcH_MAGD.c

@@ -0,0 +1,7 @@
+float t2 = magE*magE;
+float t3 = magN*magN;
+float t4 = t2+t3;
+float t5 = 1.0;/t4;
+
+H_DECL[16] = -magE*t5;
+H_DECL[17] = magN*t5;

derivations/RotationVectorAttitudeParameterisation/calcH_YAW.c

@@ -0,0 +1,16 @@
+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;
+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));

derivations/RotationVectorAttitudeParameterisation/calcK_YAW.c

@@ -0,0 +1,62 @@
+real t2 = q0*q0;
+real t3 = q1*q1;
+real t4 = q2*q2;
+real t5 = q3*q3;
+real t6 = t2+t3-t4-t5;
+real t7 = q0*q3*2.0;
+real t8 = q1*q2*2.0;
+real t9 = t7+t8;
+real t10 = 1.0/(t6*t6);
+real t11 = t9*t9;
+real t12 = t10*t11;
+real t13 = t12+1.0;
+real t14 = 1.0/t13;
+real t15 = 1.0/t6;
+real t16 = q0*q1*2.0;
+real t29 = q2*q3*2.0;
+real t17 = t16-t29;
+real t18 = t15*t17;
+real t19 = q0*q2*2.0;
+real t20 = q1*q3*2.0;
+real t21 = t19+t20;
+real t22 = t9*t10*t21;
+real t23 = t18+t22;
+real t24 = t2-t3+t4-t5;
+real t25 = t15*t24;
+real t26 = t7-t8;
+real t27 = t9*t10*t26;
+real t28 = t25+t27;
+real t30 = P[1][1]*t14*t23;
+real t31 = P[1][2]*t14*t23;
+real t32 = P[2][2]*t14*t28;
+real t33 = t31+t32;
+real t34 = t14*t28*t33;
+real t35 = P[2][1]*t14*t28;
+real t36 = t30+t35;
+real t37 = t14*t23*t36;
+real t38 = R_YAW+t34+t37;
+real t39 = 1.0/t38;
+Kfusion[0] = t39*(P[0][1]*t14*t23+P[0][2]*t14*t28);
+Kfusion[1] = t39*(t30+P[1][2]*t14*t28);
+Kfusion[2] = t39*(t32+P[2][1]*t14*t23);
+Kfusion[3] = t39*(P[3][1]*t14*t23+P[3][2]*t14*t28);
+Kfusion[4] = t39*(P[4][1]*t14*t23+P[4][2]*t14*t28);
+Kfusion[5] = t39*(P[5][1]*t14*t23+P[5][2]*t14*t28);
+Kfusion[6] = t39*(P[6][1]*t14*t23+P[6][2]*t14*t28);
+Kfusion[7] = t39*(P[7][1]*t14*t23+P[7][2]*t14*t28);
+Kfusion[8] = t39*(P[8][1]*t14*t23+P[8][2]*t14*t28);
+Kfusion[9] = t39*(P[9][1]*t14*t23+P[9][2]*t14*t28);
+Kfusion[10] = t39*(P[10][1]*t14*t23+P[10][2]*t14*t28);
+Kfusion[11] = t39*(P[11][1]*t14*t23+P[11][2]*t14*t28);
+Kfusion[12] = t39*(P[12][1]*t14*t23+P[12][2]*t14*t28);
+Kfusion[13] = t39*(P[13][1]*t14*t23+P[13][2]*t14*t28);
+Kfusion[14] = t39*(P[14][1]*t14*t23+P[14][2]*t14*t28);
+Kfusion[15] = t39*(P[15][1]*t14*t23+P[15][2]*t14*t28);
+Kfusion[16] = t39*(P[16][1]*t14*t23+P[16][2]*t14*t28);
+Kfusion[17] = t39*(P[17][1]*t14*t23+P[17][2]*t14*t28);
+Kfusion[18] = t39*(P[18][1]*t14*t23+P[18][2]*t14*t28);
+Kfusion[19] = t39*(P[19][1]*t14*t23+P[19][2]*t14*t28);
+Kfusion[20] = t39*(P[20][1]*t14*t23+P[20][2]*t14*t28);
+Kfusion[21] = t39*(P[21][1]*t14*t23+P[21][2]*t14*t28);
+Kfusion[22] = t39*(P[22][1]*t14*t23+P[22][2]*t14*t28);
+Kfusion[23] = t39*(P[23][1]*t14*t23+P[23][2]*t14*t28);

derivations/RotationVectorAttitudeParameterisation/calcMAGD.c

@@ -1,49 +1,44 @@
-  t2 = 1.0/magN;
-  t4 = magE*t2;
-  t3 = tan(t4);
-  t5 = t3*t3;
-  t6 = t5+1.0;
-  t7 = 1.0/(magN*magN);
-  t8 = OP_l_18_c_18_r_*t2*t6;
-  t15 = OP_l_17_c_18_r_*magE*t6*t7;
-  t9 = t8-t15;
-  t10 = t2*t6*t9;
-  t11 = OP_l_18_c_17_r_*t2*t6;
-  t16 = OP_l_17_c_17_r_*magE*t6*t7;
-  t12 = t11-t16;
-  t17 = magE*t6*t7*t12;
-  t13 = R_DECL+t10-t17;
-  t14 = 1.0/t13;
-  t18 = conjugate(magE);
-  t19 = conjugate(magN);
-  t21 = 1.0/t19;
-  t22 = t18*t21;
-  t20 = tan(t22);
-  t23 = t20*t20;
-  t24 = t23+1.0;
-  Kfusion[0] = t14*(OP_l_1_c_18_r_*t2*t6-OP_l_1_c_17_r_*magE*t6*t7);
-  Kfusion[1] = t14*(OP_l_2_c_18_r_*t2*t6-OP_l_2_c_17_r_*magE*t6*t7);
-  Kfusion[2] = t14*(OP_l_3_c_18_r_*t2*t6-OP_l_3_c_17_r_*magE*t6*t7);
-  Kfusion[3] = t14*(OP_l_4_c_18_r_*t2*t6-OP_l_4_c_17_r_*magE*t6*t7);
-  Kfusion[4] = t14*(OP_l_5_c_18_r_*t2*t6-OP_l_5_c_17_r_*magE*t6*t7);
-  Kfusion[5] = t14*(OP_l_6_c_18_r_*t2*t6-OP_l_6_c_17_r_*magE*t6*t7);
-  Kfusion[6] = t14*(OP_l_7_c_18_r_*t2*t6-OP_l_7_c_17_r_*magE*t6*t7);
-  Kfusion[7] = t14*(OP_l_8_c_18_r_*t2*t6-OP_l_8_c_17_r_*magE*t6*t7);
-  Kfusion[8] = t14*(OP_l_9_c_18_r_*t2*t6-OP_l_9_c_17_r_*magE*t6*t7);
-  Kfusion[9] = t14*(OP_l_10_c_18_r_*t2*t6-OP_l_10_c_17_r_*magE*t6*t7);
-  Kfusion[10] = t14*(OP_l_11_c_18_r_*t2*t6-OP_l_11_c_17_r_*magE*t6*t7);
-  Kfusion[11] = t14*(OP_l_12_c_18_r_*t2*t6-OP_l_12_c_17_r_*magE*t6*t7);
-  Kfusion[12] = t14*(OP_l_13_c_18_r_*t2*t6-OP_l_13_c_17_r_*magE*t6*t7);
-  Kfusion[13] = t14*(OP_l_14_c_18_r_*t2*t6-OP_l_14_c_17_r_*magE*t6*t7);
-  Kfusion[14] = t14*(OP_l_15_c_18_r_*t2*t6-OP_l_15_c_17_r_*magE*t6*t7);
-  Kfusion[15] = t14*(OP_l_16_c_18_r_*t2*t6-OP_l_16_c_17_r_*magE*t6*t7);
-  Kfusion[16][0] = -t18*1.0/(t19*t19)*t24;
-  Kfusion[16] = -t14*(t16-OP_l_17_c_18_r_*t2*t6);
-  Kfusion[17][0] = t21*t24;
-  Kfusion[17] = t14*(t8-OP_l_18_c_17_r_*magE*t6*t7);
-  Kfusion[18] = t14*(OP_l_19_c_18_r_*t2*t6-OP_l_19_c_17_r_*magE*t6*t7);
-  Kfusion[19] = t14*(OP_l_20_c_18_r_*t2*t6-OP_l_20_c_17_r_*magE*t6*t7);
-  Kfusion[20] = t14*(OP_l_21_c_18_r_*t2*t6-OP_l_21_c_17_r_*magE*t6*t7);
-  Kfusion[21] = t14*(OP_l_22_c_18_r_*t2*t6-OP_l_22_c_17_r_*magE*t6*t7);
-  Kfusion[22] = t14*(OP_l_23_c_18_r_*t2*t6-OP_l_23_c_17_r_*magE*t6*t7);
-  Kfusion[23] = t14*(OP_l_24_c_18_r_*t2*t6-OP_l_24_c_17_r_*magE*t6*t7);
+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;
+
+H_DECL[16] = -magE*t5;
+H_DECL[17] = magN*t5;
+
+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);