From 0058953e4fefe13ee7391cbc952a93d9d0bcc48c Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 3 Oct 2024 10:32:31 +0200 Subject: [PATCH] handle noise while hovering --- msg/EstimatorStatusFlags.msg | 1 + .../range_finder_consistency_check.cpp | 9 +++- .../range_finder_consistency_check.hpp | 44 +++++++++++++++-- .../range_finder/range_height_control.cpp | 47 +++++++++++++++---- src/modules/ekf2/EKF/common.h | 2 + src/modules/ekf2/EKF2.cpp | 1 + 6 files changed, 89 insertions(+), 15 deletions(-) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 8ab5b3d3e4..8599b00277 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -49,6 +49,7 @@ bool cs_valid_fake_pos # 41 - true if a valid constant position is bein bool cs_constant_pos # 42 - true if the vehicle is at a constant position bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended +bool cs_rng_kin_unknown # 43 - true when the range finder kinematic consistency check is not running # fault status uint32 fault_status_changes # number of filter fault status (fs) changes 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 fed9170a7a..cbdc231790 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 @@ -51,6 +51,7 @@ void RangeFinderConsistencyCheck::initMiniKF(float p1, float p2, float x1, float _x(1) = x2; _initialized = true; _sample_count = 0; + _state = KinematicState::UNKNOWN; } void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, @@ -90,8 +91,12 @@ void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, f _innov = y(1); _innov_var = S(1, 1); - if (_sample_count++ > 10) { - _is_kinematically_consistent = _test_ratio_lpf.update(test_ratio) < 1.f; + if (_sample_count++ > _min_nr_of_samples) { + if (_test_ratio_lpf.update(test_ratio) < 1.f) { + _state = KinematicState::CONSISTENT; + } else { + _state = KinematicState::INCONSISTENT; + } } else { _test_ratio_lpf.update(test_ratio); 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 3036f4ed2b..a67b9bc232 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 @@ -48,17 +48,53 @@ public: RangeFinderConsistencyCheck() = default; ~RangeFinderConsistencyCheck() = default; + enum class KinematicState : int { + INCONSISTENT = 0, + CONSISTENT = 1, + UNKNOWN = 2 + }; + float getTestRatioLpf() const { return _test_ratio_lpf.getState(); } float getInnov() const { return _innov; } float getInnovVar() const { return _innov_var; } - bool isKinematicallyConsistent() const { return _is_kinematically_consistent; } + bool isKinematicallyConsistent() const + { + if (_fixed_wing) { + return _state != KinematicState::INCONSISTENT; + } + + return _state == KinematicState::CONSISTENT; + } + bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; } void UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, uint64_t time_us); void initMiniKF(float p1, float p2, float x1, float x2); - void stopMiniKF() { _initialized = false; } + void stopMiniKF() + { + _initialized = false; + + if (_state == KinematicState::CONSISTENT) { + _state = KinematicState::UNKNOWN; + } + } + int getConsistencyState() const { return static_cast(_state); } + bool isRunning() { return _initialized; } + void setNotMoving() + { + if (_state == KinematicState::CONSISTENT) { + _state = KinematicState::UNKNOWN; + } + } + + void setFixedWing(bool is_fixed_wing, float gate) + { + _fixed_wing = is_fixed_wing; + _gate = gate; + } + matrix::SquareMatrix _R{}; matrix::SquareMatrix _P{}; matrix::SquareMatrix _A{}; @@ -73,8 +109,10 @@ private: float _dist_bottom_prev{}; AlphaFilter _test_ratio_lpf{0.3}; // average signed test ratio used to detect a bias in the data float _gate{1.f}; - bool _is_kinematically_consistent{true}; int _sample_count{0}; + KinematicState _state{KinematicState::UNKNOWN}; + const int _min_nr_of_samples{10}; // hardcoded + bool _fixed_wing{false}; }; #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP 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 9b19e1698d..4005629361 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 @@ -73,10 +73,9 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float dist_var = sq(_params.range_noise) + dist_dependant_var; - if (horizontal_motion) { - _rng_consistency_check.stopMiniKF(); + if (_control_status.flags.fixed_wing) { + _rng_consistency_check.setFixedWing(true, 2.0f * _params.range_kin_consistency_gate); - } else if (vertical_motion) { if (!_rng_consistency_check.isRunning()) { _rng_consistency_check.initMiniKF(P(State::pos.idx + 2, State::pos.idx + 2), P(State::terrain.idx, State::terrain.idx), _state.pos(2), _state.terrain); @@ -84,8 +83,32 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), _range_sensor.getRange(), dist_var, imu_sample.time_us); - } + } else { + _rng_consistency_check.setFixedWing(false, _params.range_kin_consistency_gate); + + if (horizontal_motion) { + // if state 0: keep state 0 + // if state 1: set to state 2 + // if state 2: keep state 2 + if (_rng_consistency_check.isRunning()) { + _rng_consistency_check.stopMiniKF(); + } + + } else if (vertical_motion) { + if (!_rng_consistency_check.isRunning()) { + _rng_consistency_check.initMiniKF(P(State::pos.idx + 2, State::pos.idx + 2), P(State::terrain.idx, State::terrain.idx), + _state.pos(2), _state.terrain); + } + + _rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2), + P(State::vel.idx + 2, State::vel.idx + 2), _range_sensor.getRange(), dist_var, imu_sample.time_us); + + } else { + _rng_consistency_check.setNotMoving(); + + } + } } } else { @@ -100,7 +123,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } - _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + _control_status.flags.rng_kin_consistent = _rng_consistency_check.getConsistencyState() == 1; + _control_status.flags.rng_kin_unknown = _rng_consistency_check.getConsistencyState() == 2; } else { return; @@ -118,11 +142,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && _control_status.flags.tilt_align && measurement_valid && _range_sensor.isDataHealthy() - && _rng_consistency_check.isKinematicallyConsistent(); + && _rng_consistency_check.isNotKinematicallyInconsistent(); const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) - && _range_sensor.isRegularlySendingData(); + && _range_sensor.isRegularlySendingData() + && _rng_consistency_check.isKinematicallyConsistent(); const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) @@ -148,7 +173,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _control_status.flags.rng_hgt = true; stopRngTerrFusion(); - if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected + && _rng_consistency_check.isKinematicallyConsistent()) { resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } @@ -174,7 +200,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) ECL_INFO("starting %s height fusion", HGT_SRC_NAME); _control_status.flags.rng_hgt = true; - if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected + && _rng_consistency_check.isKinematicallyConsistent()) { resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } @@ -211,7 +238,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) stopRngHgtFusion(); stopRngTerrFusion(); - } else { + } else if (_rng_consistency_check.isKinematicallyConsistent()) { resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index de4eb07ad6..828c01bbb0 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -608,6 +608,8 @@ uint64_t mag_heading_consistent : uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended uint64_t gnss_fault : 1; ///< 45 - true if GNSS measurements have been declared faulty and are no longer used + uint64_t rng_kin_unknown : 1; ///< 46 - true when the range finder kinematic consistency check is not running + } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 40b642c9b0..9fd6368edd 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1935,6 +1935,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault; status_flags.cs_gnss_vel = _ekf.control_status_flags().gnss_vel; + status_flags.cs_rng_kin_unknown = _ekf.control_status_flags().rng_kin_unknown; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;