From 259b851ba76e4b1ac946c20a70ed52e6eaaa5aff Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 12 Oct 2021 15:45:49 +0200 Subject: [PATCH] WindEstimator: remove filter reset due to airspeed measurement rejection As the purpose of this wind estimator is to (mainly) catch airspeed failures, we don't value estimator stability as much as the reliability to catch actual sensor issues, and thus do not reset the filter (as this may hide a real issue with the sensor) Signed-off-by: Silvan Fuhrer --- src/lib/wind_estimator/WindEstimator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 28851c31d6..af5223a74e 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/src/lib/wind_estimator/WindEstimator.cpp @@ -194,13 +194,13 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const bool reinit_filter = false; bool meas_is_rejected = false; + // 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); - reinit_filter |= _tas_innov_var < 0.0f; - - if (meas_is_rejected || reinit_filter) { - if (reinit_filter) { + if (meas_is_rejected || _tas_innov_var < 0.f) { + // only reset filter if _tas_innov_var gets unfeasible, but not never if tas measurement is rejected + if (_tas_innov_var < 0.0f) { _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed); }