ekf2: use Joseph covariance update

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

@@ -60,22 +60,13 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 		if (zero_gyro_update_data_ready) {
 
 			Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt;
-			Vector3f innovation = ekf.state().gyro_bias - gyro_bias;
 
 			const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f));
 
-			const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var;
-
-			for (int i = 0; i < 3; i++) {
-				Ekf::VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
-				const unsigned state_index = State::gyro_bias.idx + i;
-
-				// calculate kalman gain K = PHS, where S = 1/innovation variance
-				for (int row = 0; row < State::size; row++) {
-					K(row) = ekf.stateCovariance(row, state_index) / innov_var(i);
-				}
-
-				ekf.measurementUpdate(K, innov_var(i), innovation(i));
+			for (unsigned i = 0; i < 3; i++) {
+				const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i);
+				const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var;
+				ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i);
 			}
 
 			// Reset the integrators

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

@@ -66,7 +66,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 
 			for (unsigned i = 0; i < 3; i++) {
 				const float innovation = ekf.state().vel(i) - vel_obs(i);
-				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), State::vel.idx + i);
+				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}
 
 			_time_last_zero_velocity_fuse = imu_delayed.time_us;

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

@@ -328,7 +328,7 @@ class Ekf final : public EstimatorInterface
 	}
 
 	// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
-	bool fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index);
+	bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);
 
 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
@@ -468,35 +468,39 @@ class Ekf final : public EstimatorInterface
 	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);
-
-		if (is_healthy) {
-			// apply the covariance corrections
-			P -= KHP;
+		// Step 2: stabilized update
+		PH = P * H;
 
-			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 @@ class Ekf final : public EstimatorInterface
 #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/ekf_helper.cpp

@@ -839,38 +839,44 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 	}
 }
 
-bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index)
+bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
 {
-	VectorState Kfusion;  // Kalman gain vector for any single observation - sequential fusion is used.
+	VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
 
 	// calculate kalman gain K = PHS, where S = 1/innovation variance
 	for (int row = 0; row < State::size; row++) {
-		Kfusion(row) = P(row, state_index) / innov_var;
+		K(row) = P(row, state_index) / innov_var;
 	}
 
-	clearInhibitedStateKalmanGains(Kfusion);
+	clearInhibitedStateKalmanGains(K);
 
-	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 column = 0; column < State::size; column++) {
-			KHP(row, column) = Kfusion(row) * P(state_index, column);
+	// Step 1: conventional update
+	VectorState PH = P.row(state_index);
+
+	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 healthy = checkAndFixCovarianceUpdate(KHP);
-
-	if (healthy) {
-		// apply the covariance corrections
-		P -= KHP;
-
-		constrainStateVariances();
-
-		// apply the state corrections
-		fuse(Kfusion, innov);
+	// Step 2: stabilized update
+	PH = P.row(state_index);
 
-		return true;
+	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 false;
+	constrainStateVariances();
+
+	// apply the state corrections
+	fuse(K, innov);
+	return true;
 }

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/position_fusion.cpp

@@ -80,8 +80,8 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 {
 	// x & y
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx + 0)
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
+	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0)
+	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1)
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;
@@ -97,7 +97,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 {
 	// z
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)
+	    && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2)
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;

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/velocity_fusion.cpp

@@ -83,8 +83,8 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
 {
 	// vx, vy
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
+	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
+	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;
@@ -100,9 +100,9 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 {
 	// vx, vy, vz
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
-	    && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
+	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
+	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
+	    && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2)
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;

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;