MC_HTE: Stability improvements

- Use a low-passed value of the signed innovation test ratio to trigger
the state variance boost. The threshold of 0.2 has been chosen using log
replay and simulation scenarii.
- Do not reset the learned accel noise during a state variance boost.
After a few tests, this does not seem to help at all.
- Continue to learn the accel noise even if the measurement got rejected
to avoid ignoring sudden changes of noise
- Lower the acceleration noise time constant and increase min/max
values to avoid learning quickly a small variance that could temporarly
destabilize the filter
- Update filter time constants. Increasing the speed of the residual lpf
improves the quality of the learned accel noise

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp

@@ -61,17 +61,17 @@ void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, st
 		updateState(K, innov);
 		updateStateCovariance(K, H);
 		residual = computeInnov(acc_z, thrust); // residual != innovation since the hover thrust changed
-		updateMeasurementNoise(residual, H);
 
-	} else if (!isTestRatioPassing(_innov_test_ratio_lpf)) {
+	} else if (isLargeOffsetDetected()) {
 		// Rejecting all the measurements for some time,
 		// it means that the hover thrust suddenly changed or that the EKF
-		// is diverging. To recover, we bump the state variance and reset the accel noise.
+		// is diverging. To recover, we bump the state variance
 		bumpStateVariance();
-		resetAccelNoise();
 	}
 
-	updateLpf(residual, innov_test_ratio);
+	const float signed_innov_test_ratio = math::sign(innov) * innov_test_ratio;
+	updateLpf(residual, signed_innov_test_ratio);
+	updateMeasurementNoise(residual, H);
 
 	status_return = packStatus(innov, innov_var, innov_test_ratio);
 }
@@ -124,24 +124,30 @@ inline void ZeroOrderHoverThrustEkf::updateStateCovariance(const float K, const
 	_state_var = math::constrain((1.f - K * H) * _state_var, 1e-10f, 1.f);
 }
 
-inline void ZeroOrderHoverThrustEkf::updateMeasurementNoise(const float residual, const float H)
+inline bool ZeroOrderHoverThrustEkf::isLargeOffsetDetected() const
 {
-	const float alpha = _dt / (noise_learning_time_constant + _dt);
-	const float res_no_bias = residual - _residual_lpf;
-	const float P = _state_var;
-	_acc_var = math::constrain((1.f - alpha) * _acc_var  + alpha * (res_no_bias * res_no_bias + H * P * H), 1e-4f, 60.f);
+	return fabsf(_signed_innov_test_ratio_lpf) > 0.2f;
 }
 
 inline void ZeroOrderHoverThrustEkf::bumpStateVariance()
 {
-	_state_var += 10000.f * _process_var * _dt;
+	_state_var += 1e3f * _process_var * _dt;
 }
 
-inline void ZeroOrderHoverThrustEkf::updateLpf(const float residual, const float innov_test_ratio)
+inline void ZeroOrderHoverThrustEkf::updateLpf(const float residual, const float signed_innov_test_ratio)
 {
-	const float alpha = _dt / (5.f * noise_learning_time_constant + _dt);
+	const float alpha = _dt / (_lpf_time_constant + _dt);
 	_residual_lpf = (1.f - alpha) * _residual_lpf + alpha * residual;
-	_innov_test_ratio_lpf = (1.f - alpha) * _innov_test_ratio_lpf + alpha * math::min(innov_test_ratio, 5.f);
+	_signed_innov_test_ratio_lpf = (1.f - alpha) * _signed_innov_test_ratio_lpf + alpha * math::constrain(
+					       signed_innov_test_ratio, -1.f, 1.f);
+}
+
+inline void ZeroOrderHoverThrustEkf::updateMeasurementNoise(const float residual, const float H)
+{
+	const float alpha = _dt / (_noise_learning_time_constant + _dt);
+	const float res_no_bias = residual - _residual_lpf;
+	const float P = _state_var;
+	_acc_var = math::constrain((1.f - alpha) * _acc_var  + alpha * (res_no_bias * res_no_bias + H * P * H), 1.f, 400.f);
 }
 
 inline ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkf::packStatus(const float innov, const float innov_var,

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp

@@ -107,7 +107,7 @@ private:
 	float _dt{0.02f};
 
 	float _residual_lpf{}; ///< used to remove the constant bias of the residual
-	float _innov_test_ratio_lpf{}; ///< used as a delay to trigger the recovery logic
+	float _signed_innov_test_ratio_lpf{}; ///< used as a delay to trigger the recovery logic
 
 	float computeH(float thrust) const;
 	float computeInnovVar(float H) const;
@@ -125,12 +125,14 @@ private:
 
 	void updateState(float K, float innov);
 	void updateStateCovariance(float K, float H);
-	void updateMeasurementNoise(float residual, float H);
+	bool isLargeOffsetDetected() const;
 
 	void bumpStateVariance();
-	void updateLpf(float residual, float innov_test_ratio);
+	void updateLpf(float residual, float signed_innov_test_ratio);
+	void updateMeasurementNoise(float residual, float H);
 
 	status packStatus(float innov, float innov_var, float innov_test_ratio) const;
 
-	static constexpr float noise_learning_time_constant = 0.5f; ///< in seconds
+	static constexpr float _noise_learning_time_constant = 2.f; ///< in seconds
+	static constexpr float _lpf_time_constant = 1.f; ///< in seconds
 };

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp

@@ -61,6 +61,9 @@ private:
 
 	std::normal_distribution<float> _standard_normal_distribution;
 	std::default_random_engine _random_generator; // Pseudo-random generator with constant seed
+
+protected:
+	static constexpr float _accel_noise_var_min = 1.f; // Constrained in the implementation
 };
 
 float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust)
@@ -91,12 +94,13 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticCase)
 	const float hover_thrust_true = 0.5f;
 
 	// WHEN: we input noiseless data and run the filter
-	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 1.f);
+	ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);
 
 	// THEN: The estimate should not move and its variance decrease quickly
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-4f);
 	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
-	EXPECT_NEAR(status.accel_noise_var, 0.f, 1.f); // The noise learning is slow and takes more than 1s to go to zero
+	EXPECT_NEAR(status.accel_noise_var, _accel_noise_var_min,
+		    1.f); // The noise learning is slow and takes more time to go to zero
 }
 
 TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence)
@@ -111,7 +115,8 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence)
 	// THEN: the state should converge to the true value and its variance decrease
 	EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-2f);
 	EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f);
-	EXPECT_NEAR(status.accel_noise_var, 0.f, 1.f); // The noise learning is slow and takes more than 1s to go to zero
+	EXPECT_NEAR(status.accel_noise_var, _accel_noise_var_min,
+		    1.f); // The noise learning is slow and takes more time to go to zero
 }
 
 TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergenceWithNoise)