From 7cfea9915b82009fbd68f47f16489010fa7d1deb Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Tue, 4 Mar 2025 09:46:26 +0100 Subject: [PATCH] change consistency to only be valid if running for a second --- .../range_finder/range_finder_consistency_check.cpp | 10 +++------- .../range_finder/range_finder_consistency_check.hpp | 5 +++-- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index eb04810e24..22f4b272b1 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -50,7 +50,6 @@ void RangeFinderConsistencyCheck::init(const float z, const float z_var, const f _x(RangeFilter::z.idx) = z; _x(RangeFilter::terrain.idx) = z - dist_bottom; _initialized = true; - _state = KinematicState::UNKNOWN; _test_ratio_lpf.reset(0.f); _t_since_first_sample = 0.f; } @@ -92,8 +91,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const R = z_var; } else if (measurement_idx == 1) { - H(RangeFilter::z.idx) = _Ht(0); - H(RangeFilter::terrain.idx) = _Ht(1); + H = _Ht; R = dist_bottom_var; } @@ -137,10 +135,8 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var) { // start the consistency check after 1s - if (_t_since_first_sample + dt > 1.0f) { - _t_since_first_sample = 2.0f; - - if (abs(_test_ratio_lpf.getState()) < 1.f) { + if (_t_since_first_sample > _t_to_init) { + if (fabsf(_test_ratio_lpf.getState()) < 1.f) { const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f); if (!horizontal_motion && vertical_motion) { diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index a8334c9e87..0c967bbf0b 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -58,8 +58,8 @@ public: float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; } float getInnov() const { return _initialized ? _innov : 0.f; } float getInnovVar() const { return _initialized ? _innov_var : 0.f; } - bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } - bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; } + bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; } + bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; } void setGate(const float gate) { _gate = gate; } void run(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, uint64_t time_us); @@ -93,6 +93,7 @@ private: KinematicState _state{KinematicState::UNKNOWN}; float _t_since_first_sample{0.f}; uint8_t _last_posD_reset_count{0}; + static constexpr float _t_to_init{1.f}; }; namespace RangeFilter