From 4f1091792fc0e0aacecda8465d2d5be3442bfdbd Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 7 Jun 2022 16:45:34 +0200 Subject: [PATCH] ekf2 preflight: only check innovation of active height sources --- src/modules/ekf2/EKF2.cpp | 5 +++ src/modules/ekf2/Utility/PreFlightChecker.cpp | 36 ++++++++++++++++--- src/modules/ekf2/Utility/PreFlightChecker.hpp | 17 ++++++++- 3 files changed, 52 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7b58a8019f..0e43e218a4 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -877,6 +877,11 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _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) { diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index da332a44ae..4a9369764d 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -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(); diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp index 9b6bc21c54..10639bb915 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -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)