ekf2: move fuseVelPosHeight() -> fuseDirectStateMeasurement()

- don't bother keeping bad_vel_{N,E,D} and bad_pos_{N,E,D} fault status bits

msg/EstimatorStatusFlags.msg

@@ -55,15 +55,9 @@ bool fs_bad_airspeed          #  5 - true if fusion of the airspeed has encounte
 bool fs_bad_sideslip          #  6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 bool fs_bad_optflow_x         #  7 - true if fusion of the optical flow X axis has encountered a numerical error
 bool fs_bad_optflow_y         #  8 - true if fusion of the optical flow Y axis has encountered a numerical error
-bool fs_bad_vel_n             #  9 - true if fusion of the North velocity has encountered a numerical error
+bool fs_bad_acc_bias          #  9 - true if bad delta velocity bias estimates have been detected
-bool fs_bad_vel_e             # 10 - true if fusion of the East velocity has encountered a numerical error
+bool fs_bad_acc_vertical      # 10 - true if bad vertical accelerometer data has been detected
-bool fs_bad_vel_d             # 11 - true if fusion of the Down velocity has encountered a numerical error
+bool fs_bad_acc_clipping      # 11 - true if delta velocity data contains clipping (asymmetric railing)
-bool fs_bad_pos_n             # 12 - true if fusion of the North position has encountered a numerical error
-bool fs_bad_pos_e             # 13 - true if fusion of the East position has encountered a numerical error
-bool fs_bad_pos_d             # 14 - true if fusion of the Down position has encountered a numerical error
-bool fs_bad_acc_bias          # 15 - true if bad delta velocity bias estimates have been detected
-bool fs_bad_acc_vertical      # 16 - true if bad vertical accelerometer data has been detected
-bool fs_bad_acc_clipping      # 17 - true if delta velocity data contains clipping (asymmetric railing)
 
 
 # innovation test failures

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.fuseVelPosHeight(innovation, innov_var(i), State::vel.idx + i);
+				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), State::vel.idx + i);
 			}
 
 			_time_last_zero_velocity_fuse = imu_delayed.time_us;

src/modules/ekf2/EKF/common.h

@@ -509,15 +509,9 @@ union fault_status_u {
 		bool bad_sideslip      : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 		bool bad_optflow_X     : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
 		bool bad_optflow_Y     : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
-		bool bad_vel_N         : 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error
+		bool bad_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
-		bool bad_vel_E         : 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error
+		bool bad_acc_vertical  : 1; ///< 10 - true if bad vertical accelerometer data has been detected
-		bool bad_vel_D         : 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error
+		bool bad_acc_clipping  : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
-		bool bad_pos_N         : 1; ///< 12 - true if fusion of the North position has encountered a numerical error
-		bool bad_pos_E         : 1; ///< 13 - true if fusion of the East position has encountered a numerical error
-		bool bad_pos_D         : 1; ///< 14 - true if fusion of the Down position has encountered a numerical error
-		bool bad_acc_bias      : 1; ///< 15 - true if bad delta velocity bias estimates have been detected
-		bool bad_acc_vertical  : 1; ///< 16 - true if bad vertical accelerometer data has been detected
-		bool bad_acc_clipping  : 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing)
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/ekf.h

@@ -327,8 +327,8 @@ public:
 #endif
 	}
 
-	// fuse single velocity and position measurement
+	// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
-	bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index);
+	bool fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index);
 
 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
@@ -1137,8 +1137,6 @@ private:
 	void resetFakePosFusion();
 	void stopFakePosFusion();
 
-	void setVelPosStatus(const int state_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)
 	// yaw_variance : yaw error variance (rad^2)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1015,3 +1015,39 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 		_accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
 	}
 }
+
+bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index)
+{
+	VectorState Kfusion;  // 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;
+	}
+
+	clearInhibitedStateKalmanGains(Kfusion);
+
+	SquareMatrixState KHP;
+
+	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);
+		}
+	}
+
+	const bool healthy = checkAndFixCovarianceUpdate(KHP);
+
+	if (healthy) {
+		// apply the covariance corrections
+		P -= KHP;
+
+		constrainStateVariances();
+
+		// apply the state corrections
+		fuse(Kfusion, innov);
+
+		return true;
+	}
+
+	return false;
+}

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -136,12 +136,14 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src)
 {
 	if (!aid_src.innovation_rejected) {
 		// vx, vy
-		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
+		if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
-		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
+		    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
 		   ) {
 			aid_src.fused = true;
 			aid_src.time_last_fuse = _time_delayed_us;
 
+			_time_last_hor_vel_fuse = _time_delayed_us;
+
 		} else {
 			aid_src.fused = false;
 		}
@@ -152,13 +154,16 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 {
 	if (!aid_src.innovation_rejected) {
 		// vx, vy, vz
-		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx)
+		if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
-		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
+		    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
-		    && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
+		    && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
 		   ) {
 			aid_src.fused = true;
 			aid_src.time_last_fuse = _time_delayed_us;
 
+			_time_last_hor_vel_fuse = _time_delayed_us;
+			_time_last_ver_vel_fuse = _time_delayed_us;
+
 		} else {
 			aid_src.fused = false;
 		}
@@ -169,12 +174,14 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 {
 	// x & y
 	if (!aid_src.innovation_rejected) {
-		if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
+		if (fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx)
-		    && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
+		    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
 		   ) {
 			aid_src.fused = true;
 			aid_src.time_last_fuse = _time_delayed_us;
 
+			_time_last_hor_pos_fuse = _time_delayed_us;
+
 		} else {
 			aid_src.fused = false;
 		}
@@ -185,119 +192,11 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 {
 	// z
 	if (!aid_src.innovation_rejected) {
-		if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
+		if (fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) {
 			aid_src.fused = true;
 			aid_src.time_last_fuse = _time_delayed_us;
-		}
-	}
-}
 
-// Helper function that fuses a single velocity or position measurement
-bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index)
-{
-	VectorState Kfusion;  // 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;
-	}
-
-	clearInhibitedStateKalmanGains(Kfusion);
-
-	SquareMatrixState KHP;
-
-	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);
-		}
-	}
-
-	const bool healthy = checkAndFixCovarianceUpdate(KHP);
-
-	setVelPosStatus(state_index, healthy);
-
-	if (healthy) {
-		// apply the covariance corrections
-		P -= KHP;
-
-		constrainStateVariances();
-
-		// apply the state corrections
-		fuse(Kfusion, innov);
-
-		return true;
-	}
-
-	return false;
-}
-
-void Ekf::setVelPosStatus(const int state_index, const bool healthy)
-{
-	switch (state_index) {
-	case State::vel.idx:
-		if (healthy) {
-			_fault_status.flags.bad_vel_N = false;
-			_time_last_hor_vel_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_vel_N = true;
-		}
-
-		break;
-
-	case State::vel.idx + 1:
-		if (healthy) {
-			_fault_status.flags.bad_vel_E = false;
-			_time_last_hor_vel_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_vel_E = true;
-		}
-
-		break;
-
-	case State::vel.idx + 2:
-		if (healthy) {
-			_fault_status.flags.bad_vel_D = false;
-			_time_last_ver_vel_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_vel_D = true;
-		}
-
-		break;
-
-	case State::pos.idx:
-		if (healthy) {
-			_fault_status.flags.bad_pos_N = false;
-			_time_last_hor_pos_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_pos_N = true;
-		}
-
-		break;
-
-	case State::pos.idx + 1:
-		if (healthy) {
-			_fault_status.flags.bad_pos_E = false;
-			_time_last_hor_pos_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_pos_E = true;
-		}
-
-		break;
-
-	case State::pos.idx + 2:
-		if (healthy) {
-			_fault_status.flags.bad_pos_D = false;
 			_time_last_hgt_fuse = _time_delayed_us;
-
-		} else {
-			_fault_status.flags.bad_pos_D = true;
 		}
-
-		break;
 	}
 }

src/modules/ekf2/EKF2.cpp

@@ -1896,12 +1896,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.fs_bad_sideslip          = _ekf.fault_status_flags().bad_sideslip;
 		status_flags.fs_bad_optflow_x         = _ekf.fault_status_flags().bad_optflow_X;
 		status_flags.fs_bad_optflow_y         = _ekf.fault_status_flags().bad_optflow_Y;
-		status_flags.fs_bad_vel_n             = _ekf.fault_status_flags().bad_vel_N;
-		status_flags.fs_bad_vel_e             = _ekf.fault_status_flags().bad_vel_E;
-		status_flags.fs_bad_vel_d             = _ekf.fault_status_flags().bad_vel_D;
-		status_flags.fs_bad_pos_n             = _ekf.fault_status_flags().bad_pos_N;
-		status_flags.fs_bad_pos_e             = _ekf.fault_status_flags().bad_pos_E;
-		status_flags.fs_bad_pos_d             = _ekf.fault_status_flags().bad_pos_D;
 		status_flags.fs_bad_acc_bias          = _ekf.fault_status_flags().bad_acc_bias;
 		status_flags.fs_bad_acc_vertical      = _ekf.fault_status_flags().bad_acc_vertical;
 		status_flags.fs_bad_acc_clipping      = _ekf.fault_status_flags().bad_acc_clipping;