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

@@ -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,

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<float> _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<float> _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