From 78211f9dbb4fc46cc0bc5cd3ce3c7651d8dfcd14 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 16 Mar 2022 15:50:21 +0100 Subject: [PATCH] ekf: improve rng consistency check To pass from invalid to valid: - time hysteresis - some vertical velocity - test ratio < 1 - low-passed signed test ratio < 1 To pass from valid to invalid: - low-passed signed test ratio >= 1 --- .../ekf2/EKF/range_finder_consistency_check.cpp | 15 +++++++++++++++ .../ekf2/EKF/range_finder_consistency_check.hpp | 10 +++++++++- 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp index 27bb61ca8c..00bb5d5801 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -59,6 +59,21 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va const float signed_test_ratio = matrix::sign(innov) * _vel_bottom_test_ratio; _vel_bottom_signed_test_ratio_lpf.update(signed_test_ratio); + updateConsistency(vz, time_s); + _time_last_update_s = time_s; _dist_bottom_prev = dist_bottom; } + +void RangeFinderConsistencyCheck::updateConsistency(float vz, float time_s) +{ + if (fabsf(_vel_bottom_signed_test_ratio_lpf.getState()) >= 1.f) { + _is_kinematically_consistent = false; + _time_last_inconsistent = time_s; + + } else { + if (fabsf(vz) > _min_vz_for_valid_consistency && _vel_bottom_test_ratio < 1.f && ((time_s - _time_last_inconsistent) > _consistency_hyst_time)) { + _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 860349a29f..d85aacd00f 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -51,15 +51,23 @@ public: float getTestRatio() const { return _vel_bottom_test_ratio; } float getSignedTestRatioLpf() const { return _vel_bottom_signed_test_ratio_lpf.getState(); } - bool isKinematicallyConsistent() const { return _vel_bottom_signed_test_ratio_lpf.getState() < 1.f; } + bool isKinematicallyConsistent() const { return _is_kinematically_consistent; } private: + void updateConsistency(float vz, float time_s); + float _time_last_update_s{}; float _dist_bottom_prev{}; float _vel_bottom_test_ratio{}; AlphaFilter _vel_bottom_signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data + bool _is_kinematically_consistent{true}; + float _time_last_inconsistent{}; + static constexpr float _vel_bottom_signed_test_ratio_tau = 2.f; static constexpr float _vel_bottom_gate = 0.1f; + + static constexpr float _min_vz_for_valid_consistency = 0.5f; + static constexpr float _consistency_hyst_time = 1.f; };