From 00e88a02c0414f0d577a014f0a4dc106f8294753 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 16 May 2022 14:06:49 +0200 Subject: [PATCH] baro bias: improve innovation sequence monitoring The baro observation noise parameter is often over-estimated in order as a measure to mitigate temporary offsets in the readings due to wind gusts or poor pressure compensation tuning. The side effect is that the innovation sequence monitoring based on normalized innovation struggles to detect an offset in the state because the innovation isn't statistically significant. To counter this issue, a simpler check is added to trigger the process noise boost when the innovation has the same sign for a long period of time. --- src/modules/ekf2/EKF/baro_bias_estimator.cpp | 45 ++++++++++++++------ src/modules/ekf2/EKF/baro_bias_estimator.hpp | 13 +++--- 2 files changed, 38 insertions(+), 20 deletions(-) diff --git a/src/modules/ekf2/EKF/baro_bias_estimator.cpp b/src/modules/ekf2/EKF/baro_bias_estimator.cpp index f9625ae0d4..26e4a754a5 100644 --- a/src/modules/ekf2/EKF/baro_bias_estimator.cpp +++ b/src/modules/ekf2/EKF/baro_bias_estimator.cpp @@ -43,13 +43,23 @@ void BaroBiasEstimator::predict(const float dt) { // State is constant // Predict state covariance only - _state_var += _process_var * dt * dt; + float delta_state_var = _process_var * dt * dt; + + if (isOffsetDetected()) { + // A bias in the state has been detected by the innovation sequence check + // Boost the process noise until the offset is removed + delta_state_var *= _process_var_boost_gain; + } + + _state_var += delta_state_var; constrainStateVar(); if (dt > FLT_EPSILON && fabsf(_dt - dt) > 0.001f) { - _signed_innov_test_ratio_lpf.setParameters(dt, _lpf_time_constant); + _signed_innov_test_ratio_lpf.setParameters(dt, _innov_sequence_monitnoring_time_constant); _dt = dt; } + + _status.bias_var = _state_var; } void BaroBiasEstimator::constrainStateVar() @@ -70,14 +80,7 @@ void BaroBiasEstimator::fuseBias(const float measurement, const float measuremen } - if (isLargeOffsetDetected()) { - // A bias in the state has been detected by the innovation - // sequence check. - bumpStateVariance(); - } - - const float signed_innov_test_ratio = matrix::sign(innov) * innov_test_ratio; - _signed_innov_test_ratio_lpf.update(math::constrain(signed_innov_test_ratio, -1.f, 1.f)); + updateOffsetDetection(innov, innov_test_ratio); _status = packStatus(innov, innov_var, innov_test_ratio); } @@ -103,14 +106,28 @@ inline void BaroBiasEstimator::updateStateCovariance(const float K) constrainStateVar(); } -inline bool BaroBiasEstimator::isLargeOffsetDetected() const +inline void BaroBiasEstimator::updateOffsetDetection(const float innov, const float innov_test_ratio) { - return fabsf(_signed_innov_test_ratio_lpf.getState()) > 0.2f; + const float signed_innov_test_ratio = matrix::sign(innov) * innov_test_ratio; + _signed_innov_test_ratio_lpf.update(math::constrain(signed_innov_test_ratio, -1.f, 1.f)); + + if (innov > 0.f) { + _time_since_last_positive_innov = 0.f; + _time_since_last_negative_innov += _dt; + + } else { + _time_since_last_negative_innov = 0.f; + _time_since_last_positive_innov += _dt; + } } -inline void BaroBiasEstimator::bumpStateVariance() +inline bool BaroBiasEstimator::isOffsetDetected() const { - _state_var += _process_var_boost_gain * _process_var * _dt * _dt; + // There is an offset in the estimate if the average of innovation is statistically too large + // or if the sign of the innovation is constantly the same + return fabsf(_signed_innov_test_ratio_lpf.getState()) > 0.2f + || (_time_since_last_positive_innov > _innov_sequence_monitnoring_time_constant) + || (_time_since_last_negative_innov > _innov_sequence_monitnoring_time_constant); } inline BaroBiasEstimator::status BaroBiasEstimator::packStatus(const float innov, const float innov_var, diff --git a/src/modules/ekf2/EKF/baro_bias_estimator.hpp b/src/modules/ekf2/EKF/baro_bias_estimator.hpp index 5280b542d0..e559b0a544 100644 --- a/src/modules/ekf2/EKF/baro_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/baro_bias_estimator.hpp @@ -78,7 +78,6 @@ public: void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; - _state_drift_rate = 3.f * process_noise; } void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; } void setInnovGate(float gate_size) { _gate_size = gate_size; } @@ -92,7 +91,6 @@ public: private: float _state{0.f}; float _state_max{100.f}; - float _state_drift_rate{0.005f}; ///< in m/s float _dt{0.01f}; float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio) @@ -100,7 +98,10 @@ private: float _process_var{25.0e-6f}; ///< State process noise variance (m^2/s^2) float _state_var_max{2.f}; ///< Used to constrain the state variance (m^2) - AlphaFilter _signed_innov_test_ratio_lpf; ///< innovation sequence monitoring; used to detect a bias in the state + // Innovation sequence monitoring; used to detect a bias in the state + AlphaFilter _signed_innov_test_ratio_lpf; + float _time_since_last_negative_innov{0.f}; + float _time_since_last_positive_innov{0.f}; status _status; @@ -118,12 +119,12 @@ private: void updateState(float K, float innov); void updateStateCovariance(float K); - bool isLargeOffsetDetected() const; - void bumpStateVariance(); + void updateOffsetDetection(float innov, float innov_test_ratio); + bool isOffsetDetected() const; status packStatus(float innov, float innov_var, float innov_test_ratio) const; - static constexpr float _lpf_time_constant{10.f}; ///< in seconds + static constexpr float _innov_sequence_monitnoring_time_constant{10.f}; ///< in seconds static constexpr float _process_var_boost_gain{1.0e3f}; }; #endif // !EKF_BARO_BIAS_ESTIMATOR_HPP