EKF: Add derivation for for dual antenna yaw observations

EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m

@@ -415,6 +415,29 @@ fix_c_code('calcH_YAW312.c');
 clear all;
 reset(symengine);
 
+%% derive equations for fusion of dual antenna yaw measurement
+load('StatePrediction.mat');
+
+syms ant_yaw real; % yaw angle of antenna array axis wrt X body axis
+
+% define antenna vector in body frame
+ant_vec_bf = [cos(ant_yaw);sin(ant_yaw);0];
+
+% rotate into earth frame
+ant_vec_ef = Tbn * ant_vec_bf;
+
+% Calculate the yaw angle from the projection
+angMeas = atan(ant_vec_ef(2)/ant_vec_ef(1));
+
+H_YAWGPS = jacobian(angMeas,stateVector); % measurement Jacobian
+H_YAWGPS = simplify(H_YAWGPS);
+ccode(H_YAWGPS,'file','calcH_YAWGPS.c');
+fix_c_code('calcH_YAWGPS.c');
+
+% reset workspace
+clear all;
+reset(symengine);
+
 %% derive equations for fusion of declination
 load('StatePrediction.mat');
 

EKF/matlab/scripts/Inertial Nav EKF/calcH_YAWGPS.c

@@ -0,0 +1,38 @@
+t2 = sin(ant_yaw);
+t3 = cos(ant_yaw);
+t4 = q0*q3*2.0;
+t5 = q0*q0;
+t6 = q1*q1;
+t7 = q2*q2;
+t8 = q3*q3;
+t9 = q1*q2*2.0;
+t10 = t5+t6-t7-t8;
+t11 = t3*t10;
+t12 = t4+t9;
+t13 = t3*t12;
+t14 = t5-t6+t7-t8;
+t15 = t2*t14;
+t16 = t13+t15;
+t17 = t4-t9;
+t19 = t2*t17;
+t20 = t11-t19;
+t18 = 1.0/(t20*t20);
+t21 = t16*t16;
+t22 = 1.0/pow(t11-t19][2.0);
+t23 = q1*t3*2.0;
+t24 = q2*t2*2.0;
+t25 = t23+t24;
+t26 = 1.0/t20;
+t27 = q1*t2*2.0;
+t28 = t21*t22;
+t29 = t28+1.0;
+t30 = 1.0/t29;
+t31 = q0*t3*2.0;
+t32 = t31-q3*t2*2.0;
+t33 = q3*t3*2.0;
+t34 = q0*t2*2.0;
+t35 = t33+t34;
+A0[0][0] = (t35/(t11-t2*(t4-q1*q2*2.0))-t16*t18*t32)/(t18*t21+1.0);
+A0[0][1] = -t30*(t26*(t27-q2*t3*2.0)+t16*t22*t25);
+A0[0][2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0));
+A0[0][3] = t30*(t26*t32+t16*t22*t35);