From 03f3df741589627bf15225327289b01916cc1a90 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 14 May 2016 08:33:05 +0930 Subject: [PATCH] EKF: Fix bug in output observer Bug was causing output observer state history to be stored at the the EKF, not IMU output rate and resulted in degraded tracking performance --- EKF/ekf.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index e660014498..f8ea0f5e30 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -674,10 +674,7 @@ 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 - if (_imu_updated) { - _output_buffer.push(_output_new); - _imu_updated = false; - } + _output_buffer.push(_output_new); // get the oldest INS state data from the ring buffer // this data will be at the EKF fusion time horizon