ekf2: use Joseph stabilized covariance update

src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp

@@ -75,7 +75,10 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 					K(row) = ekf.stateCovariance(row, state_index) / innov_var(i);
 				}
 
-				ekf.measurementUpdate(K, innov_var(i), innovation(i));
+				Ekf::VectorState H{};
+				H(State::gyro_bias.idx + i) = 1.f;
+
+				ekf.measurementUpdate(K, H, obs_var, innovation(i), innov_var(i));
 			}
 
 			// Reset the integrators

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -208,7 +208,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation);
+	const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
 
 	aid_src.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;

src/modules/ekf2/EKF/covariance.cpp

@@ -222,14 +222,6 @@ void Ekf::constrainStateVariances()
 #endif // CONFIG_EKF2_WIND
 }
 
-void Ekf::forceCovarianceSymmetry()
-{
-	// DEBUG
-	// P.isBlockSymmetric(0, 1e-9f);
-
-	P.makeRowColSymmetric<State::size>(0);
-}
-
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
 {
 	for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
@@ -256,22 +248,6 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max,
 	}
 }
 
-// if the covariance correction will result in a negative variance, then
-// the covariance matrix is unhealthy and must be corrected
-bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
-{
-	bool healthy = true;
-
-	for (int i = 0; i < State::size; i++) {
-		if (P(i, i) < KHP(i, i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.f);
-			healthy = false;
-		}
-	}
-
-	return healthy;
-}
-
 void Ekf::resetQuatCov(const float yaw_noise)
 {
 	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -160,7 +160,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 			VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index];
 
-			if (measurementUpdate(K, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) {
+			if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) {
 				fused[axis_index] = true;
 			}
 		}

src/modules/ekf2/EKF/ekf.h

@@ -468,35 +468,39 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
-	bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
+	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation)
 	{
 		clearInhibitedStateKalmanGains(K);
 
-		const VectorState KS = K * innovation_variance;
-		SquareMatrixState KHP;
+		// Efficient implementation of the Joseph stabilized covariance update
+		// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
 
-		for (unsigned row = 0; row < State::size; row++) {
-			for (unsigned col = 0; col < State::size; col++) {
-				// Instead of literally computing KHP, use an equivalent
-				// equation involving less mathematical operations
-				KHP(row, col) = KS(row) * K(col);
+		// Step 1: conventional update
+		VectorState PH = P * H;
+
+		for (unsigned i = 0; i < State::size; i++) {
+			for (unsigned j = 0; j <= i; j++) {
+				P(i, j) = P(i, j) - K(i) * PH(j);
+				P(j, i) = P(i, j);
 			}
 		}
 
-		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
+		// Step 2: stabilized update
+		PH = P * H;
 
-		if (is_healthy) {
-			// apply the covariance corrections
-			P -= KHP;
-
-			constrainStateVariances();
-			forceCovarianceSymmetry();
-
-			// apply the state corrections
-			fuse(K, innovation);
+		for (unsigned i = 0; i < State::size; i++) {
+			for (unsigned j = 0; j <= i; j++) {
+				float s = .5f * (P(i, j) - PH(i) * K(j) + P(i, j) - PH(j) * K(i));
+				P(i, j) = s + K(i) * R * K(j);
+				P(j, i) = P(i, j);
+			}
 		}
 
-		return is_healthy;
+		constrainStateVariances();
+
+		// apply the state corrections
+		fuse(K, innovation);
+		return true;
 	}
 
 	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
@@ -942,15 +946,9 @@ private:
 #endif // CONFIG_EKF2_WIND
 	}
 
-	// if the covariance correction will result in a negative variance, then
-	// the covariance matrix is unhealthy and must be corrected
-	bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP);
-
 	// limit the diagonal of the covariance matrix
 	void constrainStateVariances();
 
-	void forceCovarianceSymmetry();
-
 	void constrainStateVar(const IdxDof &state, float min, float max);
 	void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f);
 

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -134,7 +134,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 	// only calculate gains for states we are using
 	VectorState Kfusion = P * H / gnss_yaw.innovation_variance;
 
-	const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation);
+	const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation);
 	_fault_status.flags.bad_hdg = !is_fused;
 	gnss_yaw.fused = is_fused;
 

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -111,7 +111,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 		const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
 
 		if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
-			fused[index] = measurementUpdate(K, _aid_src_gravity.innovation_variance[index], _aid_src_gravity.innovation[index]);
+			fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]);
 		}
 	}
 

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -191,7 +191,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 			Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 		}
 
-		if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
 			fused[index] = true;
 			limitDeclination();
 
@@ -261,7 +261,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
 		// Calculate the Kalman gains
 		VectorState Kfusion = P * H / innovation_variance;
 
-		const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
+		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
 
 		_fault_status.flags.bad_mag_decl = !is_fused;
 

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -146,7 +146,7 @@ void Ekf::fuseOptFlow()
 
 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
 
-		if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
 	}

src/modules/ekf2/EKF/sideslip_fusion.cpp

@@ -142,7 +142,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation);
+	const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation);
 
 	sideslip.fused = is_fused;
 	_fault_status.flags.bad_sideslip = !is_fused;

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -113,7 +113,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
-	if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
+	if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) {
 
 		_time_last_heading_fuse = _time_delayed_us;