ekf2: remove legacy accel z bias checks

PX4 · Jul 15, 2024 · f798feb · f798feb
1 parent 1cd7d54
commit f798feb
Show file tree

Hide file tree

Showing 7 changed files with 1 addition and 111 deletions.
diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld
@@ -379,7 +379,6 @@
 *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
 *(.text.imxrt_epsubmit)
 *(.text._ZN15PositionControl6updateEf)
-*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE)
 *(.text._ZN23MavlinkStreamScaledIMU24sendEv)
 *(.text._ZN5PX4IO10io_reg_getEhhPtj)
 *(.text.imxrt_dma_send)

diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg
@@ -57,7 +57,6 @@ 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_acc_bias          #  9 - true if bad delta velocity bias estimates have been detected
 bool fs_bad_acc_vertical      # 10 - true if bad vertical accelerometer data has been detected
 bool fs_bad_acc_clipping      # 11 - true if delta velocity data contains clipping (asymmetric railing)
 

diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
@@ -506,7 +506,7 @@ 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_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
+		bool __UNUSED          : 1; ///< 9 -
 		bool bad_acc_vertical  : 1; ///< 10 - true if bad vertical accelerometer data has been detected
 		bool bad_acc_clipping  : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
 	} flags;

diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
@@ -113,8 +113,6 @@ void Ekf::reset()
 
 	_last_known_pos.setZero();
 
-	_time_acc_bias_check = 0;
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	_baro_counter = 0;
 #endif // CONFIG_EKF2_BAROMETER

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -588,8 +588,6 @@ class Ekf final : public EstimatorInterface
 
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
-	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
-
 	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s
 
 	Dcmf _R_to_earth{};	///< transformation matrix from body frame to earth frame from last EKF prediction
@@ -1060,7 +1058,6 @@ class Ekf final : public EstimatorInterface
 	void stopAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL
 
-	void checkVerticalAccelerationBias(const imuSample &imu_delayed);
 	void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
 	Likelihood estimateInertialNavFallingLikelihood() const;
 

diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp
@@ -39,7 +39,6 @@
 
 void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 {
-	checkVerticalAccelerationBias(imu_delayed);
 	checkVerticalAccelerationHealth(imu_delayed);
 
 #if defined(CONFIG_EKF2_BAROMETER)
@@ -120,106 +119,6 @@ void Ekf::checkHeightSensorRefFallback()
 	}
 }
 
-void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed)
-{
-	// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
-	// calculate accel bias term aligned with the gravity vector
-	const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
-	const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg;
-	const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
-
-	// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
-	bool bad_acc_bias = false;
-
-	if (fabsf(down_dvel_bias) > dVel_bias_lim) {
-
-		bool bad_vz = false;
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-
-		if (_control_status.flags.ev_hgt) {
-			if (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.f) {
-				bad_vz = true;
-			}
-		}
-
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_GNSS)
-
-		if (_control_status.flags.gps) {
-			if (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.f) {
-				bad_vz = true;
-			}
-		}
-
-#endif // CONFIG_EKF2_GNSS
-
-		if (bad_vz) {
-#if defined(CONFIG_EKF2_BAROMETER)
-
-			if (_control_status.flags.baro_hgt) {
-				if (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-
-			if (_control_status.flags.ev_hgt) {
-				if (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_GNSS)
-
-			if (_control_status.flags.gps_hgt) {
-				if (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_GNSS
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-
-			if (_control_status.flags.rng_hgt) {
-				if (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_RANGE_FINDER
-		}
-	}
-
-	// record the pass/fail
-	if (!bad_acc_bias) {
-		_fault_status.flags.bad_acc_bias = false;
-		_time_acc_bias_check = _time_delayed_us;
-
-	} else {
-		_fault_status.flags.bad_acc_bias = true;
-	}
-
-	// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
-	// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
-	if (_fault_status.flags.bad_acc_bias && isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) {
-
-		resetAccelBiasCov();
-
-		_time_acc_bias_check = imu_delayed.time_us;
-
-		_fault_status.flags.bad_acc_bias = false;
-		ECL_WARN("invalid accel bias - covariance reset");
-	}
-}
-
 void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)
 {
 	// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
@@ -1942,7 +1942,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_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;
 
@@ -2654,7 +2653,6 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 	const bool bias_valid = (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::AccelBias))
 				&& _ekf.control_status_flags().tilt_align
 				&& (_ekf.fault_status().value == 0)
-				&& !_ekf.fault_status_flags().bad_acc_bias
 				&& !_ekf.fault_status_flags().bad_acc_clipping
 				&& !_ekf.fault_status_flags().bad_acc_vertical;