From 7dcea6b2e498835ba6c656eac7370ba554bec9da Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Fri, 30 Aug 2024 17:25:56 +0200 Subject: [PATCH] EKF2: range measurement rejection in rain/fog (#23579) --- .../range_finder/range_height_control.cpp | 1 + .../range_finder/sensor_range_finder.cpp | 21 +++++- .../range_finder/sensor_range_finder.hpp | 12 ++++ src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/ekf.cpp | 1 + src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/params_range_finder.yaml | 13 ++++ .../ekf2/test/test_SensorRangeFinder.cpp | 64 ++++++++++++++++--- 9 files changed, 106 insertions(+), 9 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 0aafe8a1b0..678b6f282c 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 @@ -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); diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 359d10ca52..c0ae2a71eb 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index f3c59be541..fd53407fa8 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -44,6 +44,7 @@ #include "Sensor.hpp" #include +#include 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 _median_dist{}; + float _prev_median_dist{0.f}; }; } // namespace sensor diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 6eda8b9cc7..cc1d48cf16 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/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 diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index c8448daa9b..910eb54dd6 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 97ff93bf2a..38e5660e4f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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)), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 867cf9ae57..489e0bbd5a 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -623,6 +623,7 @@ private: (ParamExtFloat) _param_ekf2_rng_a_igate, (ParamExtFloat) _param_ekf2_rng_qlty_t, (ParamExtFloat) _param_ekf2_rng_k_gate, + (ParamExtFloat) _param_ekf2_rng_fog, (ParamExtFloat) _param_ekf2_rng_pos_x, (ParamExtFloat) _param_ekf2_rng_pos_y, (ParamExtFloat) _param_ekf2_rng_pos_z, diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 5baa4fb267..78c30bbcca 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/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 diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index fc515a8757..c2e9d1c667 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/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; - uint64_t t_now_us = _good_sample.time_us; + rangeSample new_sample = sample; + 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()); + +}