diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp index 28227dcec5..2824635cb6 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp +++ b/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, diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp index 1bbe1f7596..cfbbdefd53 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp +++ b/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 }; diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp index 2bbad62ae6..34d2cea07e 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp @@ -61,6 +61,9 @@ private: std::normal_distribution _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)