From 128e49358e4c450cf7042b10ea866726bd7952f5 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 18 Oct 2022 20:03:52 +0200 Subject: [PATCH] Wind Estimator: remove filter reset due to beta fusion timeout Signed-off-by: Silvan Fuhrer --- src/lib/wind_estimator/WindEstimator.cpp | 54 ++++++------------------ src/lib/wind_estimator/WindEstimator.hpp | 6 +-- 2 files changed, 15 insertions(+), 45 deletions(-) diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 84a5ba6bc3..7b3fd720f7 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -67,10 +67,6 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari _P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = sq(INITIAL_WIND_ERROR); } - // reset the timestamp for measurement rejection - _time_rejected_tas = 0; - _time_rejected_beta = 0; - _wind_estimator_reset = true; return true; @@ -129,20 +125,14 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const sym::FuseAirspeed(velI, _state, _P, true_airspeed, _tas_var, FLT_EPSILON, &H_tas, &K, &_tas_innov_var, &_tas_innov); - bool reinit_filter = false; - bool meas_is_rejected = false; + const bool meas_is_rejected = check_if_meas_is_rejected(_tas_innov, _tas_innov_var, _tas_gate); - // note: _time_rejected_tas and reinit_filter are not used anymore as filter can't get reset due to tas rejection - meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas, - reinit_filter); + if (_tas_innov_var < 0.0f) { + // re init filter in case of a negative variance, and trigger early return to not fuse measurement + _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var); + return; - if (meas_is_rejected || _tas_innov_var < 0.f) { - // only reset filter if _tas_innov_var gets unfeasible - if (_tas_innov_var < 0.0f) { - _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var); - } - - // we either did a filter reset or the current measurement was rejected so do not fuse + } else if (meas_is_rejected) { return; } @@ -180,20 +170,14 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const sym::FuseBeta(velI, _state, _P, q_att, _beta_var, FLT_EPSILON, &H_beta, &K, &_beta_innov_var, &_beta_innov); - bool reinit_filter = false; - bool meas_is_rejected = false; + const bool meas_is_rejected = check_if_meas_is_rejected(_beta_innov, _beta_innov_var, _beta_gate); - meas_is_rejected = check_if_meas_is_rejected(time_now, _beta_innov, _beta_innov_var, _beta_gate, _time_rejected_beta, - reinit_filter); + if (_beta_innov_var < 0.0f) { + // re init filter in case of a negative variance, and trigger early return to not fuse measurement + _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi()); + return; - reinit_filter |= _beta_innov_var < 0.0f; - - if (meas_is_rejected || reinit_filter) { - if (reinit_filter) { - _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi()); - } - - // we either did a filter reset or the current measurement was rejected so do not fuse + } else if (meas_is_rejected) { return; } @@ -243,17 +227,7 @@ WindEstimator::run_sanity_checks() } bool -WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, - uint64_t &time_meas_rejected, bool &reinit_filter) +WindEstimator::check_if_meas_is_rejected(float innov, float innov_var, uint8_t gate_size) { - if (innov * innov > gate_size * gate_size * innov_var) { - time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected; - - } else { - time_meas_rejected = 0; - } - - reinit_filter = time_now - time_meas_rejected > 5_s && time_meas_rejected != 0; - - return time_meas_rejected != 0; + return (innov * innov > gate_size * gate_size * innov_var); } diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index 9fc6a182b6..4c7f405552 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/src/lib/wind_estimator/WindEstimator.hpp @@ -70,8 +70,7 @@ public: bool is_estimate_valid() { return _initialised; } - bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, - uint64_t &time_meas_rejected, bool &reinit_filter); + bool check_if_meas_is_rejected(float innov, float innov_var, uint8_t gate_size); matrix::Vector2f get_wind() { return matrix::Vector2f{_state(INDEX_W_N), _state(INDEX_W_E)}; } @@ -132,9 +131,6 @@ private: uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction - uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected - uint64_t _time_rejected_tas = - 0; ///< timestamp of when true airspeed measurements have consistently started to be rejected bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle