diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
index 18994cda5b..8f7606d87a 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
@@ -307,8 +307,10 @@ void NavEKF2_core::FuseOptFlow()
         // the corrected value is the predicted range from the sensor focal point to the
         // centre of the image on the ground assuming flat terrain
         Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
-        Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
-        range -= posOffsetEarth.z / prevTnb.c.z;
+        if (!posOffsetBody.is_zero()) {
+            Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+            range -= posOffsetEarth.z / prevTnb.c.z;
+        }
 
         // calculate relative velocity in sensor frame including the relative motion due to rotation
         relVelSensor = prevTnb*stateStruct.velocity + ofDataDelayed.bodyRadXYZ % posOffsetBody;