fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-09 06:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add bad accel bias checking for EV and range finder height use cases

Browse Source
This commit is contained in:
Paul Riseborough
2021-01-09 11:03:31 -05:00
committed by Daniel Agar
parent ff3c0a6136
commit 99575d73e1
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+4 -2
View File
@@ -949,8 +949,10 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
&& ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps) && ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
|| (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) ) || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) )
&& ( (down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt) && ( (down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
|| (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt) ) ); || (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt)
|| (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt)
|| (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt) ) );
// record the pass/fail // record the pass/fail
if (!bad_acc_bias) { if (!bad_acc_bias) {