ekf2: fusion helpers return success/fail and set pos/vel update timestamps centrally (if healthy)

src/modules/ekf2/EKF/control.cpp

@@ -1065,7 +1065,6 @@ void Ekf::controlAuxVelFusion()
 bool Ekf::hasHorizontalAidingTimedOut() const
 {
 	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
 	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
 	       && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
 }

src/modules/ekf2/EKF/ekf.cpp

@@ -213,7 +213,6 @@ bool Ekf::initialiseFilter()
 	// reset the essential fusion timeout counters
 	_time_last_hgt_fuse = _time_last_imu;
 	_time_last_hor_pos_fuse = _time_last_imu;
-	_time_last_delpos_fuse = _time_last_imu;
 	_time_last_hor_vel_fuse = _time_last_imu;
 	_time_last_hagl_fuse = _time_last_imu;
 	_time_last_flow_terrain_fuse = _time_last_imu;

src/modules/ekf2/EKF/ekf.h

@@ -102,8 +102,8 @@ public:
 
 	void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
 	void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
-
 	void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
+
 	void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
 	void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
 	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
@@ -219,8 +219,8 @@ public:
 	{
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
 		const bool is_mag_alignment_in_flight_complete = is_using_mag
-		                                                 && _control_status.flags.mag_aligned_in_flight
-		                                                 && ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6);
+				&& _control_status.flags.mag_aligned_in_flight
+				&& ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6);
 		return _control_status.flags.yaw_align
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
 	}
@@ -380,7 +380,6 @@ private:
 	uint64_t _time_last_hgt_fuse{0};	///< time the last fusion of vertical position measurements was performed (uSec)
 	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
-	uint64_t _time_last_delpos_fuse{0};	///< time the last fusion of incremental horizontal position measurements was performed (uSec)
 	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_arsp_fuse{0};	///< time the last fusion of airspeed measurements were performed (uSec)
@@ -584,20 +583,20 @@ private:
 	// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
 	// yaw_variance : variance of the yaw angle observation (rad^2)
 	// zero_innovation : Fuse data with innovation set to zero
-	void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
+	bool fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
 
 	// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
 	// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
 	// yaw_variance : variance of the yaw angle observation (rad^2)
 	// zero_innovation : Fuse data with innovation set to zero
-	void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
+	bool fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
 
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	// innovation : prediction - measurement
 	// variance : observaton variance
 	// gate_sigma : innovation consistency check gate size (Sigma)
 	// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
-	void updateQuaternion(const float innovation, const float variance, const float gate_sigma,
+	bool updateQuaternion(const float innovation, const float variance, const float gate_sigma,
 			      const Vector4f &yaw_jacobian);
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
@@ -629,7 +628,7 @@ private:
 	void fuseEvHgt();
 
 	// fuse single velocity and position measurement
-	void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
+	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
 
 	void resetVelocity();
 
@@ -1024,7 +1023,7 @@ private:
 	void stopFakePosFusion();
 	void fuseFakePosition();
 
-	void setVelPosFaultStatus(const int index, const bool status);
+	void setVelPosStatus(const int index, const bool healthy);
 
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1013,8 +1013,8 @@ void Ekf::update_deadreckoning_status()
 {
 	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
 				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
-				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
-				      || isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max));
+				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
+
 	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
 	const bool airDataAiding = _control_status.flags.wind &&
 				   isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -401,14 +401,18 @@ void Ekf::fuseMag(const Vector3f &mag)
 
 		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index));
 
-		if (index == 0) {
+		switch (index) {
+		case 0:
 			_fault_status.flags.bad_mag_x = !is_fused;
+			break;
 
-		} else if (index == 1) {
+		case 1:
 			_fault_status.flags.bad_mag_y = !is_fused;
+			break;
 
-		} else if (index == 2) {
+		case 2:
 			_fault_status.flags.bad_mag_z = !is_fused;
+			break;
 		}
 
 		if (is_fused) {
@@ -417,7 +421,7 @@ void Ekf::fuseMag(const Vector3f &mag)
 	}
 }
 
-void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
+bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 {
 	// assign intermediate state variables
 	const float &q0 = _state.quat_nominal(0);
@@ -481,7 +485,7 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 		H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
 		H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3);
 	} else {
-		return;
+		return false;
 	}
 
 	// calculate the yaw innovation and wrap to the interval between +-pi
@@ -498,10 +502,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 	float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
 
 	// Update the quaternion states and covariance matrix
-	updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
+	return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
 }
 
-void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
+bool Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 {
 	// assign intermediate state variables
 	const float q0 = _state.quat_nominal(0);
@@ -565,7 +569,7 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 		H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
 		H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3);
 	} else {
-		return;
+		return false;
 	}
 
 	float innovation;
@@ -582,11 +586,11 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 	float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
 
 	// Update the quaternion states and covariance matrix
-	updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
+	return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
 }
 
 // update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian
-void Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma,
+bool Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma,
 			   const Vector4f &yaw_jacobian)
 {
 	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
@@ -618,7 +622,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 		// we reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
 		ECL_ERR("mag yaw fusion numerical error - covariance reset");
-		return;
+		return false;
 	}
 
 	// calculate the Kalman gains
@@ -668,7 +672,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 			resetZDeltaAngBiasCov();
 
 		} else {
-			return;
+			return false;
 		}
 
 	} else {
@@ -711,7 +715,10 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 		// apply the state corrections
 		fuse(Kfusion, _heading_innov);
 
+		return true;
 	}
+
+	return false;
 }
 
 void Ekf::fuseHeading(float measured_hdg, float obs_var)

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -47,7 +47,6 @@
 bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
 				 Vector3f &innov_var, Vector2f &test_ratio)
 {
-
 	innov_var(0) = P(4, 4) + obs_var(0);
 	innov_var(1) = P(5, 5) + obs_var(1);
 	test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate) * innov_var(0)),
@@ -56,13 +55,12 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate,
 	const bool innov_check_pass = (test_ratio(0) <= 1.0f);
 
 	if (innov_check_pass) {
-		_time_last_hor_vel_fuse = _time_last_imu;
 		_innov_check_fail_status.flags.reject_hor_vel = false;
 
-		fuseVelPosHeight(innov(0), innov_var(0), 0);
-		fuseVelPosHeight(innov(1), innov_var(1), 1);
+		bool fuse_vx = fuseVelPosHeight(innov(0), innov_var(0), 0);
+		bool fuse_vy = fuseVelPosHeight(innov(1), innov_var(1), 1);
 
-		return true;
+		return fuse_vx && fuse_vy;
 
 	} else {
 		_innov_check_fail_status.flags.reject_hor_vel = true;
@@ -73,7 +71,6 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const float innov_gate,
 bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
 			       Vector3f &innov_var, Vector2f &test_ratio)
 {
-
 	innov_var(2) = P(6, 6) + obs_var(2);
 	test_ratio(1) = sq(innov(2)) / (sq(innov_gate) * innov_var(2));
 	_vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2));
@@ -94,12 +91,9 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, co
 	}
 
 	if (innov_check_pass) {
-		_time_last_ver_vel_fuse = _time_last_imu;
 		_innov_check_fail_status.flags.reject_ver_vel = false;
 
-		fuseVelPosHeight(innovation, innov_var(2), 2);
-
-		return true;
+		return fuseVelPosHeight(innovation, innov_var(2), 2);
 
 	} else {
 		_innov_check_fail_status.flags.reject_ver_vel = true;
@@ -124,19 +118,12 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate,
 			return false;
 		}
 
-		if (!_fuse_hpos_as_odom) {
-			_time_last_hor_pos_fuse = _time_last_imu;
-
-		} else {
-			_time_last_delpos_fuse = _time_last_imu;
-		}
-
 		_innov_check_fail_status.flags.reject_hor_pos = false;
 
-		fuseVelPosHeight(innov(0), innov_var(0), 3);
-		fuseVelPosHeight(innov(1), innov_var(1), 4);
+		bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3);
+		bool fuse_y = fuseVelPosHeight(innov(1), innov_var(1), 4);
 
-		return true;
+		return fuse_x && fuse_y;
 
 	} else {
 		_innov_check_fail_status.flags.reject_hor_pos = true;
@@ -167,11 +154,9 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
 	}
 
 	if (innov_check_pass) {
-		_time_last_hgt_fuse = _time_last_imu;
 		_innov_check_fail_status.flags.reject_ver_pos = false;
-		fuseVelPosHeight(innovation, innov_var, 5);
 
-		return true;
+		return fuseVelPosHeight(innovation, innov_var, 5);
 
 	} else {
 		_innov_check_fail_status.flags.reject_ver_pos = true;
@@ -180,9 +165,8 @@ bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const
 }
 
 // Helper function that fuses a single velocity or position measurement
-void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
+bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
 {
-
 	Vector24f Kfusion;  // Kalman gain vector for any single observation - sequential fusion is used.
 	const unsigned state_index = obs_index + 4;  // we start with vx and this is the 4. state
 
@@ -212,7 +196,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 		}
 	}
 
-	setVelPosFaultStatus(obs_index, !healthy);
+	setVelPosStatus(obs_index, healthy);
 
 	if (healthy) {
 		// apply the covariance corrections
@@ -222,27 +206,80 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 
 		// apply the state corrections
 		fuse(Kfusion, innov);
+
+		return true;
 	}
+
+	return false;
 }
 
-void Ekf::setVelPosFaultStatus(const int index, const bool status)
+void Ekf::setVelPosStatus(const int index, const bool healthy)
 {
-	if (index == 0) {
-		_fault_status.flags.bad_vel_N = status;
+	switch (index) {
+	case 0:
+		if (healthy) {
+			_fault_status.flags.bad_vel_N = false;
+			_time_last_hor_vel_fuse = _time_last_imu;
+
+		} else {
+			_fault_status.flags.bad_vel_N = true;
+		}
+
+		break;
+
+	case 1:
+		if (healthy) {
+			_fault_status.flags.bad_vel_E = false;
+			_time_last_hor_vel_fuse = _time_last_imu;
 
-	} else if (index == 1) {
-		_fault_status.flags.bad_vel_E = status;
+		} else {
+			_fault_status.flags.bad_vel_E = true;
+		}
+
+		break;
 
-	} else if (index == 2) {
-		_fault_status.flags.bad_vel_D = status;
+	case 2:
+		if (healthy) {
+			_fault_status.flags.bad_vel_D = false;
+			_time_last_ver_vel_fuse = _time_last_imu;
+
+		} else {
+			_fault_status.flags.bad_vel_D = true;
+		}
 
-	} else if (index == 3) {
-		_fault_status.flags.bad_pos_N = status;
+		break;
+
+	case 3:
+		if (healthy) {
+			_fault_status.flags.bad_pos_N = false;
+			_time_last_hor_pos_fuse = _time_last_imu;
+
+		} else {
+			_fault_status.flags.bad_pos_N = true;
+		}
 
-	} else if (index == 4) {
-		_fault_status.flags.bad_pos_E = status;
+		break;
+
+	case 4:
+		if (healthy) {
+			_fault_status.flags.bad_pos_E = false;
+			_time_last_hor_pos_fuse = _time_last_imu;
+
+		} else {
+			_fault_status.flags.bad_pos_E = true;
+		}
+
+		break;
+
+	case 5:
+		if (healthy) {
+			_fault_status.flags.bad_pos_D = false;
+			_time_last_hgt_fuse = _time_last_imu;
+
+		} else {
+			_fault_status.flags.bad_pos_D = true;
+		}
 
-	} else if (index == 5) {
-		_fault_status.flags.bad_pos_D = status;
+		break;
 	}
 }