Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

EKF: Restore protection from extreme innovation values

Browse Source
This commit is contained in:
Paul Riseborough
2021-01-21 10:13:28 +11:00
committed by Paul Riseborough
parent efb78deef9
commit b919f3ddf4
1 changed files with 4 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+4
View File
@@ -108,6 +108,10 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
const bool innov_check_pass = test_ratio(0) <= 1.0f; const bool innov_check_pass = test_ratio(0) <= 1.0f;
if (innov_check_pass || inhibit_gate) { if (innov_check_pass || inhibit_gate) {
if (inhibit_gate && test_ratio(0) > sq(100.0f / innov_gate(0))) {
// always protect against extreme values that could result in a NaN
return false;
}
if (!_fuse_hpos_as_odom) { if (!_fuse_hpos_as_odom) {
_time_last_hor_pos_fuse = _time_last_imu; _time_last_hor_pos_fuse = _time_last_imu;