From 064518f57aab08a90d11c18773cdb6c8ed86862a Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 15 Mar 2022 17:08:29 +0100
Subject: [PATCH] ekf: extract range finder noise computation

---
 src/modules/ekf2/EKF/ekf.h          | 3 ++-
 src/modules/ekf2/EKF/ekf_helper.cpp | 8 ++++++++
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 03e807f661..fcbcdef301 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -930,8 +930,9 @@ private:
 
 	void updateGroundEffect();
 
-	// return an estimation of the GPS altitude variance
+	// return an estimation of the sensor altitude variance
 	float getGpsHeightVariance();
+	float getRngHeightVariance() const;
 
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar();
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 8830804d5f..2725d72b31 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -1361,6 +1361,14 @@ void Ekf::updateBaroHgtBias()
 	}
 }
 
+float Ekf::getRngHeightVariance() const
+{
+	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+	const float var = sq(_params.range_noise) + dist_dependant_var;
+	const float var_sat = fmaxf(var, 0.01f);
+	return var_sat;
+}
+
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {