Derivations

Add method to maintain declination angle of earth field when not fusing
earth relative observations (e.g. doing optical flow only nav)

This method uses the supplied declination at that location as an
observation.

derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m

@@ -69,6 +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
 
 %% define the process equations
 
@@ -165,7 +166,7 @@
 
 % derive the control(disturbance) influence matrix
 G = jacobian(newStateVector, distVector);
-G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0},0);
+G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 [G,SG]=OptimiseAlgebra(G,'SG');
 
 % derive the state error matrix
@@ -181,7 +182,7 @@
 % derive the state transition matrix
 F = jacobian(newStateVector, stateVector);
 % set the rotation error states to zero
-F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0},0);
+F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 [F,SF]=OptimiseAlgebra(F,'SF');
 
 % define a symbolic covariance matrix using strings to represent 
@@ -205,7 +206,7 @@
 %% derive equations for fusion of true airspeed measurements
 VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
 H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
-H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0},0);
+H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 [H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing
 K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
 
@@ -215,14 +216,14 @@
 % calculate predicted angle of sideslip using small angle assumption
 BetaPred = Vbw(2)/Vbw(1);
 H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
-H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0},0);
+H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 [H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
 K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
 
 %% derive equations for fusion of magnetic field measurement
 magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
 H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
-H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0},0);
+H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 [H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG');
 
 K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector
@@ -264,7 +265,7 @@
 ccode(K_LOSX,'file','K_LOSX.txt');
 ccode(K_LOSY,'file','K_LOSY.txt');
 
-%% derive equations for fusion of magnetic deviation measurement
+%% derive equations for fusion of magnetic heading measurement
 
 % rotate magnetic field into earth axes
 magMeasNED = Tbn*[magX;magY;magZ];
@@ -278,10 +279,26 @@
 %[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
 ccode(H_MAGS,'file','calcH_MAGS.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));
+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');
+% 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');
+
 %% Save output and convert to m and c code fragments
 fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
 save(fileName);
 SaveScriptCode(nStates);
 ConvertToM(nStates);
-ConvertToC(nStates);
-fuseCompass
+ConvertToC(nStates);

derivations/RotationVectorAttitudeParameterisation/calcMAGD.c

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