diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index de7cf6e3fc..4c2f4e87f9 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -2535,7 +2535,9 @@ void NavEKF::RunAuxiliaryEKF()
             return;
         }
         distanceTravelledSq = min(distanceTravelledSq, 100.0f);
-        Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma)));
+        float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
+        Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))) + sq(1.0f * timeLapsed);
+        timeAtLastAuxEKF_ms = imuSampleTime_ms;
     }
 
     // fuse range finder data
@@ -4202,7 +4204,7 @@ void NavEKF::ZeroVariables()
     lastFixTime_ms = imuSampleTime_ms;
     secondLastFixTime_ms = imuSampleTime_ms;
     lastDecayTime_ms = imuSampleTime_ms;
-    airborneDetectTime_ms = imuSampleTime_ms;
+    timeAtLastAuxEKF_ms = imuSampleTime_ms;
 
     gpsNoiseScaler = 1.0f;
     velTimeout = false;
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 7aa9586891..0a03377615 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -503,6 +503,7 @@ private:
     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 timeAtLastAuxEKF_ms;   // last time the auxilliary filter was run to fuse range or optical flow measurements
     uint32_t secondLastFixTime_ms;  // time of second last GPS fix used to determine how long since last update
     uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
     Vector3f lastAngRate;           // angular rate from previous IMU sample used for trapezoidal integrator