From f26aca55a1c0194f15cb94c339864870321c2d5b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 8 Jun 2016 13:23:30 +1000
Subject: [PATCH] EKF: Fix bug in quaternion covariance initialisation function

The covariances between quaternion and non-quaternion states were not being updated
---
 EKF/ekf_helper.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index a9a91fc595..404f6f71a8 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -921,7 +921,11 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 		float t43 = t16-t25;
 		float t44 = t17-t36;
 
-		// auto-code generated using matlab symbolic toolbox
+		// zero all the quaternion covariances
+		zeroRows(P,0,3);
+		zeroCols(P,0,3);
+
+		// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
 		P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
 		P[0][1] = t22;
 		P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;