diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index b2e18e4515..f4c1f13828 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -655,8 +655,10 @@ void NavEKF2_core::selectHeightForFusion()
     // co-located with the IMU
     if (rangeDataToFuse) {
         Vector3f posOffsetBody = rangeDataDelayed.body_offset - accelPosOffset;
-        Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
-        rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
+        if (!posOffsetBody.is_zero()) {
+            Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+            rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
+        }
     }
 
     // read baro height data from the sensor and check for new data in the buffer