fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 16:27: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-05-18 09:48:52 +10: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
+16 -22
View File
@@ -137,31 +137,25 @@ 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; // correct the state
// correct the state _terrain_vpos -= gain * _hagl_innov;
_terrain_vpos -= gain * _hagl_innov; // correct the variance
// correct the variance _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); // 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 {
// If we have been rejecting range data for too long, reset to measurement
if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) {
_terrain_vpos = _state.pos(2) + meas_hagl;
_terrain_var = obs_variance;
} else { } else {
// If we have been rejecting range data for too long, reset to measurement _innov_check_fail_status.flags.reject_hagl = true;
if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) {
_terrain_vpos = _state.pos(2) + meas_hagl;
_terrain_var = obs_variance;
} else {
_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;