From 17c195d1cdb22f8428bfe4b27c2825a804838474 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 17 Oct 2016 22:42:10 +0200 Subject: [PATCH] ekf2 height reset: do not apply height change to height state twice (bug) Signed-off-by: Roman --- EKF/ekf_helper.cpp | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 1651e1fb8b..83b506ab9a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -271,9 +271,15 @@ void Ekf::resetHeight() _state_reset_status.velD_counter++; } - // add the reset amount to the output observer states - _output_new.pos(2) += _state_reset_status.posD_change; - _output_new.vel(2) += _state_reset_status.velD_change; + // apply the change in height / height rate to our newest height / height rate estimate + // which have already been taken out from the output buffer + if (vert_pos_reset) { + _output_new.pos(2) += _state_reset_status.posD_change; + } + + if (vert_vel_reset) { + _output_new.vel(2) += _state_reset_status.velD_change; + } // add the reset amount to the output observer buffered data outputSample output_states; @@ -292,17 +298,6 @@ void Ekf::resetHeight() _output_buffer.push_to_index(i,output_states); } - - // apply the change in height / height rate to our newest height / height rate estimate - // which have already been taken out from the output buffer - if (vert_pos_reset) { - _output_new.pos(2) += _state_reset_status.posD_change; - } - - if (vert_vel_reset) { - _output_new.vel(2) += _state_reset_status.velD_change; - } - } // align output filter states to match EKF states at the fusion time horizon