From 1977b11b50528c59817c6d5c196e6306ec4423ab Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 28 Jan 2019 11:56:39 +1100
Subject: [PATCH] EKF: Update cleaned up autocode fragment with sign error fix
 and missing LD

---
 .../scripts/Inertial Nav EKF/YawCovariance.c  | 30 ++++++++++++-------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c b/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c
index a4cf3934d1..89e0283708 100644
--- a/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c	
+++ b/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c	
@@ -3,6 +3,8 @@
  The variables _state.quat_nominal(0) -> _state.quat_nominal(3) are the attitude quaternions
  The variable daYawVar is the variance of the yaw angle uncertainty in rad**2
  See DeriveYawResetEquations.m for the derivation
+ The gnerate autocode has been cleaned up with removal of 0 coefficient terms and mirroring of lower
+ diagonal terms missing from the derivation script raw autocode output of C_code4.txt
 */
 
 // Intermediate variables
@@ -20,15 +22,21 @@ SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2])
 // Variance of yaw angle uncertainty (rad**2)
 const float daYawVar = TBD;
 
-// Add covariances for additonal yaw uncertainty to exisiting covariances.
+// Add covariances for additonal yaw uncertainty to existing covariances.
 // This assumes that the additional yaw error is uncorrrelated
-P[0][0] += daYawVar*sq(SQ[2]);
-P[0][1] += daYawVar*SQ[1]*SQ[2];
-P[1][1] += daYawVar*sq(SQ[1]);
-P[0][2] += daYawVar*SQ[0]*SQ[2];
-P[1][2] += daYawVar*SQ[0]*SQ[1];
-P[2][2] += daYawVar*sq(SQ[0]);
-P[0][3] += daYawVar*SQ[2]*SQ[3];
-P[1][3] += daYawVar*SQ[1]*SQ[3];
-P[2][3] += daYawVar*SQ[0]*SQ[3];
-P[3][3] += daYawVar*sq(SQ[3]);
+P[0][0] += yaw_variance*sq(SQ[2]);
+P[0][1] += yaw_variance*SQ[1]*SQ[2];
+P[1][1] += yaw_variance*sq(SQ[1]);
+P[0][2] += yaw_variance*SQ[0]*SQ[2];
+P[1][2] += yaw_variance*SQ[0]*SQ[1];
+P[2][2] += yaw_variance*sq(SQ[0]);
+P[0][3] -= yaw_variance*SQ[2]*SQ[3];
+P[1][3] -= yaw_variance*SQ[1]*SQ[3];
+P[2][3] -= yaw_variance*SQ[0]*SQ[3];
+P[3][3] += yaw_variance*sq(SQ[3]);
+P[1][0] += yaw_variance*SQ[1]*SQ[2];
+P[2][0] += yaw_variance*SQ[0]*SQ[2];
+P[2][1] += yaw_variance*SQ[0]*SQ[1];
+P[3][0] -= yaw_variance*SQ[2]*SQ[3];
+P[3][1] -= yaw_variance*SQ[1]*SQ[3];
+P[3][2] -= yaw_variance*SQ[0]*SQ[3];