From 3780f4c3d6728d82a54d5a1dd17eee63125c6218 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Tue, 21 Nov 2023 13:37:04 +0100 Subject: [PATCH 1/2] ekf2-terrain: fix validity switching Bug not present after 1.14 --- src/modules/ekf2/EKF/terrain_estimator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 657b989f4882..f368e1f615c9 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -199,7 +199,6 @@ void Ekf::resetHaglRng() _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; _time_last_hagl_fuse = _time_delayed_us; - _time_last_healthy_rng_data = 0; } void Ekf::stopHaglRngFusion() From ef8af1038d3867e950072884ae0760b61501e0ab Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Thu, 21 Sep 2023 17:40:36 +0200 Subject: [PATCH 2/2] ekf2 rng kin: allow check to become true during horizontal motion Even if there is some horizontal motion, a passing check should be accepted as the terrain can be flat. However, the vehicle must not be moving horizontally to invalidate the consistency as a change in terrain can make the kinematic check temporarily fail. --- .../EKF/range_finder_consistency_check.cpp | 20 +++++++++++++++---- .../EKF/range_finder_consistency_check.hpp | 3 ++- src/modules/ekf2/EKF/range_height_control.cpp | 8 ++++---- 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp index 0d8daee7dea9..453da560b421 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -37,8 +37,12 @@ #include "range_finder_consistency_check.hpp" -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us) +void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us) { + if (horizontal_motion) { + _time_last_horizontal_motion = time_us; + } + const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; if ((_time_last_update_us == 0) @@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { + if (fabsf(vz) < _min_vz_for_valid_consistency) { + // We can only check consistency if there is vertical motion + return; + } + if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { - _is_kinematically_consistent = false; - _time_last_inconsistent_us = time_us; + if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { + _is_kinematically_consistent = false; + _time_last_inconsistent_us = time_us; + } } else { - if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { + if (_test_ratio < 1.f + && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { _is_kinematically_consistent = true; } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp index 1498a4aa6cce..fa185f662955 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -47,7 +47,7 @@ class RangeFinderConsistencyCheck final RangeFinderConsistencyCheck() = default; ~RangeFinderConsistencyCheck() = default; - void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us); + void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us); void setGate(float gate) { _gate = gate; } @@ -71,6 +71,7 @@ class RangeFinderConsistencyCheck final bool _is_kinematically_consistent{true}; uint64_t _time_last_inconsistent_us{}; + uint64_t _time_last_horizontal_motion{}; static constexpr float _signed_test_ratio_tau = 2.f; diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 88a9cb719711..fa2a365f356a 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion() const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - // Run the kinematic consistency check when not moving horizontally - if (_control_status.flags.in_air && !_control_status.flags.fixed_wing - && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + if (_control_status.flags.in_air) { + const bool horizontal_motion = _control_status.flags.fixed_wing + || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P(4, 4) + P(5, 5), 0.1f)); const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float var = sq(_params.range_noise) + dist_dependant_var; _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), horizontal_motion, _time_delayed_us); } } else {