Browse Source

ekf: robustify bad_acc_vertical check

when the vertical pos or vel innov ratio is above the threshold, the
other one needs to be significant too and not just positive to trigger
the failure
v1.13.0-BW
bresch 3 years ago committed by Daniel Agar
parent
commit
db0274e19b
  1. 3
      src/modules/ekf2/EKF/common.h
  2. 4
      src/modules/ekf2/EKF/control.cpp

3
src/modules/ekf2/EKF/common.h

@ -368,7 +368,8 @@ struct parameters { @@ -368,7 +368,8 @@ struct parameters {
float mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s)
// control of accel error detection and mitigation (IMU clipping)
const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure
const float vert_innov_test_min{1.0f}; ///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure
const int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec)
// auxiliary velocity fusion

4
src/modules/ekf2/EKF/control.cpp

@ -785,8 +785,8 @@ void Ekf::checkVerticalAccelerationHealth() @@ -785,8 +785,8 @@ void Ekf::checkVerticalAccelerationHealth()
const bool using_gps_for_both = _control_status.flags.gps_hgt && _control_status.flags.gps;
const bool using_ev_for_both = _control_status.flags.ev_hgt && _control_status.flags.ev_vel;
are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both);
is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > 0.0f;
is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > 0.0f;
is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > _params.vert_innov_test_min;
is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > _params.vert_innov_test_min;
} else {
// only height sensing available

Loading…
Cancel
Save