From e8e9e34a73d3eda0923245e4ac1495b9fc52ae0d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 10 May 2018 13:14:09 +1000 Subject: [PATCH] EKF: fix bug causing height offset when GPS use stops This bug causes the last vertical velocity observation to be continuously fused. --- EKF/vel_pos_fusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 1b9b2f1204..e87f1899d6 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -183,7 +183,7 @@ void Ekf::fuseVelPosHeight() innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; // record the successful velocity fusion event - if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) { + if ((_fuse_hor_vel || _fuse_hor_vel_aux || _fuse_vert_vel) && vel_check_pass) { _time_last_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_vel_NED = false; @@ -191,7 +191,7 @@ void Ekf::fuseVelPosHeight() _innov_check_fail_status.flags.reject_vel_NED = true; } - _fuse_hor_vel = _fuse_hor_vel_aux = false; + _fuse_hor_vel = _fuse_hor_vel_aux = _fuse_vert_vel = false; // record the successful position fusion event if (pos_check_pass && _fuse_pos) {