EKF: consolidate covariance corrections

Combines the forced symmetry, variance limiting and zeroing of covariances for unwanted states in the one function.
This ensures a consistent correction is applied after every covariance prediction or correction.

EKF/airspeed_fusion.cpp

@@ -187,8 +187,7 @@ void Ekf::fuseAirspeed()
 			}
 
 			// correct the covariance marix for gross errors
-			makeSymmetrical();
+			fixCovarianceErrors();
-			limitCov();
 
 			// apply the state corrections
 			fuse(Kfusion, _airspeed_innov);

EKF/covariance.cpp

@@ -691,29 +691,13 @@ void Ekf::predictCovariance()
 		P[i][i] = nextP[i][i];
 	}
 
-	// prevent variances from becoming too large or negative
+	// fix gross errors in the covariance matrix and ensure rows and
-	limitCov();
+	// columns for un-used states are zero
-
+	fixCovarianceErrors();
-	// ensure coresponding rows and zeros for unwanted states are zero
-	if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) {
-		// deactivate XYZ accel bias states
-		zeroRows(P,13,15);
-		zeroCols(P,13,15);
-	}
-	if (!_control_status.flags.mag_3D) {
-		// deactivate magnetic field states
-		zeroRows(P,16,21);
-		zeroCols(P,16,21);
-	}
-	if (!_control_status.flags.wind) {
-		// deactivate wind states
-		zeroRows(P,22,23);
-		zeroCols(P,22,23);
-	}
 
 }
 
-void Ekf::limitCov()
+void Ekf::fixCovarianceErrors()
 {
 	// NOTE: This limiting is a last resort and should not be relied on
 	// TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions
@@ -731,43 +715,70 @@ void Ekf::limitCov()
 	P_lim[7] = 1e6f;		// wind max var
 
 	for (int i = 0; i <= 3; i++) {
-
+		// quaternion states
 		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[0]);
 	}
 
 	for (int i = 4; i <= 6; i++) {
-
+		// NED velocity states
 		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[1]);
 	}
 
 	for (int i = 7; i <= 9; i++) {
-
+		// NED position states
-
 		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[2]);
 	}
 
 	for (int i = 10; i <= 12; i++) {
-
+		// gyro bias states
-
 		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[3]);
 	}
 
-	for (int i = 13; i <= 15; i++) {
+	// force symmetry on the quaternion, velocity and positon state covariances
+	makeSymmetrical(P,0,12);
 
+	// the following states are optional and are deactivaed when not required
+	// by ensuring the corresponding covariance matrix values are kept at zero
 
-		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]);
+	// accelerometer bias states
+	if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) {
+		zeroRows(P,13,15);
+		zeroCols(P,13,15);
+	} else {
+		// constrain variances
+		for (int i = 13; i <= 15; i++) {
+			P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]);
+		}
+		// force symmetry
+		makeSymmetrical(P,13,15);
 	}
 
-
+	// magnetic field states
-	for (int i = 16; i <= 18; i++) {
+	if (!_control_status.flags.mag_3D) {
-		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]);
+		zeroRows(P,16,21);
+		zeroCols(P,16,21);
+	} else {
+		// constrain variances
+		for (int i = 16; i <= 18; i++) {
+			P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]);
+		}
+		for (int i = 19; i <= 21; i++) {
+			P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]);
+		}
+		// force symmetry
+		makeSymmetrical(P,16,21);
 	}
 
-	for (int i = 19; i <= 21; i++) {
+	// wind velocity states
-		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]);
+	if (!_control_status.flags.wind) {
-	}
+		zeroRows(P,22,23);
-
+		zeroCols(P,22,23);
-	for (int i = 22; i <= 23; i++) {
+	} else {
-		P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]);
+		// constrain variances
+		for (int i = 22; i <= 23; i++) {
+			P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]);
+		}
+		// force symmetry
+		makeSymmetrical(P,22,23);
 	}
 }

EKF/ekf.h

@@ -307,13 +307,11 @@ private:
 	// reset height state of the ekf
 	void resetHeight();
 
-	void makeCovSymetrical();
-
 	// limit the diagonal of the covariance matrix
-	void limitCov();
+	void fixCovarianceErrors();
 
-	// make ekf covariance matrix symmetric
+	// make ekf covariance matrix symmetric between a nominated state indexe range
-	void makeSymmetrical();
+	void makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
 
 	// constrain the ekf states
 	void constrainStates();

EKF/ekf_helper.cpp

@@ -292,13 +292,13 @@ void Ekf::calcMagDeclination()
 }
 
 // This function forces the covariance matrix to be symmetric
-void Ekf::makeSymmetrical()
+void Ekf::makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
 {
-	for (unsigned row = 0; row < _k_num_states; row++) {
+	for (unsigned row = first; row <= last; row++) {
 		for (unsigned column = 0; column < row; column++) {
-			float tmp = (P[row][column] + P[column][row]) / 2;
+			float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2;
-			P[row][column] = tmp;
+			cov_mat[row][column] = tmp;
-			P[column][row] = tmp;
+			cov_mat[column][row] = tmp;
 		}
 	}
 }

EKF/mag_fusion.cpp

@@ -362,8 +362,7 @@ void Ekf::fuseMag()
 			}
 
 			// correct the covariance marix for gross errors
-			makeSymmetrical();
+			fixCovarianceErrors();
-			limitCov();
 
 			// apply the state corrections
 			fuse(Kfusion, _mag_innov[index]);
@@ -637,8 +636,7 @@ void Ekf::fuseHeading()
 		}
 
 		// correct the covariance marix for gross errors
-		makeSymmetrical();
+		fixCovarianceErrors();
-		limitCov();
 
 		// apply the state corrections
 		fuse(Kfusion, _heading_innov);
@@ -772,8 +770,7 @@ void Ekf::fuseDeclination()
 		}
 
 		// correct the covariance marix for gross errors
-		makeSymmetrical();
+		fixCovarianceErrors();
-		limitCov();
 
 		// apply the state corrections
 		fuse(Kfusion, innovation);

EKF/optflow_fusion.cpp

@@ -486,8 +486,7 @@ void Ekf::fuseOptFlow()
 			}
 
 			// correct the covariance marix for gross errors
-			makeSymmetrical();
+			fixCovarianceErrors();
-			limitCov();
 
 			// apply the state corrections
 			fuse(gain, _flow_innov[obs_index]);

EKF/vel_pos_fusion.cpp

@@ -262,8 +262,7 @@ void Ekf::fuseVelPosHeight()
 			}
 
 			// correct the covariance marix for gross errors
-			makeSymmetrical();
+			fixCovarianceErrors();
-			limitCov();
 
 			// apply the state corrections
 			fuse(Kfusion, _vel_pos_innov[obs_index]);