fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 03:24:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix wrong miniEKF init

Browse Source
This commit is contained in:
Marco Hauswirth 2024-10-04 11:02:01 +02:00
parent 0058953e4f
commit 4e40a86971
2 changed files with 8 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
View File

@ -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<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};
int _sample_count{0};
KinematicState _state{KinematicState::UNKNOWN};

16
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
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());
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<int32_t>(RngCtrl::CONDITIONAL))