dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix bug preventing use of terrain estimator

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
EKF/terrain_estimator.cpp
View File

@ -137,31 +137,25 @@ void Ekf::fuseHagl()
float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
_terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (!_inhibit_gndobs_use) {
if (_terr_test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state
_terrain_vpos -= gain * _hagl_innov;
// correct the variance
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// record last successful fusion event
_time_last_hagl_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hagl = false;
if (_terr_test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state
_terrain_vpos -= gain * _hagl_innov;
// correct the variance
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// record last successful fusion event
_time_last_hagl_fuse = _time_last_imu;
_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 {
// 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 {
_innov_check_fail_status.flags.reject_hagl = true;
}
_innov_check_fail_status.flags.reject_hagl = true;
}
}
} else {
_innov_check_fail_status.flags.reject_hagl = true;
return;