Merge pull request #159 from priseborough/pr-ekf2MagBugFix

EKF: Fix critical bugs in mag fusion and yaw reset

EKF/airspeed_fusion.cpp

@@ -40,6 +40,7 @@
  * @author Paul Riseborough <[email protected]>
  *
  */
+#include "../ecl.h"
 #include "ekf.h"
 #include "mathlib.h"
 
@@ -92,9 +93,10 @@ void Ekf::fuseAirspeed()
 			SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
 			_fault_status.flags.bad_airspeed = false;
 
-		} else { // Reset the estimator
+		} else { // Reset the estimator covarinace matrix
 			_fault_status.flags.bad_airspeed = true;
 			initialiseCovariance();
+			ECL_ERR("EKF airspeed fusion numerical error - covariance reset");
 			return;
 		}
 

EKF/covariance.cpp

@@ -91,21 +91,16 @@ void Ekf::initialiseCovariance()
 	P[15][15] = P[13][13];
 
 	// 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.0f;
-	P[17][17] = 0.0f;
-	P[18][18] = 0.0f;
-
-	// body magnetic field
-	P[19][19] = 0.0f;
-	P[20][20] = 0.0f;
-	P[21][21] = 0.0f;
+	// earth frame and body frame magnetic field
+	// set to observation variance
+	for (uint8_t index=16; index <= 21; index ++) {
+		P[index][index] = sq(_params.mag_noise);
+	}
 
 	// wind
-	P[22][22] = 0.0f;
-	P[23][23] = 0.0f;
+	P[22][22] = 1.0f;
+	P[23][23] = 1.0f;
 
 }
 
@@ -783,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 @@ class Ekf : public EstimatorInterface
 	// 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/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;