From 4e40a86971ee2c0077e73f91e65e738917563bab Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Fri, 4 Oct 2024 11:02:01 +0200 Subject: [PATCH] fix wrong miniEKF init --- .../range_finder_consistency_check.hpp | 13 +++---------- .../range_finder/range_height_control.cpp | 16 +++++----------- 2 files changed, 8 insertions(+), 21 deletions(-) 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 a67b9bc232..d25d509b3e 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,15 +58,8 @@ public: float getInnov() const { return _innov; } float getInnovVar() const { return _innov_var; } - bool isKinematicallyConsistent() const - { - if (_fixed_wing) { - return _state != KinematicState::INCONSISTENT; - } - - return _state == KinematicState::CONSISTENT; - } - bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; } + bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } + bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; } 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); @@ -107,7 +100,7 @@ private: float _innov_var{}; uint64_t _time_last_update_us{0}; float _dist_bottom_prev{}; - AlphaFilter _test_ratio_lpf{0.3}; // average signed test ratio used to detect a bias in the data + AlphaFilter _test_ratio_lpf{0.3}; float _gate{1.f}; int _sample_count{0}; KinematicState _state{KinematicState::UNKNOWN}; 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 4005629361..8f8ab3d4bb 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 @@ -66,9 +66,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); 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.trace<2>(State::vel.idx), 0.1f)); - const bool vertical_motion = sq(_state.vel(2)) > fmaxf(P(State::vel.idx + 2, State::vel.idx + 2), 0.1f); const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float dist_var = sq(_params.range_noise) + dist_dependant_var; @@ -78,7 +75,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) 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); + _state.pos(2), _state.pos(2) + _range_sensor.getRange()); } _rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2), @@ -86,11 +83,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { _rng_consistency_check.setFixedWing(false, _params.range_kin_consistency_gate); + const bool horizontal_motion = (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); + const bool vertical_motion = sq(_state.vel(2)) > fmaxf(P(State::vel.idx + 2, State::vel.idx + 2), 0.1f); 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(); } @@ -98,7 +94,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } 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); + _state.pos(2), _state.pos(2) + _range_sensor.getRange()); } _rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2), @@ -146,9 +142,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) - && _range_sensor.isRegularlySendingData() - && _rng_consistency_check.isKinematicallyConsistent(); - + && _range_sensor.isRegularlySendingData(); const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) && (_params.ekf2_rng_ctrl == static_cast(RngCtrl::CONDITIONAL))