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

ekf2 preflight: only check innovation of active height sources

Browse Source
This commit is contained in:
bresch 2022-06-07 16:45:34 +02:00 committed by Daniel Agar
parent cb2738e187
commit 4f1091792f
3 changed files with 52 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -877,6 +877,11 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
} else if (_ekf.control_status_flags().in_air != _ekf.control_status_prev_flags().in_air) {

36
src/modules/ekf2/Utility/PreFlightChecker.cpp
View File

@ -102,10 +102,29 @@ bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s
bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
{
const float hgt_innov = fmaxf(fabsf(innov.gps_vpos), fmaxf(fabs(innov.ev_vpos),
fabs(innov.rng_vpos))); // only temporary solution
const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
return checkInnovFailed(hgt_innov_lpf, hgt_innov, _hgt_innov_test_lim, _hgt_innov_spike_lim);
bool has_failed = false;
if (_is_using_baro_hgt_aiding) {
const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
if (_is_using_gps_hgt_aiding) {
const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
if (_is_using_rng_hgt_aiding) {
const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
if (_is_using_ev_hgt_aiding) {
const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
return has_failed;
}
bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
@ -127,6 +146,10 @@ void PreFlightChecker::reset()
_is_using_flow_aiding = false;
_is_using_ev_pos_aiding = false;
_is_using_ev_vel_aiding = false;
_is_using_baro_hgt_aiding = false;
_is_using_gps_hgt_aiding = false;
_is_using_rng_hgt_aiding = false;
_is_using_ev_hgt_aiding = false;
_has_heading_failed = false;
_has_horiz_vel_failed = false;
_has_vert_vel_failed = false;
@ -134,7 +157,10 @@ void PreFlightChecker::reset()
_filter_vel_n_innov.reset();
_filter_vel_e_innov.reset();
_filter_vel_d_innov.reset();
_filter_hgt_innov.reset();
_filter_baro_hgt_innov.reset();
_filter_gps_hgt_innov.reset();
_filter_rng_hgt_innov.reset();
_filter_ev_hgt_innov.reset();
_filter_heading_innov.reset();
_filter_flow_x_innov.reset();
_filter_flow_y_innov.reset();

17
src/modules/ekf2/Utility/PreFlightChecker.hpp
View File

@ -79,6 +79,11 @@ public:
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
bool hasHeadingFailed() const { return _has_heading_failed; }
bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
bool hasVertVelFailed() const { return _has_vert_vel_failed; }
@ -149,15 +154,25 @@ private:
bool _is_using_ev_pos_aiding{};
bool _is_using_ev_vel_aiding{};
bool _is_using_baro_hgt_aiding{};
bool _is_using_gps_hgt_aiding{};
bool _is_using_rng_hgt_aiding{};
bool _is_using_ev_hgt_aiding{};
// Low-pass filters for innovation pre-flight checks
InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec)
InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec)
InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec)
InnovationLpf _filter_hgt_innov; ///< Preflight low pass filter height innovation (m)
InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad)
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
// Preflight low pass filter height innovation (m)
InnovationLpf _filter_baro_hgt_innov;
InnovationLpf _filter_gps_hgt_innov;
InnovationLpf _filter_rng_hgt_innov;
InnovationLpf _filter_ev_hgt_innov;
// Preflight low pass filter time constant inverse (1/sec)
static constexpr float _innov_lpf_tau_inv = 0.2f;
// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)