diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
index 5f0dabef2d..1d51a94509 100644
--- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
@@ -100,9 +100,33 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 	if (rng_data_ready && _range_sensor.getSampleAddress()) {
 
-		updateRangeHagl(aid_src);
+		const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
+		const float measurement_variance = getRngVar();
+
+		float innovation_variance;
+		sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
+
+		const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
+		updateAidSourceStatus(aid_src,
+				      _range_sensor.getSampleAddress()->time_us, // sample timestamp
+				      measurement,                               // observation
+				      measurement_variance,                      // observation variance
+				      getHagl() - measurement,                   // innovation
+				      innovation_variance,                       // innovation variance
+				      innov_gate);                               // innovation gate
+
 		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
 
+		// z special case if there is bad vertical acceleration data, then don't reject measurement,
+		// but limit innovation to prevent spikes that could destabilise the filter
+		if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected
+		    && measurement_valid && _range_sensor.isDataHealthy()
+		   ) {
+			const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+			aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+			aid_src.innovation_rejected = false;
+		}
+
 		const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
 				|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
 				&& _control_status.flags.tilt_align
@@ -247,32 +271,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 	}
 }
 
-void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
-{
-	const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
-	const float measurement_variance = getRngVar();
-
-	float innovation_variance;
-	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
-
-	const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
-	updateAidSourceStatus(aid_src,
-			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
-			      measurement,                               // observation
-			      measurement_variance,                      // observation variance
-			      getHagl() - measurement,                   // innovation
-			      innovation_variance,                       // innovation variance
-			      innov_gate);                               // innovation gate
-
-	// z special case if there is bad vertical acceleration data, then don't reject measurement,
-	// but limit innovation to prevent spikes that could destabilise the filter
-	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
-		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
-		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
-		aid_src.innovation_rejected = false;
-	}
-}
-
 float Ekf::getRngVar() const
 {
 	return fmaxf(
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index bbabed8bc0..3c5f7ba3f5 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -765,7 +765,6 @@ private:
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
-	void updateRangeHagl(estimator_aid_source1d_s &aid_src);
 	void resetTerrainToRng(estimator_aid_source1d_s &aid_src);
 	float getRngVar() const;
 # endif // CONFIG_EKF2_RANGE_FINDER