fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: remove legacy accel z bias checks (#23341)

Browse Source 
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar 2024-08-29 11:51:27 -04:00 committed by GitHub
parent f8188f0981
commit 5b0014cb06
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
7 changed files with 1 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld
View File

@ -377,7 +377,6 @@
*(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
*(.text.imxrt_epsubmit)
*(.text._ZN15PositionControl6updateEf)
*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE)
*(.text._ZN23MavlinkStreamScaledIMU24sendEv)
*(.text._ZN5PX4IO10io_reg_getEhhPtj)
*(.text.imxrt_dma_send)

1
msg/EstimatorStatusFlags.msg
View File

@ -59,7 +59,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)

2
src/modules/ekf2/EKF/common.h
View File

@ -513,7 +513,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;

2
src/modules/ekf2/EKF/ekf.cpp
View File

@ -114,8 +114,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

3
src/modules/ekf2/EKF/ekf.h
View File

@ -612,8 +612,6 @@ private:
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
@ -1094,7 +1092,6 @@ private:
void stopAuxVelFusion();
#endif // CONFIG_EKF2_AUXVEL
void checkVerticalAccelerationBias(const imuSample &imu_delayed);
void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
Likelihood estimateInertialNavFallingLikelihood() const;

101
src/modules/ekf2/EKF/height_control.cpp
View File

@ -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

2
src/modules/ekf2/EKF2.cpp
View File

@ -1937,7 +1937,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;
@ -2666,7 +2665,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;