From e3309b9f87970b61164a46126659b2899fa06242 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 30 Sep 2025 15:14:04 -0400
Subject: [PATCH] ekf2: rng don't allow bad measurement in bad_acc_vertical
 (#25636)

- if bad vertical acceleration is detected there's an emergency case
   where rejected range finder observations are allowed to be used, but
   this still can't happen if the sample itself is known to be bag
---
 .../range_finder/range_height_control.cpp     | 52 +++++++++----------
 src/modules/ekf2/EKF/ekf.h                    |  1 -
 2 files changed, 25 insertions(+), 28 deletions(-)

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