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

@@ -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);
+	}
+}

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();
+
 };

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;
 			}