diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index dc3e7f8664..9f984428c2 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -2498,10 +2498,18 @@ void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last)
 // store states in a history array along with time stamp
 void NavEKF::StoreStates()
 {
-    if (storeIndex > 49) storeIndex = 0;
-    for (uint8_t i=0; i<=30; i++) storedStates[i][storeIndex] = states[i];
-    statetimeStamp[storeIndex] = hal.scheduler->millis();
-    storeIndex = storeIndex + 1;
+    // Don't need to store states more often than every 10 msec
+    if (hal.scheduler->millis() - lastStateStoreTime_ms >= 10) {
+        lastStateStoreTime_ms = hal.scheduler->millis();
+        if (storeIndex > 49) {
+            storeIndex = 0;
+        }
+        for (uint8_t i=0; i<=30; i++) {
+            storedStates[i][storeIndex] = states[i];
+        }
+        statetimeStamp[storeIndex] = lastStateStoreTime_ms;
+        storeIndex = storeIndex + 1;
+    }
 }
 
 // reset the stored state history and store the current state
@@ -3080,6 +3088,7 @@ void NavEKF::ZeroVariables()
     velTimeout = false;
     posTimeout = false;
     hgtTimeout = false;
+    lastStateStoreTime_ms = 0;
     lastFixTime_ms = 0;
     secondLastFixTime_ms = 0;
     lastMagUpdate = 0;
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 8a6bde3921..c9117318f6 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -405,6 +405,7 @@ private:
     uint32_t posFailTime;           // time stamp when GPS position measurement last failed covaraiance consistency check (msec)
     uint32_t hgtFailTime;           // time stamp when height measurement last failed covaraiance consistency check (msec)
     uint8_t storeIndex;             // State vector storage index
+    uint32_t lastStateStoreTime_ms; // time of last state vector storage
     uint32_t lastFixTime_ms;        // time of last GPS fix used to determine if new data has arrived
     uint32_t secondLastFixTime_ms;  // time of second last GPS fix used to determine how long since last update
     Vector3f lastAngRate;           // angular rate from previous IMU sample used for trapezoidal integrator