diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 62f4e1cbbc..12a1ffd177 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -160,8 +160,6 @@ bool Ekf::init(uint64_t timestamp) bool Ekf::update() { - // Only run the filter if IMU data in the buffer has been updated - if (_imu_updated) { if (!_filter_initialised) { _filter_initialised = initialiseFilter(); @@ -170,6 +168,9 @@ bool Ekf::update() } } + // Only run the filter if IMU data in the buffer has been updated + if (_imu_updated) { + // perform state and covariance prediction for the main filter predictState(); predictCovariance();