From 0605c88eccae178f9ba4999baa5c8eabde73712f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:25:20 +1000 Subject: [PATCH] EKF: capture horizontal position reset events --- EKF/ekf_helper.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0f344573ed..7ba3e2ada2 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -148,7 +148,7 @@ bool Ekf::resetPosition() } // calculate the change in position and apply to the output predictor state history - Vector3f posNE_change; + Vector2f posNE_change; posNE_change(0) = _state.pos(0) - posNE_before_reset(0); posNE_change(1) = _state.pos(1) - posNE_before_reset(1); outputSample output_states; @@ -160,6 +160,10 @@ bool Ekf::resetPosition() _output_buffer.push_to_index(index,output_states); } + // capture the reset event + _state_reset_status.posNE_change = posNE_change; + _state_reset_status.posNE_time_us = _imu_sample_delayed.time_us; + } // Reset height state using the last height measurement