diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 17328b5fa1..3323c72452 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -784,7 +784,8 @@ void NavEKF3_core::readRngBcnData()
 
     // Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
     if (rngBcnDataToFuse) {
-        rngBcnDataDelayed.beacon_posNED += bcnPosOffsetNED;
+        rngBcnDataDelayed.beacon_posNED.x += bcnPosOffsetNED.x;
+        rngBcnDataDelayed.beacon_posNED.y += bcnPosOffsetNED.y;
     }
 
 }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
index 72ef1d5850..39e8665044 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
@@ -531,7 +531,7 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
     float obsDeriv; // derivative of observation relative to state
 
     const float stateNoiseVar = 0.1f; // State process noise variance
-    const float filtAlpha = 0.01f; // LPF constant
+    const float filtAlpha = 0.1f; // LPF constant
     const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std
 
     // estimate upper value for offset
@@ -552,9 +552,6 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
         // Calculate innovation
         innov = sqrtf(t8) - rngBcnDataDelayed.rng;
 
-        // calculate a filtered innovation magnitude to be used to select between the high or low offset
-        OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov);
-
         // covariance prediction
         bcnPosOffsetMaxVar += stateNoiseVar;
 
@@ -562,10 +559,14 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
         innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar;
         innovVar = MAX(innovVar, obsVar);
 
+        // calculate the Kalman gain
+        gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
+
+        float stateChange = innov * gain;
+        OffsetMaxInnovFilt = (1.0f - filtAlpha) * OffsetMaxInnovFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
+
         // Reject range innovation spikes using a 5-sigma threshold unless aligning
         if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
-            // calculate the Kalman gain
-            gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
 
             // state update
             bcnPosDownOffsetMax -= innov * gain;
@@ -589,9 +590,6 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
         // Calculate innovation
         innov = sqrtf(t8) - rngBcnDataDelayed.rng;
 
-        // calculate a filtered innovation magnitude to be used to select between the high or low offset
-        OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov);
-
         // covariance prediction
         bcnPosOffsetMinVar += stateNoiseVar;
 
@@ -599,13 +597,18 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
         innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar;
         innovVar = MAX(innovVar, obsVar);
 
+        // calculate the Kalman gain
+        gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
+
+        // calculate a filtered state change magnitude to be used to select between the high or low offset
+        float stateChange = innov * gain;
+        OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + fminf(fabsf(filtAlpha * stateChange) , 1.0f);
+
         // Reject range innovation spikes using a 5-sigma threshold unless aligning
         if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
-            // calculate the Kalman gain
-            gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
 
             // state update
-            bcnPosDownOffsetMin -= innov * gain;
+            bcnPosDownOffsetMin -= stateChange;
 
             // covariance update
             bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar;
@@ -620,16 +623,21 @@ void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
     bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
     bcnPosDownOffsetMin  = MIN(bcnPosDownOffsetMin,  vehiclePosNED.z - bcnMidPosD - 0.5f);
 
-    // calculate the innovation for the main filter using the offset with the smallest innovation history
+    // calculate the innovation for the main filter using the offset that is most stable
     // apply hysteresis to prevent rapid switching
-    if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) {
-        bcnPosOffsetNED.z = bcnPosDownOffsetMin;
+    if (!usingMinHypothesis && (OffsetMinInnovFilt < (0.8f * OffsetMaxInnovFilt))) {
         usingMinHypothesis = true;
-    } else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) {
-        bcnPosOffsetNED.z = bcnPosDownOffsetMax;
+    } else if (usingMinHypothesis && (OffsetMaxInnovFilt < (0.8f * OffsetMinInnovFilt))) {
         usingMinHypothesis = false;
     }
+    if (usingMinHypothesis) {
+        bcnPosOffsetNED.z = bcnPosDownOffsetMin;
+    } else {
+        bcnPosOffsetNED.z = bcnPosDownOffsetMax;
+    }
 
+    // apply the vertical offset to the beacon positions
+    rngBcnDataDelayed.beacon_posNED.z += bcnPosOffsetNED.z;
 }
 
 #endif // HAL_CPU_CLASS