fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 13:33:40 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix wrong miniEKF init

Browse Source
This commit is contained in:
Marco Hauswirth
2025-08-26 15:07:33 +02:00
parent 0058953e4f
commit 4e40a86971
2 changed files with 8 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
+3 -10
View File
@@ -58,15 +58,8 @@ public:
float getInnov() const { return _innov; } float getInnov() const { return _innov; }
float getInnovVar() const { return _innov_var; } float getInnovVar() const { return _innov_var; }
bool isKinematicallyConsistent() const bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; }
{ bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; }
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, void UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
uint64_t time_us); uint64_t time_us);
void initMiniKF(float p1, float p2, float x1, float x2); void initMiniKF(float p1, float p2, float x1, float x2);
@@ -107,7 +100,7 @@ private:
float _innov_var{}; float _innov_var{};
uint64_t _time_last_update_us{0}; uint64_t _time_last_update_us{0};
float _dist_bottom_prev{}; float _dist_bottom_prev{};
AlphaFilter<float> _test_ratio_lpf{0.3}; // average signed test ratio used to detect a bias in the data AlphaFilter<float> _test_ratio_lpf{0.3};
float _gate{1.f}; float _gate{1.f};
int _sample_count{0}; int _sample_count{0};
KinematicState _state{KinematicState::UNKNOWN}; KinematicState _state{KinematicState::UNKNOWN};
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+5 -11
View File
@@ -66,9 +66,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
if (_control_status.flags.in_air) { 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_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float dist_var = sq(_params.range_noise) + dist_dependant_var; 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()) { 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), _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), _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 { } else {
_rng_consistency_check.setFixedWing(false, _params.range_kin_consistency_gate); _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 (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()) { if (_rng_consistency_check.isRunning()) {
_rng_consistency_check.stopMiniKF(); _rng_consistency_check.stopMiniKF();
} }
@@ -98,7 +94,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} else if (vertical_motion) { } else if (vertical_motion) {
if (!_rng_consistency_check.isRunning()) { 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), _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), _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 const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) && 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) const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
&& (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)) && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))