EKF2: range measurement rejection in rain/fog (#23579)

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -54,6 +54,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+		_range_sensor.setMaxFogDistance(_params.rng_fog);
 
 		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp

@@ -84,8 +84,9 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 
 			if (isTiltOk() && isDataInRange()) {
 				updateStuckCheck();
+				updateFogCheck(getDistBottom(), _sample.time_us);
 
-				if (!_is_stuck) {
+				if (!_is_stuck && !_is_blocked) {
 					_is_sample_valid = true;
 					_time_last_valid_us = _sample.time_us;
 				}
@@ -146,5 +147,23 @@ void SensorRangeFinder::updateStuckCheck()
 	}
 }
 
+void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
+{
+	if (_max_fog_dist > 0.f && time_us - _time_last_valid_us < 1e6) {
+
+		const float median_dist = _median_dist.apply(dist_bottom);
+		const float factor = 2.f; // magic hardcoded factor
+
+		if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) {
+			_is_blocked = true;
+
+		} else if (_is_blocked && median_dist > factor * _max_fog_dist) {
+			_is_blocked = false;
+		}
+
+		_prev_median_dist = median_dist;
+	}
+}
+
 } // namespace sensor
 } // namespace estimator

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -44,6 +44,7 @@
 #include "Sensor.hpp"
 
 #include <matrix/math.hpp>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
 
 namespace estimator
 {
@@ -99,6 +100,8 @@ public:
 		_rng_valid_max_val = max_distance;
 	}
 
+	void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; }
+
 	void setQualityHysteresis(float valid_quality_threshold_s)
 	{
 		_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
@@ -129,6 +132,7 @@ private:
 	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
 	bool isDataInRange() const;
 	void updateStuckCheck();
+	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
 
 	rangeSample _sample{};
 
@@ -172,6 +176,14 @@ private:
 	 */
 	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
 	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
+
+	/*
+	 * Fog check
+	 */
+	bool _is_blocked{false};
+	float _max_fog_dist{0.f};	//< maximum distance at which the range finder could detect fog (m)
+	math::MedianFilter<float, 5> _median_dist{};
+	float _prev_median_dist{0.f};
 };
 
 } // namespace sensor

src/modules/ekf2/EKF/common.h

@@ -421,6 +421,7 @@ struct parameters {
 	float range_valid_quality_s{1.0f};      ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
+	float rng_fog{0.f};                 	///< max distance which a blocked range sensor measures (fog, dirt) [m]
 
 	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
 #endif // CONFIG_EKF2_RANGE_FINDER

src/modules/ekf2/EKF/ekf.cpp

@@ -81,6 +81,7 @@ void Ekf::reset()
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+	_range_sensor.setMaxFogDistance(_params.rng_fog);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 	_control_status.value = 0;

src/modules/ekf2/EKF2.cpp

@@ -162,6 +162,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
 	_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
 	_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
+	_param_ekf2_rng_fog(_params->rng_fog),
 	_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
 	_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
 	_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),

src/modules/ekf2/EKF2.hpp

@@ -623,6 +623,7 @@ private:
 		(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>) _param_ekf2_rng_a_igate,
 		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
 		(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
+		(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z,

src/modules/ekf2/params_range_finder.yaml

@@ -150,3 +150,16 @@ parameters:
       default: 0.0
       unit: m
       decimal: 3
+    EKF2_RNG_FOG:
+      description:
+        short: Maximum distance at which the range finder could detect fog (m)
+        long: Limit for fog detection. If the range finder measures a distance greater
+          than this value, the measurement is considered to not be blocked by fog or rain.
+          If there's a jump from larger than RNG_FOG to smaller than EKF2_RNG_FOG, the
+          measurement may gets rejected. 0 - disabled
+      type: float
+      default: 1.0
+      min: 0.0
+      max: 20.0
+      unit: m
+      decimal: 1

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -51,6 +51,7 @@ public:
 		_range_finder.setPitchOffset(0.f);
 		_range_finder.setCosMaxTilt(0.707f);
 		_range_finder.setLimits(_min_range, _max_range);
+		_range_finder.setMaxFogDistance(2.f);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -60,20 +61,21 @@ public:
 
 protected:
 	SensorRangeFinder _range_finder{};
-	const rangeSample _good_sample{(uint64_t)2e6, 1.f, 100}; // {time_us, range, quality}
+	const rangeSample _good_sample{(uint64_t)2e6, 5.f, 100}; // {time_us, range, quality}
 	const float _min_range{0.5f};
 	const float _max_range{10.f};
 
-	void updateSensorAtRate(uint64_t duration_us, uint64_t dt_update_us, uint64_t dt_sensor_us);
+	void updateSensorAtRate(rangeSample sample, uint64_t duration_us, uint64_t dt_update_us, uint64_t dt_sensor_us);
 	void testTilt(const Eulerf &euler, bool should_pass);
 };
 
-void SensorRangeFinderTest::updateSensorAtRate(uint64_t duration_us, uint64_t dt_update_us, uint64_t dt_sensor_us)
+void SensorRangeFinderTest::updateSensorAtRate(rangeSample sample, uint64_t duration_us, uint64_t dt_update_us,
+		uint64_t dt_sensor_us)
 {
 	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
 
-	rangeSample new_sample = _good_sample;
+	rangeSample new_sample = sample;
-	uint64_t t_now_us = _good_sample.time_us;
+	uint64_t t_now_us = sample.time_us;
 
 	for (int i = 0; i < int(duration_us / dt_update_us); i++) {
 		t_now_us += dt_update_us;
@@ -307,7 +309,7 @@ TEST_F(SensorRangeFinderTest, continuity)
 	const uint64_t dt_update_us = 10e3;
 	uint64_t dt_sensor_us = 4e6;
 	uint64_t duration_us = 8e6;
-	updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us);
+	updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us);
 
 	// THEN: the data should be marked as unhealthy
 	// Note that it also fails the out-of-date test here
@@ -317,14 +319,14 @@ TEST_F(SensorRangeFinderTest, continuity)
 	// AND WHEN: the data rate is acceptable
 	dt_sensor_us = 3e5;
 	duration_us = 5e5;
-	updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us);
+	updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us);
 
 	// THEN: it should still fail until the filter converge
 	// to the new datarate
 	EXPECT_FALSE(_range_finder.isDataHealthy());
 	EXPECT_FALSE(_range_finder.isHealthy());
 
-	updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us);
+	updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us);
 	EXPECT_TRUE(_range_finder.isDataHealthy());
 	EXPECT_TRUE(_range_finder.isHealthy());
 }
@@ -345,3 +347,49 @@ TEST_F(SensorRangeFinderTest, distBottom)
 	_range_finder.runChecks(sample.time_us, attitude20);
 	EXPECT_FLOAT_EQ(_range_finder.getDistBottom(), sample.rng * cosf(-0.35));
 }
+
+TEST_F(SensorRangeFinderTest, blockedByFog)
+{
+	// WHEN: sensor is not blocked by fog
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+	const uint64_t dt_update_us = 10e3;
+	uint64_t dt_sensor_us = 3e5;
+	uint64_t duration_us = 5e5;
+
+	updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us);
+	// THEN: the data should be marked as healthy
+	EXPECT_TRUE(_range_finder.isDataHealthy());
+	EXPECT_TRUE(_range_finder.isHealthy());
+
+
+	// WHEN: sensor is then blocked by fog
+	// range jumps to value below 2m
+	uint64_t t_now_us = _range_finder.getSampleAddress()->time_us;
+	rangeSample sample{t_now_us, 1.f, 100};
+	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
+
+	// THEN: the data should be marked as unhealthy
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+
+	// WHEN: the sensor is not blocked by fog anymore
+	sample.rng = 5.f;
+	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
+
+	// THEN: the data should be marked as healthy again
+	EXPECT_TRUE(_range_finder.isDataHealthy());
+	EXPECT_TRUE(_range_finder.isHealthy());
+
+	// WHEN: the sensor is is not jumping to a value below 2m
+	while (sample.rng > _min_range) {
+		sample.time_us += dt_update_us;
+		_range_finder.setSample(sample);
+		_range_finder.runChecks(sample.time_us, attitude);
+		sample.rng -= 0.5f;
+	}
+
+	// THEN: the data should still be marked as healthy
+	EXPECT_TRUE(_range_finder.isDataHealthy());
+	EXPECT_TRUE(_range_finder.isHealthy());
+
+}