From 99b34f0df48e104f6f15188c5f1d558f871cc0d8 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 7 Jun 2016 21:09:17 +1000
Subject: [PATCH] EKF: Only reset necessary terms when mag fusion covariance
 reset required

Only the quaternion and mag fusion state covariances are used in the mag fusion calculations.
---
 EKF/covariance.cpp | 16 ++++++++++++++++
 EKF/ekf.h          |  3 +++
 EKF/mag_fusion.cpp | 12 ++++++------
 3 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index f592dc941b..c994c1242b 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -778,3 +778,19 @@ void Ekf::fixCovarianceErrors()
 		makeSymmetrical(P,22,23);
 	}
 }
+
+void Ekf::resetMagCovariance()
+{	
+	// set the quaternion covariance terms to zero
+	zeroRows(P,0,3);
+	zeroCols(P,0,3);
+
+	// set the magnetic field covariance terms to zero
+	zeroRows(P,16,21);
+	zeroCols(P,16,21);
+
+	// set the field state variance to the observation variance
+	for (uint8_t rc_index=16; rc_index <= 21; rc_index ++) {
+		P[rc_index][rc_index] = sq(_params.mag_noise);
+	}
+}
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 17aa1bf28a..fbaa4448c2 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -399,4 +399,7 @@ private:
 	// initialise the quaternion covariances using rotation vector variances
 	void initialiseQuatCovariances(Vector3f &rot_vec_var);
 
+	// perform a limited reset of the magnetic field state covariances
+	void resetMagCovariance();
+
 };
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index b2d58703d7..d25a938a10 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -156,8 +156,8 @@ void Ekf::fuseMag()
 				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 				_fault_status.flags.bad_mag_x = true;
 
-				// we need to reinitialise the covariance matrix and abort this fusion step
-				initialiseCovariance();
+				// we need to re-initialise covariances and abort this fusion step
+				resetMagCovariance();
 				ECL_ERR("EKF magX fusion numerical error - covariance reset");
 				return;
 			}
@@ -208,8 +208,8 @@ void Ekf::fuseMag()
 				// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
 				_fault_status.flags.bad_mag_y = true;
 
-				// we need to reinitialise the covariance matrix and abort this fusion step
-				initialiseCovariance();
+				// we need to re-initialise covariances and abort this fusion step
+				resetMagCovariance();
 				ECL_ERR("EKF magY fusion numerical error - covariance reset");
 				return;
 			}
@@ -260,8 +260,8 @@ void Ekf::fuseMag()
 				// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
 				_fault_status.flags.bad_mag_z = true;
 
-				// we need to reinitialise the covariance matrix and abort this fusion step
-				initialiseCovariance();
+				// we need to re-initialise covariances and abort this fusion step
+				resetMagCovariance();
 				ECL_ERR("EKF magZ fusion numerical error - covariance reset");
 				return;
 			}