fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 07:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix bug preventing use of terrain estimator

Browse Source
This commit is contained in:
Paul Riseborough
2018-06-09 22:23:55 +02:00
committed by Lorenz Meier
parent adb4a09beb
commit 225057aaf5
1 changed files with 16 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
-6
View File
@@ -137,7 +137,6 @@ void Ekf::fuseHagl()
float gate_size = fmaxf(_params.range_innov_gate, 1.0f); float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
_terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); _terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (!_inhibit_gndobs_use) {
if (_terr_test_ratio <= 1.0f) { if (_terr_test_ratio <= 1.0f) {
// calculate the Kalman gain // calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var; float gain = _terrain_var / _hagl_innov_var;
@@ -148,20 +147,15 @@ void Ekf::fuseHagl()
// record last successful fusion event // record last successful fusion event
_time_last_hagl_fuse = _time_last_imu; _time_last_hagl_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hagl = false; _innov_check_fail_status.flags.reject_hagl = false;
} else { } else {
// If we have been rejecting range data for too long, reset to measurement // If we have been rejecting range data for too long, reset to measurement
if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) { if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) {
_terrain_vpos = _state.pos(2) + meas_hagl; _terrain_vpos = _state.pos(2) + meas_hagl;
_terrain_var = obs_variance; _terrain_var = obs_variance;
} else { } else {
_innov_check_fail_status.flags.reject_hagl = true; _innov_check_fail_status.flags.reject_hagl = true;
} }
} }
}
} else { } else {
_innov_check_fail_status.flags.reject_hagl = true; _innov_check_fail_status.flags.reject_hagl = true;
return; return;