diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 241335965b39aa7581435a73cbe389d5ef64cb95..5d3e0c5f48b29c59b6401b4510606003e062daf2 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3520,18 +3520,19 @@ void NavEKF::getAccelNED(Vector3f &accelNED) const {
 void NavEKF::getVelNED(Vector3f &vel) const
 {
     vel = state.velocity;
-    if (constPosMode) {
-        vel.x = lastKnownPositionNE.x;
-        vel.y = lastKnownPositionNE.y;
-    }
 }
 
 // return the last calculated NED position relative to the reference point (m).
 // return false if no position is available
 bool NavEKF::getPosNED(Vector3f &pos) const
 {
-    pos.x = state.position.x;
-    pos.y = state.position.y;
+    if (constPosMode) {
+        pos.x = lastKnownPositionNE.x;
+        pos.y = lastKnownPositionNE.y;
+    } else {
+        pos.x = state.position.x;
+        pos.y = state.position.y;
+    }
     // If relying on optical flow, then output ground relative position so that the vehicle does terain following
     if (_fusionModeGPS == 3) {
         pos.z = state.position.z - terrainState;