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

handle noise while hovering

Browse Source
This commit is contained in:
Marco Hauswirth 2024-10-03 10:32:31 +02:00
parent 47b163ccc3
commit 5a9da7de9f
6 changed files with 90 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/EstimatorStatusFlags.msg
View File

@ -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

9
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
View File

@ -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);

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

@ -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<int>(_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<float, 2> _R{};
matrix::SquareMatrix<float, 2> _P{};
matrix::SquareMatrix<float, 2> _A{};
@ -73,8 +109,10 @@ private:
float _dist_bottom_prev{};
AlphaFilter<float> _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

47
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -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);
}

3
src/modules/ekf2/EKF/common.h
View File

@ -622,6 +622,9 @@ uint64_t mag_heading_consistent :
uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position
uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
uint64_t rng_kin_unknown : 1; ///< xx - true when the range finder kinematic consistency check is not running
} flags;
uint64_t value;
};

1
src/modules/ekf2/EKF2.cpp
View File

@ -1936,6 +1936,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
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;