ekf2: fusion always succeeds, do not return boolean

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -215,17 +215,13 @@ 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, H, aid_src.observation_variance, aid_src.innovation);
+	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
 
-	aid_src.fused = is_fused;
+	aid_src.fused = true;
-	_fault_status.flags.bad_airspeed = !is_fused;
+	aid_src.time_last_fuse = _time_delayed_us;
 
-	if (is_fused) {
+	if (!update_wind_only) {
-		aid_src.time_last_fuse = _time_delayed_us;
+		_time_last_hor_vel_fuse = _time_delayed_us;
-
-		if (!update_wind_only) {
-			_time_last_hor_vel_fuse = _time_delayed_us;
-		}
 	}
 }
 

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -107,8 +107,6 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 		bcoef_inv(1) = bcoef_inv(0);
 	}
 
-	bool fused[] {false, false};
-
 	Vector2f observation{};
 	Vector2f observation_variance{R_ACC, R_ACC};
 	Vector2f innovation{};
@@ -152,7 +150,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 		if (innovation_variance(axis_index) < R_ACC) {
 			// calculation is badly conditioned
-			break;
+			return;
 		}
 
 		const float test_ratio = sq(innovation(axis_index)) / (sq(innov_gate) * innovation_variance(axis_index));
@@ -164,9 +162,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 			VectorState K = P * H / innovation_variance(axis_index);
 
-			if (measurementUpdate(K, H, R_ACC, innovation(axis_index))) {
+			measurementUpdate(K, H, R_ACC, innovation(axis_index));
-				fused[axis_index] = true;
-			}
 		}
 	}
 
@@ -178,8 +174,6 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 			      innovation_variance,  // innovation variance
 			      innov_gate);          // innovation gate
 
-	if (fused[0] && fused[1]) {
+	_aid_src_drag.fused = true;
-		_aid_src_drag.fused = true;
+	_aid_src_drag.time_last_fuse = _time_delayed_us;
-		_aid_src_drag.time_last_fuse = _time_delayed_us;
-	}
 }

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -182,27 +182,24 @@ void Ekf::fuseBodyFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_
 			      innovation_gate);				// innovation gate
 
 	if (!aid_src.innovation_rejected) {
-		aid_src.fused = true;
-
 		for (uint8_t index = 0; index <= 2; index++) {
-			if (aid_src.fused) {
+			if (index == 1) {
-				if (index == 1) {
+				sym::ComputeBodyVelYInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]);
-					sym::ComputeBodyVelYInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]);
 
-				} else if (index == 2) {
+			} else if (index == 2) {
-					sym::ComputeBodyVelZInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]);
+				sym::ComputeBodyVelZInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]);
-				}
-
-				aid_src.innovation[index] = Vector3f(_R_to_earth.transpose().row(index)) * _state.vel - measurement(index);
-				VectorState Kfusion = P * H[index] / aid_src.innovation_variance[index];
-				aid_src.fused &= measurementUpdate(Kfusion, H[index], aid_src.observation_variance[index], aid_src.innovation[index]);
 			}
+
+			aid_src.innovation[index] = Vector3f(_R_to_earth.transpose().row(index)) * _state.vel - measurement(index);
+
+			VectorState Kfusion = P * H[index] / aid_src.innovation_variance[index];
+			measurementUpdate(Kfusion, H[index], aid_src.observation_variance[index], aid_src.innovation[index]);
 		}
 
-		if (aid_src.fused) {
+		aid_src.fused = true;
-			_time_last_hor_vel_fuse = _time_delayed_us;
+		aid_src.time_last_fuse = _time_delayed_us;
-			_time_last_ver_vel_fuse = _time_delayed_us;
+
-			aid_src.time_last_fuse = _time_delayed_us;
+		_time_last_hor_vel_fuse = _time_delayed_us;
-		}
+		_time_last_ver_vel_fuse = _time_delayed_us;
 	}
 }

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

@@ -63,10 +63,10 @@ void Ekf::controlFakeHgtFusion()
 
 				// always protect against extreme values that could result in a NaN
 				if (aid_src.test_ratio < sq(100.0f / innov_gate)) {
-					if (!aid_src.innovation_rejected
+					if (!aid_src.innovation_rejected) {
-					    && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance,
+						fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance,
-									  State::pos.idx + 2)
+									   State::pos.idx + 2);
-					   ) {
+
 						aid_src.fused = true;
 						aid_src.time_last_fuse = _time_delayed_us;
 					}

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

@@ -108,12 +108,12 @@ bool Ekf::runFakePosStateMachine(const bool enable_conditions_passing, bool stat
 {
 	if (status_flag) {
 		if (enable_conditions_passing) {
-			if (!aid_src.innovation_rejected
+			if (!aid_src.innovation_rejected) {
-			    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
+				for (unsigned i = 0; i < 2; i++) {
-							  State::pos.idx + 0)
+					fuseDirectStateMeasurement(aid_src.innovation[i], aid_src.innovation_variance[i], aid_src.observation_variance[i],
-			    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
+								   State::pos.idx + i);
-							  State::pos.idx + 1)
+				}
-			   ) {
+
 				aid_src.fused = true;
 				aid_src.time_last_fuse = _time_delayed_us;
 			}

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -205,14 +205,13 @@ void Ekf::fuseGnssYaw(float antenna_yaw_offset)
 	// only calculate gains for states we are using
 	VectorState Kfusion = P * H / aid_src.innovation_variance;
 
-	const bool is_fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
+	measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
-	_fault_status.flags.bad_hdg = !is_fused;
-	aid_src.fused = is_fused;
 
-	if (is_fused) {
+	_fault_status.flags.bad_hdg = false;
-		_time_last_heading_fuse = _time_delayed_us;
+	aid_src.fused = true;
-		aid_src.time_last_fuse = _time_delayed_us;
+	aid_src.time_last_fuse = _time_delayed_us;
-	}
+
+	_time_last_heading_fuse = _time_delayed_us;
 }
 
 bool Ekf::resetYawToGnss(const float gnss_yaw, const float gnss_yaw_offset)

src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp

@@ -78,8 +78,6 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 			      innovation_variance,                                         // innovation variance
 			      0.25f);                                                      // innovation gate
 
-	bool fused[3] {false, false, false};
-
 	// update the states and covariance using sequential fusion
 	for (uint8_t index = 0; index <= 2; index++) {
 		// Calculate Kalman gains and observation jacobians
@@ -108,13 +106,10 @@ 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, H, _aid_src_gravity.observation_variance[index],
+			measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]);
-							 _aid_src_gravity.innovation[index]);
 		}
 	}
 
-	if (fused[0] && fused[1] && fused[2]) {
+	_aid_src_gravity.fused = true;
-		_aid_src_gravity.fused = true;
+	_aid_src_gravity.time_last_fuse = imu.time_us;
-		_aid_src_gravity.time_last_fuse = imu.time_us;
-	}
 }

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -60,8 +60,6 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 
 	const auto state_vector = _state.vector();
 
-	bool fused[3] {false, false, false};
-
 	// update the states and covariance using sequential fusion of the magnetometer components
 	for (uint8_t index = 0; index <= 2; index++) {
 		// Calculate Kalman gains and observation jacobians
@@ -79,7 +77,6 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 		} else if (index == 2) {
 			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
 			if (_control_status.flags.synthetic_mag_z) {
-				fused[2] = true;
 				continue;
 			}
 
@@ -126,29 +123,21 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 			Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 		}
 
-		if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
+		measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index]);
-			fused[index] = true;
-		}
 	}
 
+	_fault_status.flags.bad_mag_x = false;
+	_fault_status.flags.bad_mag_y = false;
+	_fault_status.flags.bad_mag_z = false;
+
+	aid_src.fused = true;
+	aid_src.time_last_fuse = _time_delayed_us;
+
 	if (update_all_states) {
-		_fault_status.flags.bad_mag_x = !fused[0];
+		_time_last_heading_fuse = _time_delayed_us;
-		_fault_status.flags.bad_mag_y = !fused[1];
-		_fault_status.flags.bad_mag_z = !fused[2];
 	}
 
-	if (fused[0] && fused[1] && fused[2]) {
+	return true;
-		aid_src.fused = true;
-		aid_src.time_last_fuse = _time_delayed_us;
-
-		if (update_all_states) {
-			_time_last_heading_fuse = _time_delayed_us;
-		}
-
-		return true;
-	}
-
-	return false;
 }
 
 bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
@@ -184,11 +173,11 @@ bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_s
 		Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 	}
 
-	const bool is_fused = measurementUpdate(Kfusion, H, R, innovation);
+	measurementUpdate(Kfusion, H, R, innovation);
 
-	_fault_status.flags.bad_mag_decl = !is_fused;
+	_fault_status.flags.bad_mag_decl = false;
 
-	return is_fused;
+	return true;
 }
 
 float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -54,18 +54,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 		return false;
 	}
 
-	bool fused[2] {false, false};
-
 	// fuse observation axes sequentially
 	for (uint8_t index = 0; index <= 1; index++) {
-
-		if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) {
-			// we need to reinitialise the covariance matrix and abort this fusion step
-			ECL_ERR("Opt flow error - covariance reset");
-			initialiseCovariance();
-			return false;
-		}
-
 		if (index == 0) {
 			// everything was already computed above
 
@@ -80,35 +70,36 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1];
 		}
 
+		if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) {
+			// we need to reinitialise the covariance matrix and abort this fusion step
+			ECL_ERR("Opt flow error - covariance reset");
+			initialiseCovariance();
+			return false;
+		}
+
 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
 
 		if (!update_terrain) {
 			Kfusion(State::terrain.idx) = 0.f;
 		}
 
-		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index],
+		measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index],
-				      _aid_src_optical_flow.innovation[index])) {
+				  _aid_src_optical_flow.innovation[index]);
-			fused[index] = true;
-		}
 	}
 
-	_fault_status.flags.bad_optflow_X = !fused[0];
+	_fault_status.flags.bad_optflow_X = false;
-	_fault_status.flags.bad_optflow_Y = !fused[1];
+	_fault_status.flags.bad_optflow_Y = false;
 
-	if (fused[0] && fused[1]) {
+	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
-		_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
+	_aid_src_optical_flow.fused = true;
-		_aid_src_optical_flow.fused = true;
 
-		_time_last_hor_vel_fuse = _time_delayed_us;
+	_time_last_hor_vel_fuse = _time_delayed_us;
 
-		if (update_terrain) {
+	if (update_terrain) {
-			_time_last_terrain_fuse = _time_delayed_us;
+		_time_last_terrain_fuse = _time_delayed_us;
-		}
-
-		return true;
 	}
 
-	return false;
+	return true;
 }
 
 float Ekf::predictFlowRange() const

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -138,14 +138,12 @@ bool 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, H, sideslip.observation_variance, sideslip.innovation);
+	measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation);
 
-	sideslip.fused = is_fused;
+	sideslip.fused = true;
-	_fault_status.flags.bad_sideslip = !is_fused;
+	sideslip.time_last_fuse = _time_delayed_us;
 
-	if (is_fused) {
+	_fault_status.flags.bad_sideslip = false;
-		sideslip.time_last_fuse = _time_delayed_us;
-	}
 
-	return is_fused;
+	return true;
 }

src/modules/ekf2/EKF/ekf.h

@@ -246,7 +246,7 @@ public:
 	}
 
 	// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
-	bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);
+	void fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);
 
 	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation);
 
@@ -834,7 +834,8 @@ private:
 	void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H)
 	{
 		VectorState Kfusion = P * H / innov_var;
-		aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
+		measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
+		aid_src.fused = true;
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1005,7 +1005,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 	}
 }
 
-bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
+void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
 {
 	VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
 
@@ -1063,7 +1063,6 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c
 
 	// apply the state corrections
 	fuse(K, innov);
-	return true;
 }
 
 bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation)

src/modules/ekf2/EKF/position_fusion.cpp

@@ -56,12 +56,12 @@ void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, con
 bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 {
 	// x & y
-	if (!aid_src.innovation_rejected
+	if (!aid_src.innovation_rejected) {
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
+		for (unsigned i = 0; i < 2; i++) {
-					  State::pos.idx + 0)
+			fuseDirectStateMeasurement(aid_src.innovation[i], aid_src.innovation_variance[i], aid_src.observation_variance[i],
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
+						   State::pos.idx + i);
-					  State::pos.idx + 1)
+		}
-	   ) {
+
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;
 
@@ -77,10 +77,10 @@ bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 {
 	// z
-	if (!aid_src.innovation_rejected
+	if (!aid_src.innovation_rejected) {
-	    && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance,
+		fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance,
-					  State::pos.idx + 2)
+					   State::pos.idx + 2);
-	   ) {
+
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;
 

src/modules/ekf2/EKF/velocity_fusion.cpp

@@ -36,12 +36,12 @@
 bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
 {
 	// vx, vy
-	if (!aid_src.innovation_rejected
+	if (!aid_src.innovation_rejected) {
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
+		for (unsigned i = 0; i < 2; i++) {
-					  State::vel.idx + 0)
+			fuseDirectStateMeasurement(aid_src.innovation[i], aid_src.innovation_variance[i], aid_src.observation_variance[i],
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
+						   State::vel.idx + i);
-					  State::vel.idx + 1)
+		}
-	   ) {
+
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;
 
@@ -57,14 +57,12 @@ bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
 bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 {
 	// vx, vy, vz
-	if (!aid_src.innovation_rejected
+	if (!aid_src.innovation_rejected) {
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
+		for (unsigned i = 0; i < 3; i++) {
-					  State::vel.idx + 0)
+			fuseDirectStateMeasurement(aid_src.innovation[i], aid_src.innovation_variance[i], aid_src.observation_variance[i],
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
+						   State::vel.idx + i);
-					  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

@@ -98,21 +98,16 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
-	if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) {
+	measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation);
 
-		_time_last_heading_fuse = _time_delayed_us;
+	_time_last_heading_fuse = _time_delayed_us;
 
-		aid_src_status.time_last_fuse = _time_delayed_us;
+	aid_src_status.time_last_fuse = _time_delayed_us;
-		aid_src_status.fused = true;
+	aid_src_status.fused = true;
 
-		_fault_status.flags.bad_hdg = false;
+	_fault_status.flags.bad_hdg = false;
 
-		return true;
+	return true;
-	}
-
-	// otherwise
-	_fault_status.flags.bad_hdg = true;
-	return false;
 }
 
 void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const