diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index 0b319761b3..79e54a54ab 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -60,8 +60,8 @@ void NavEKF2_core::readRangeFinder(void)
                     minIndex = 1;
                     maxIndex = 0;
                 } else {
-                    maxIndex = 0;
-                    minIndex = 1;
+                    minIndex = 0;
+                    maxIndex = 1;
                 }
                 if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
                     midIndex = maxIndex;