diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index ca8047640b..c098f51fd2 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -652,8 +652,8 @@ void NavEKF2_core::calcOutputStates()
             float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f);
 
             // use a PI feedback to calculate a correction that will be applied to the output state history
-            posCorrection += posDelta * velPosGain * 0.9f; // I term
-            velCorrection += velDelta * velPosGain * 0.9f; //  I term
+            posCorrection += posDelta * sq(velPosGain) * 0.1f; // I term
+            velCorrection += velDelta * sq(velPosGain) * 0.1f; // I term
             velDelta *= velPosGain; // P term
             posDelta *= velPosGain; // P term