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