From 627d08ecc99eeb858d4865b7ecdf6cd7843add28 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 14 May 2016 12:26:46 +0930 Subject: [PATCH] Revert "EKF: Fix bug in output observer" This reverts commit 03f3df741589627bf15225327289b01916cc1a90. --- EKF/ekf.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f8ea0f5e30..e660014498 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -674,7 +674,10 @@ void Ekf::calculateOutputStates() _output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt; // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer - _output_buffer.push(_output_new); + if (_imu_updated) { + _output_buffer.push(_output_new); + _imu_updated = false; + } // get the oldest INS state data from the ring buffer // this data will be at the EKF fusion time horizon