From 69b8982043258bfa32d344b25c34d4a20d19da21 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 16 Feb 2016 10:42:18 +1100
Subject: [PATCH] EKF: Initialise variances to zero for un-used states

Setting these variances to zero makes it less likely that these states will be modified by fusion processing.
---
 EKF/covariance.cpp | 19 +++++++++++--------
 1 file changed, 11 insertions(+), 8 deletions(-)

diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 105a692602..45b63a00d0 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -81,19 +81,22 @@ void Ekf::initialiseCovariance()
 	// accel z bias
 	P[15][15] = 0.0001f;
 
+	// variances for optional states
+	// these state variances are set to zero until the states are required, then they must be initialised
+
 	// earth magnetic field
-	P[16][16] = 0.0001f;
-	P[17][17] = 0.0001f;
-	P[18][18] = 0.0001f;
+	P[16][16] = 0.0f;
+	P[17][17] = 0.0f;
+	P[18][18] = 0.0f;
 
 	// body magnetic field
-	P[19][19] = 0.0001f;
-	P[20][20] = 0.0001f;
-	P[21][21] = 0.0001f;
+	P[19][19] = 0.0f;
+	P[20][20] = 0.0f;
+	P[21][21] = 0.0f;
 
 	// wind
-	P[22][22] = 0.01f;
-	P[23][23] = 0.01f;
+	P[22][22] = 0.0f;
+	P[23][23] = 0.0f;
 
 }