Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

EKF: misc improvements to handling of accel clipping

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+1 -1
View File
@@ -354,7 +354,7 @@ struct parameters {
float bcoef_y{25.0f}; ///< ballistic coefficient along the Y-axis (kg/m**2) float bcoef_y{25.0f}; ///< ballistic coefficient along the Y-axis (kg/m**2)
// control of accel error detection and mitigation (IMU clipping) // 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) 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 // auxiliary velocity fusion
EKF/control.cpp
+4 -9
View File
@@ -893,8 +893,8 @@ void Ekf::checkVerticalAccelerationHealth()
{ {
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
// innovations being positive and not stale. // innovations being positive and not stale.
// Clipping causes the average accel reading to move towards zero which makes the INS // Clipping usually causes the average accel reading to move towards zero which makes the INS
// think it is falling and produces positive vertical innovations // think it is falling and produces positive vertical innovations.
// Don't use stale innovation data. // Don't use stale innovation data.
bool is_inertial_nav_falling = false; bool is_inertial_nav_falling = false;
bool are_vertical_pos_and_vel_independant = 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_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; 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); are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both);
if (are_vertical_pos_and_vel_independant) { is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > 0.0f;
if (_vert_pos_innov_ratio > 0.0f && _vert_vel_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_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;
}
} else { } else {
// only height sensing available // only height sensing available
is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim; is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim;
EKF/covariance.cpp
+1 -1
View File
@@ -152,7 +152,7 @@ void Ekf::predictCovariance()
// When on ground, only consider an accel bias observable if aligned with the gravity vector // 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 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) { if (do_inhibit_axis) {
// store the bias state variances to be reinstated later // store the bias state variances to be reinstated later