From 58f63909ca81fbc9367c572f82a3312c52ae1bff Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Mon, 14 Jul 2025 09:03:30 +0200 Subject: [PATCH] * naming conventions * add landed-state for range-finder-cons-check * ajdust unit-tests --- .../range_finder_consistency_check.cpp | 41 ++++++++++++++----- .../range_finder_consistency_check.hpp | 37 ++++++++--------- .../range_finder/range_height_control.cpp | 26 ++++++------ src/modules/ekf2/EKF/common.h | 2 +- src/modules/ekf2/EKF2.cpp | 2 +- src/modules/ekf2/EKF2.hpp | 2 +- .../ekf2/test/test_EKF_accelerometer.cpp | 3 -- .../ekf2/test/test_EKF_height_fusion.cpp | 4 +- src/modules/ekf2/test/test_EKF_terrain.cpp | 2 +- 9 files changed, 64 insertions(+), 55 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 d6fa31dcb8..780a38b88b 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 @@ -59,7 +59,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const { const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; - if (dt > 1.f) { + if (dt > _kTestRatioLpfTimeConstant || _landed) { _time_last_update_us = time_us; _initialized = false; return; @@ -108,8 +108,7 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const } else if (measurement_idx == 1) { _innov = y; const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers - _test_ratio_lpf.setParameters(dt, _t_to_init); - _test_ratio_lpf.update(sign(_innov) * test_ratio); + _test_ratio_lpf.update(test_ratio, dt); } // update step @@ -129,21 +128,21 @@ 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 > _t_to_init) { + // let the filter settle before starting the consistency check + if (_t_since_first_sample > _kTestRatioLpfTimeConstant) { if (fabsf(_test_ratio_lpf.getState()) < 1.f) { const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f); - if (!horizontal_motion && vertical_motion) { - _state = KinematicState::CONSISTENT; + if (!_horizontal_motion && vertical_motion) { + _state = KinematicState::kConsistent; - } else if (_state == KinematicState::CONSISTENT || vertical_motion) { - _state = KinematicState::UNKNOWN; + } else if (_state == KinematicState::kConsistent || vertical_motion) { + _state = KinematicState::kUnknown; } } else { _t_since_first_sample = 0.f; - _state = KinematicState::INCONSISTENT; + _state = KinematicState::kInconsistent; } } else { @@ -152,7 +151,9 @@ void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, } void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var, - const float dist_bottom, const float dist_bottom_var, const uint64_t time_us) + const float dist_bottom, const float dist_bottom_var, const uint64_t time_us, + const uint8_t current_posD_reset_count + ) { if (!_initialized || current_posD_reset_count != _last_posD_reset_count) { _last_posD_reset_count = current_posD_reset_count; @@ -161,3 +162,21 @@ void RangeFinderConsistencyCheck::run(const float z, const float z_var, const fl update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us); } + +void RangeFinderConsistencyCheck::setIsLanded(const bool landed) +{ + if (landed) { + _state = KinematicState::kUnknown; + } + + _landed = landed; +} + +void RangeFinderConsistencyCheck::reset() +{ + if (_initialized && _state == KinematicState::kConsistent) { + _state = KinematicState::kUnknown; + } + + _initialized = false; +} 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 0fb9e11a5e..72bd904e66 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 @@ -49,31 +49,25 @@ public: ~RangeFinderConsistencyCheck() = default; enum class KinematicState : int { - INCONSISTENT = 0, - CONSISTENT = 1, - UNKNOWN = 2 + kInconsistent = 0, + kConsistent = 1, + kUnknown = 2 }; 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 && _t_since_first_sample > _t_to_init; } - bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; } + bool isKinematicallyConsistent() const { return _state == KinematicState::kConsistent && _t_since_first_sample > _kTestRatioLpfTimeConstant; } + bool isNotKinematicallyInconsistent() const { return _state != KinematicState::kInconsistent && (_t_since_first_sample > _kTestRatioLpfTimeConstant || _landed); } 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); + void run(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, + uint64_t time_u, uint8_t current_posD_reset_coun); void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; } - void reset() - { - if (_initialized && _state == KinematicState::CONSISTENT) { - _state = KinematicState::UNKNOWN; - } + void reset(); - _initialized = false; - } - - uint8_t current_posD_reset_count{0}; - bool horizontal_motion{false}; + void setHorizontalMotion(const bool horizontal_motion) { _horizontal_motion = horizontal_motion; } + bool getHorizontalMotion() const { return _horizontal_motion; } + void setIsLanded(bool landed); private: @@ -89,13 +83,14 @@ private: float _innov{0.f}; float _innov_var{0.f}; uint64_t _time_last_update_us{0}; - static constexpr float time_constant{1.f}; - AlphaFilter _test_ratio_lpf{time_constant}; + static constexpr float _kTestRatioLpfTimeConstant{1.f}; + AlphaFilter _test_ratio_lpf{_kTestRatioLpfTimeConstant}; float _gate{1.0f}; - KinematicState _state{KinematicState::UNKNOWN}; + KinematicState _state{KinematicState::kUnknown}; float _t_since_first_sample{0.f}; uint8_t _last_posD_reset_count{0}; - static constexpr float _t_to_init{1.f}; + bool _horizontal_motion{false}; + bool _landed{false}; }; namespace RangeFilter 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 fa9bc8c594..5612348a30 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 @@ -53,10 +53,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (_range_sensor.isDataReady()) { - _range_sensor.setPitchOffset(_params.rng_sens_pitch); + _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.setMaxFogDistance(_params.rng_fog); + _range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t); + _range_sensor.setMaxFogDistance(_params.ekf2_rng_fog); _rng_consistency_check.setGate(_params.range_kin_consistency_gate); _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); @@ -66,24 +66,22 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - const float dist_var = getRngVar(); - _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); + const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.5f); - const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); - - if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { + if (!updated_horizontal_motion && _rng_consistency_check.getHorizontalMotion()) { _rng_consistency_check.reset(); } - _rng_consistency_check.horizontal_motion = updated_horizontal_motion; + _rng_consistency_check.setIsLanded(!_control_status.flags.in_air); + _rng_consistency_check.setHorizontalMotion(updated_horizontal_motion); const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), - dist_var, imu_sample.time_us); + dist_var, imu_sample.time_us, get_posD_reset_count()); } else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) { - _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setRange(_params.ekf2_min_rng); _range_sensor.setValidity(true); } else { @@ -220,7 +218,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { if (starting_conditions_passing) { if (_control_status.flags.opt_flow_terrain) { - if (!aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected && _rng_consistency_check.isNotKinematicallyInconsistent()) { _control_status.flags.rng_terrain = true; fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); } @@ -273,8 +271,8 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) float Ekf::getRngVar() const { - const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); - const float dist_var = sq(_params.range_noise) + dist_dependant_var; + const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom()); + const float dist_var = sq(_params.ekf2_rng_noise) + dist_dependant_var; return dist_var; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 9a258093c0..d05ef6aac5 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -426,7 +426,7 @@ struct parameters { float ekf2_rng_qlty_t{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check - float rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] + float ekf2_rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) #endif // CONFIG_EKF2_RANGE_FINDER diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9fd6368edd..3196818ce2 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -163,7 +163,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax), _param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax), _param_ekf2_rng_qlty_t(_params->ekf2_rng_qlty_t), - _param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate), + _param_range_kin_consistency_gate(_params->range_kin_consistency_gate), _param_ekf2_rng_fog(_params->ekf2_rng_fog), _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index c95e3c3623..466d6d9717 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -622,7 +622,7 @@ private: (ParamExtFloat) _param_ekf2_rng_a_vmax, (ParamExtFloat) _param_ekf2_rng_a_hmax, (ParamExtFloat) _param_ekf2_rng_qlty_t, - (ParamExtFloat) _param_ekf2_rng_k_gate, + (ParamExtFloat) _param_range_kin_consistency_gate, (ParamExtFloat) _param_ekf2_rng_fog, (ParamExtFloat) _param_ekf2_rng_pos_x, (ParamExtFloat) _param_ekf2_rng_pos_y, diff --git a/src/modules/ekf2/test/test_EKF_accelerometer.cpp b/src/modules/ekf2/test/test_EKF_accelerometer.cpp index 8b5d02f2aa..619207dbd4 100644 --- a/src/modules/ekf2/test/test_EKF_accelerometer.cpp +++ b/src/modules/ekf2/test/test_EKF_accelerometer.cpp @@ -197,9 +197,6 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroRange) const float bias = CONSTANTS_ONE_G; _sensor_simulator._imu.setAccelData(Vector3f(0.f, 0.f, -CONSTANTS_ONE_G + bias)); _sensor_simulator.runSeconds(2); - _sensor_simulator._imu.setAccelClipping(false, false, true); - _sensor_simulator.runSeconds(2); - // THEN: the bad vertical is detected because both sources agree EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical); } diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index bb4c07da39..7f7aba1917 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -155,7 +155,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) _ekf_wrapper.enableBaroHeightFusion(); _ekf_wrapper.enableGpsHeightFusion(); _ekf_wrapper.enableRangeHeightFusion(); - _sensor_simulator.runSeconds(10); + _sensor_simulator.runSeconds(2); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); @@ -235,7 +235,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver) _ekf_wrapper.enableGpsHeightFusion(); _ekf_wrapper.enableRangeHeightFusion(); - _sensor_simulator.runSeconds(10); + _sensor_simulator.runSeconds(0.1); EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp index bc1a9b1040..7edfc665f7 100644 --- a/src/modules/ekf2/test/test_EKF_terrain.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -207,7 +207,7 @@ TEST_F(EkfTerrainTest, testHeightReset) const float new_baro_height = _sensor_simulator._baro.getData() + 50.f; _sensor_simulator._baro.setData(new_baro_height); _sensor_simulator.stopGps(); // prevent from switching to GNSS height - _sensor_simulator.runSeconds(100); + _sensor_simulator.runSeconds(10); // THEN: a height reset occurred and the estimated distance to the ground remains constant reset_logging_checker.capturePostResetState();