Introduce checkAndFixCovarianceUpdate(KHP) function

EKF/airspeed_fusion.cpp

@@ -177,21 +177,9 @@ void Ekf::fuseAirspeed()
 			}
 		}
 
-		// if the covariance correction will result in a negative variance, then
-		// the covariance matrix is unhealthy and must be corrected
-		bool healthy = true;
-		_fault_status.flags.bad_airspeed = false;
+		const bool healthy = checkAndFixCovarianceUpdate(KHP);
 
-		for (int i = 0; i < _k_num_states; i++) {
-			if (P(i,i) < KHP(i,i)) {
-				P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
-
-				healthy = false;
-
-				_fault_status.flags.bad_airspeed = true;
-
-			}
-		}
+		_fault_status.flags.bad_airspeed = !healthy;
 
 		if (healthy) {
 			// apply the covariance corrections

EKF/covariance.cpp

@@ -848,6 +848,20 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	}
 }
 
+// if the covariance correction will result in a negative variance, then
+// the covariance matrix is unhealthy and must be corrected
+bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) {
+	bool healthy = true;
+	for (int i = 0; i < _k_num_states; i++) {
+		if (P(i, i) < KHP(i, i)) {
+			P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
+			healthy = false;
+		}
+	}
+	return healthy;
+}
+
+
 void Ekf::resetMagRelatedCovariances()
 {
 	resetQuatCov();

EKF/drag_fusion.cpp

@@ -239,17 +239,7 @@ void Ekf::fuseDrag()
 				}
 			}
 
-			// if the covariance correction will result in a negative variance, then
-			// the covariance matrix is unhealthy and must be corrected
-			bool healthy = true;
-
-			for (int i = 0; i < _k_num_states; i++) {
-				if (P(i,i) < KHP(i,i)) {
-					P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
-
-					healthy = false;
-				}
-			}
+			bool healthy = checkAndFixCovarianceUpdate(KHP);
 
 			if (healthy) {
 				// apply the covariance corrections

EKF/ekf.h

@@ -662,6 +662,10 @@ private:
 
 	Vector3f getVisionVelocityVarianceInEkfFrame() const;
 
+	// if the covariance correction will result in a negative variance, then
+	// the covariance matrix is unhealthy and must be corrected
+	bool checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP);
+
 	// limit the diagonal of the covariance matrix
 	// force symmetry when the argument is true
 	void fixCovarianceErrors(bool force_symmetry);

EKF/gps_yaw_fusion.cpp

@@ -232,21 +232,9 @@ void Ekf::fuseGpsYaw()
 		}
 	}
 
-	// if the covariance correction will result in a negative variance, then
-	// the covariance matrix is unhealthy and must be corrected
-	bool healthy = true;
-	_fault_status.flags.bad_hdg = false;
+	const bool healthy = checkAndFixCovarianceUpdate(KHP);
 
-	for (int i = 0; i < _k_num_states; i++) {
-		if (P(i,i) < KHP(i,i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
-
-			healthy = false;
-
-			_fault_status.flags.bad_hdg = true;
-
-		}
-	}
+	_fault_status.flags.bad_hdg = !healthy;
 
 	if (healthy) {
 		// apply the covariance corrections

EKF/mag_fusion.cpp

@@ -143,13 +143,13 @@ void Ekf::fuseMag()
 	}
 
 	// Perform an innovation consistency check and report the result
-	bool healthy = true;
+	bool all_innovation_checks_passed = true;
 
 	for (uint8_t index = 0; index <= 2; index++) {
 		_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
 
 		if (_mag_test_ratio(index) > 1.0f) {
-			healthy = false;
+			all_innovation_checks_passed = false;
 			_innov_check_fail_status.value |= (1 << (index + 3));
 
 		} else {
@@ -161,7 +161,7 @@ void Ekf::fuseMag()
 	_yaw_test_ratio = 0.0f;
 
 	// if any axis fails, abort the mag fusion
-	if (!healthy) {
+	if (!all_innovation_checks_passed) {
 		return;
 	}
 
@@ -169,10 +169,6 @@ void Ekf::fuseMag()
 	// before they are used to constrain heading drift
 	const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
 
-	_fault_status.flags.bad_mag_x = false;
-	_fault_status.flags.bad_mag_y = false;
-	_fault_status.flags.bad_mag_z = false;
-
 	// Observation jacobian and Kalman gain vectors
 	Vector24f H_MAG;
 	Vector24f Kfusion;
@@ -367,28 +363,16 @@ void Ekf::fuseMag()
 			}
 		}
 
-		// if the covariance correction will result in a negative variance, then
-		// the covariance matrix is unhealthy and must be corrected
-
-		for (int i = 0; i < _k_num_states; i++) {
-			if (P(i,i) < KHP(i,i)) {
-				// zero rows and columns
-				P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
-
-				//flag as unhealthy
-				healthy = false;
+		const bool healthy = checkAndFixCovarianceUpdate(KHP);
 
-				// update individual measurement health status
-				if (index == 0) {
-					_fault_status.flags.bad_mag_x = true;
+		if (index == 0) {
+			_fault_status.flags.bad_mag_x = !healthy;
 
-				} else if (index == 1) {
-					_fault_status.flags.bad_mag_y = true;
+		} else if (index == 1) {
+			_fault_status.flags.bad_mag_y = !healthy;
 
-				} else if (index == 2) {
-					_fault_status.flags.bad_mag_z = true;
-				}
-			}
+		} else if (index == 2) {
+			_fault_status.flags.bad_mag_z = !healthy;
 		}
 
 		if (healthy) {
@@ -698,21 +682,9 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 		}
 	}
 
-	// if the covariance correction will result in a negative variance, then
-	// the covariance matrix is unhealthy and must be corrected
-	bool healthy = true;
-	_fault_status.flags.bad_hdg = false;
-
-	for (int i = 0; i < _k_num_states; i++) {
-		if (P(i,i) < KHP(i,i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
+	const bool healthy = checkAndFixCovarianceUpdate(KHP);
 
-			healthy = false;
-
-			_fault_status.flags.bad_hdg = true;
-
-		}
-	}
+	_fault_status.flags.bad_hdg = !healthy;
 
 	if (healthy) {
 		// apply the covariance corrections
@@ -983,20 +955,9 @@ void Ekf::fuseDeclination(float decl_sigma)
 		}
 	}
 
-	// if the covariance correction will result in a negative variance, then
-	// the covariance matrix is unhealthy and must be corrected
-	bool healthy = true;
-	_fault_status.flags.bad_mag_decl = false;
-	for (int i = 0; i < _k_num_states; i++) {
-		if (P(i,i) < KHP(i,i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
+	const bool healthy = checkAndFixCovarianceUpdate(KHP);
 
-			healthy = false;
-
-			_fault_status.flags.bad_mag_decl = true;
-
-		}
-	}
+	_fault_status.flags.bad_mag_decl = !healthy;
 
 	if (healthy) {
 		// apply the covariance corrections

EKF/optflow_fusion.cpp

@@ -425,9 +425,6 @@ void Ekf::fuseOptFlow()
 
 	}
 
-	_fault_status.flags.bad_optflow_X = false;
-	_fault_status.flags.bad_optflow_Y = false;
-
 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
 
 		// copy the Kalman gain vector for the axis we are fusing
@@ -465,23 +462,13 @@ void Ekf::fuseOptFlow()
 			}
 		}
 
-		// if the covariance correction will result in a negative variance, then
-		// the covariance matrix is unhealthy and must be corrected
-		bool healthy = true;
-
-		for (int i = 0; i < _k_num_states; i++) {
-			if (P(i,i) < KHP(i,i)) {
-				P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
-
-				healthy = false;
+		const bool healthy = checkAndFixCovarianceUpdate(KHP);
 
-				if (obs_index == 0) {
-					_fault_status.flags.bad_optflow_X = true;
+		if (obs_index == 0) {
+			_fault_status.flags.bad_optflow_X = !healthy;
 
-				} else if (obs_index == 1) {
-					_fault_status.flags.bad_optflow_Y = true;
-				}
-			}
+		} else if (obs_index == 1) {
+			_fault_status.flags.bad_optflow_Y = !healthy;
 		}
 
 		if (healthy) {

EKF/sideslip_fusion.cpp

@@ -224,20 +224,9 @@ void Ekf::fuseSideslip()
 			}
 		}
 
-		// if the covariance correction will result in a negative variance, then
-		// the covariance matrix is unhealthy and must be corrected
-		bool healthy = true;
-		_fault_status.flags.bad_sideslip = false;
+		const bool healthy = checkAndFixCovarianceUpdate(KHP);
 
-		for (int i = 0; i < _k_num_states; i++) {
-			if (P(i,i) < KHP(i,i)) {
-				P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
-
-				healthy = false;
-
-				_fault_status.flags.bad_sideslip = true;
-			}
-		}
+		_fault_status.flags.bad_sideslip = !healthy;
 
 		if (healthy) {
 			// apply the covariance corrections