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

EKF: misc improvements to handling of accel clipping

This commit is contained in:
Paul Riseborough 2020-06-23 19:21:27 +10:00 committed by Paul Riseborough
parent 9c89fa3b29
commit c566318db5
3 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/common.h
View File

@ -354,7 +354,7 @@ struct parameters {
float bcoef_y{25.0f}; ///< ballistic coefficient along the Y-axis (kg/m**2)
// control of accel error detection and mitigation (IMU clipping)
float vert_innov_test_lim{4.5f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
float vert_innov_test_lim{3.0f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
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

13
EKF/control.cpp
View File

@ -893,8 +893,8 @@ void Ekf::checkVerticalAccelerationHealth()
{
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
// innovations being positive and not stale.
// Clipping causes the average accel reading to move towards zero which makes the INS
// think it is falling and produces positive vertical innovations
// Clipping usually causes the average accel reading to move towards zero which makes the INS
// think it is falling and produces positive vertical innovations.
// Don't use stale innovation data.
bool is_inertial_nav_falling = false;
bool are_vertical_pos_and_vel_independant = false;
@ -905,13 +905,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);
if (are_vertical_pos_and_vel_independant) {
if (_vert_pos_innov_ratio > 0.0f && _vert_vel_innov_ratio > 0.0f) {
is_inertial_nav_falling = _vert_pos_innov_ratio * _vert_vel_innov_ratio > 0.5f * sq(_params.vert_innov_test_lim);
}
} else {
is_inertial_nav_falling = _vert_vel_innov_ratio > _params.vert_innov_test_lim || _vert_pos_innov_ratio > _params.vert_innov_test_lim;
}
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;
} else {
// only height sensing available
is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim;

2
EKF/covariance.cpp
View File

@ -152,7 +152,7 @@ void Ekf::predictCovariance()
// When on ground, only consider an accel bias observable if aligned with the gravity vector
const bool is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.8f) || _control_status.flags.in_air;
const bool do_inhibit_axis = do_inhibit_all_axes || !is_bias_observable;
const bool do_inhibit_axis = do_inhibit_all_axes || !is_bias_observable || _imu_sample_delayed.delta_vel_clipping[index];
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later