dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
bresch 2022-04-25 10:24:22 +02:00 committed by Daniel Agar
parent 980f696023
commit db0274e19b
2 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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