diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 7f1f93d159..ac87d7a42b 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3576,7 +3576,14 @@ void NavEKF::getVelNED(Vector3f &vel) const
 // return false if no position is available
 bool NavEKF::getPosNED(Vector3f &pos) const
 {
-    pos = state.position;
+    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 - flowStates[1];
+    } else {
+        pos.z = state.position.z;
+    }
     return true;
 }