From bce1b96d17bfdd374b829db68948cd6330289a92 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 21 Dec 2018 11:34:57 +1100
Subject: [PATCH] EKF: Add function enabling yaw variance to be increased

---
 EKF/ekf.h                                     |  4 +++
 EKF/ekf_helper.cpp                            | 32 +++++++++++++++++++
 .../scripts/Inertial Nav EKF/YawCovariance.c  | 16 +++++-----
 3 files changed, 44 insertions(+), 8 deletions(-)

diff --git a/EKF/ekf.h b/EKF/ekf.h
index 83a3d1cc8c..b68207d2a3 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -668,4 +668,8 @@ private:
 	// check that the range finder data is continuous
 	void checkRangeDataContinuity();
 
+	// Increase the yaw error variance of the quaternions
+	// Argument is additional yaw variance in rad**2
+	void increaseQuatYawErrVariance(float yaw_variance);
+
 };
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 8956546384..e911e8f467 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -1647,3 +1647,35 @@ void Ekf::get_ekf2ev_quaternion(float *quat)
 		quat[i] = quat_ekf2ev(i);
 	}
 }
+
+// Increase the yaw error variance of the quaternions
+// Argument is additional yaw variance in rad**2
+void Ekf::increaseQuatYawErrVariance(float yaw_variance)
+{
+	// See DeriveYawResetEquations.m for derivation
+
+	// Intermediate variables
+	float SG[3];
+	SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3));
+	SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3);
+	SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3);
+
+	float SQ[4];
+	SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
+	SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
+	SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
+	SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
+
+	// Add covariances for additonal yaw uncertainty to existing covariances.
+	// This assumes that the additional yaw error is uncorrrelated to existing errors
+	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]);
+}
diff --git a/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c b/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c
index 5db9f72376..a4cf3934d1 100644
--- a/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c	
+++ b/EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c	
@@ -1,21 +1,21 @@
 /*
  C code fragment for function that enables the yaw uncertainty to be increased following a yaw reset.
- The variables q0 -> q3 are the attitude quaternions
+ 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
 */
 
 // Intermediate variables
 float SG[3];
-SG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
-SG[1] = 2*q0*q2 - 2*q1*q3;
-SG[2] = 2*q0*q1 + 2*q2*q3;
+SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3));
+SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3);
+SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3);
 
 float SQ[4];
-SQ[0] = 0.5f * ((q1*SG[0]) - (q0*SG[2]) + (q3*SG[1]));
-SQ[1] = 0.5f * ((q0*SG[1]) - (q2*SG[0]) + (q3*SG[2]));
-SQ[2] = 0.5f * ((q3*SG[0]) - (q1*SG[1]) + (q2*SG[2]));
-SQ[3] = 0.5f * ((q0*SG[0]) + (q1*SG[2]) + (q2*SG[1]));
+SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
+SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
+SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
+SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
 
 // Variance of yaw angle uncertainty (rad**2)
 const float daYawVar = TBD;