From ea29e39e5bf5862627b2d146763e21bb1a50f98e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 1 Mar 2016 15:21:36 +1100
Subject: [PATCH] EKF: Updated derivation of Jacobians for direct heading
 measurement

Adds an alternative using a 312 Euler sequence.
---
 .../Inertial Nav EKF/Yaw Angle Fusion.txt     | 34 ++++++++++++++-----
 .../GenerateNavFilterEquations.m              | 27 +++++++++------
 2 files changed, 42 insertions(+), 19 deletions(-)

diff --git a/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt b/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt
index 010ede7cd5..0c856be00f 100644
--- a/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt	
+++ b/matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt	
@@ -1,10 +1,8 @@
 /* 
-Autocode for fusion of an Euler yaw measurement.
-
-Protection against /0 and covariance matrix errors will need to be added.
+Code fragments for fusion of an Euler yaw measurement from a 321 sequence.
 */
 
-// intermediate variables
+// calculate observation jacobians
 float t2 = q0*q0;
 float t3 = q1*q1;
 float t4 = q2*q2;
@@ -19,9 +17,29 @@ 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[0] = 0.0f;
 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));
+
+/*
+Code fragments for fusion of an Euler yaw measurement from a 312 sequence.
+*/
+
+// calculate 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 t10 = q1*q2*2.0f;
+float t8 = t7-t10;
+float t9 = 1.0f/(t6*t6);
+float t11 = t8*t8;
+float t12 = t9*t11;
+float t13 = t12+1.0f;
+float t14 = 1.0f/t13;
+float t15 = 1.0f/t6;
+H_YAW[0] = -t14*(t15*(q0*q2*2.0f+q1*q3*2.0f)-t8*t9*(q0*q1*2.0f-q2*q3*2.0f));
+H_YAW[1] = 0.0f;
+H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
index 89715472bd..44d0eeeb40 100644
--- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m	
+++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m	
@@ -289,22 +289,27 @@ K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL)
 %[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS');
 ccode(K_MAGS,'file','calcK_MAGS.c');
 
-%% derive equations for fusion of yaw measurements
+%% derive equations for fusion of 321 sequence 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));
+% Calculate the yaw (first rotation) angle from the 321 rotation sequence
+angMeas = atan(Tbn(2,1)/Tbn(1,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');
+ccode(H_YAW,'file','calcH_YAW321.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');
+ccode([K_YAW;H_YAW'],'file','calcYAW321.c');
+
+%% derive equations for fusion of 312 sequence yaw measurement
+
+% Calculate the yaw (first rotation) angle from an Euler 312 sequence
+angMeas = atan(-Tbn(1,2)/Tbn(2,2));
+H_YAW2 = jacobian(angMeas,stateVector); % measurement Jacobianclea
+H_YAW2 = subs(H_YAW2, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+ccode(H_YAW2,'file','calcH_YAW312.c');
+% Calculate Kalman gain vector
+K_YAW2 = (P*transpose(H_YAW2))/(H_YAW2*P*transpose(H_YAW2) + R_YAW);
+ccode([K_YAW2;H_YAW2'],'file','calcYAW312.c');
 
 %% derive equations for fusion of synthetic deviation measurement
 % used to keep correct heading when operating without absolute position or