diff --git a/libraries/AP_NavEKF/EKF_Buffer.cpp b/libraries/AP_NavEKF/EKF_Buffer.cpp
index 8474ccecf4255a..bc9032e599a5ac 100644
--- a/libraries/AP_NavEKF/EKF_Buffer.cpp
+++ b/libraries/AP_NavEKF/EKF_Buffer.cpp
@@ -152,13 +152,14 @@ void ekf_imu_buffer::push_youngest_element(const void *element)
         return;
     }
     // push youngest to the buffer
-    _youngest = (_youngest+1) % _size;
+    _youngest = (_youngest+1);
+    if (_youngest == _size) {
+        _youngest = 0;
+        _filled = true;
+    }
     memcpy(get_offset(_youngest), element, elsize);
     // set oldest data index
     _oldest = (_youngest+1) % _size;
-    if (_oldest == 0) {
-        _filled = true;
-    }
 }
 
 // retrieve the oldest data from the ring buffer tail