From 7ca13a67b3c9791f99f0c960bee9e1e5c0c1f89e Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 31 Jan 2020 17:36:30 +0100 Subject: [PATCH] ZeroOrderHoverThrustEstimatorTest: add hover thrust jump test case This is done to test the recovery function of the estimator in case of divergence or sudden extreme hover thrust change. Also specify seed of random generator --- .../zero_order_hover_thrust_ekf_test.cpp | 70 +++++++++++++------ 1 file changed, 49 insertions(+), 21 deletions(-) diff --git a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp b/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp index 01f03ad723..2bbad62ae6 100644 --- a/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp +++ b/src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp @@ -47,16 +47,20 @@ using namespace matrix; class ZeroOrderHoverThrustEkfTest : public ::testing::Test { public: + ZeroOrderHoverThrustEkfTest() + { + _random_generator.seed(42); + } float computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust); - ZeroOrderHoverThrustEkf::status runEkf(float accel, float thrust, float time, float accel_noise = 0.f, + ZeroOrderHoverThrustEkf::status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f, float thr_noise = 0.f); - std::normal_distribution standard_normal_distribution_; - std::default_random_engine random_generator_; // Pseudo-random generator with constant seed - private: ZeroOrderHoverThrustEkf _ekf{}; static constexpr float _dt = 0.02f; + + std::normal_distribution _standard_normal_distribution; + std::default_random_engine _random_generator; // Pseudo-random generator with constant seed }; float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust) @@ -64,15 +68,16 @@ float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float th return CONSTANTS_ONE_G * thrust / hover_thrust - CONSTANTS_ONE_G; } -ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkfTest::runEkf(float accel, float thrust, float time, +ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust, float time, float accel_noise, float thr_noise) { ZeroOrderHoverThrustEkf::status status{}; for (float t = 0.f; t <= time; t += _dt) { _ekf.predict(_dt); - float noisy_accel = accel + accel_noise * standard_normal_distribution_(random_generator_); - float noisy_thrust = thrust + thr_noise * standard_normal_distribution_(random_generator_); + float noisy_thrust = thrust + thr_noise * _standard_normal_distribution(_random_generator); + float accel_theory = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true); + float noisy_accel = accel_theory + accel_noise * _standard_normal_distribution(_random_generator); _ekf.fuseAccZ(noisy_accel, noisy_thrust, status); } @@ -84,10 +89,9 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticCase) // GIVEN: a vehicle at hover, (the estimator starting at the true value) const float thrust = 0.5f; const float hover_thrust_true = 0.5f; - const float accel_meas = 0.f; // WHEN: we input noiseless data and run the filter - ZeroOrderHoverThrustEkf::status status = runEkf(accel_meas, thrust, 1.f); + ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 1.f); // THEN: The estimate should not move and its variance decrease quickly EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-4f); @@ -100,10 +104,9 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence) // GIVEN: a vehicle at hover, but the estimator is starting at hover_thrust = 0.5 const float thrust = 0.72f; const float hover_thrust_true = 0.72f; - const float accel_meas = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true); // WHEN: we input noiseless data and run the filter - ZeroOrderHoverThrustEkf::status status = runEkf(accel_meas, thrust, 2.f); + ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f); // THEN: the state should converge to the true value and its variance decrease EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-2f); @@ -118,16 +121,15 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergenceWithNoise) const float noise_var = sigma_noise * sigma_noise; const float thrust = 0.72f; const float hover_thrust_true = 0.72f; - const float accel_meas = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true); const float t_sim = 10.f; // WHEN: we input noisy accel data and run the filter - ZeroOrderHoverThrustEkf::status status = runEkf(accel_meas, thrust, t_sim, sigma_noise); + ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise); // THEN: the estimate should converge and the accel noise variance should be close to the true noise value EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f); EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); - EXPECT_NEAR(status.accel_noise_var, noise_var, 0.3f * noise_var); + EXPECT_NEAR(status.accel_noise_var, noise_var, 0.2f * noise_var); } TEST_F(ZeroOrderHoverThrustEkfTest, testLargeAccelNoiseAndBias) @@ -137,14 +139,13 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testLargeAccelNoiseAndBias) const float noise_var = sigma_noise * sigma_noise; const float thrust = 0.4f; // Below hover thrust const float hover_thrust_true = 0.72f; - const float accel_meas = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true); const float t_sim = 15.f; // WHEN: we input noisy accel data and run the filter - ZeroOrderHoverThrustEkf::status status = runEkf(accel_meas, thrust, t_sim, sigma_noise); + ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise); // THEN: the estimate should converge and the accel noise variance should be close to the true noise value - EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2); + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 7e-2); EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); EXPECT_NEAR(status.accel_noise_var, noise_var, 0.2f * noise_var); } @@ -155,18 +156,45 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testThrustAndAccelNoise) // and the input thrust are noisy const float accel_noise = 2.f; const float accel_var = accel_noise * accel_noise; - const float thr_noise = 0.1f; + const float thr_noise = 0.01f; const float thrust = 0.72f; // Above hover thrust const float hover_thrust_true = 0.6f; - const float accel_meas = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true); const float t_sim = 15.f; // WHEN: we input noisy accel and thrust data, and run the filter - ZeroOrderHoverThrustEkf::status status = runEkf(accel_meas, thrust, t_sim, accel_noise, thr_noise); + ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise); // THEN: the estimate should converge and the accel noise variance should be close to the true noise value EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f); EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); // Because of the nonlinear measurment model and the thust noise, the accel noise estimation is a bit worse - EXPECT_NEAR(status.accel_noise_var, accel_var, 0.5f * accel_var); + EXPECT_NEAR(status.accel_noise_var, accel_var, 0.4f * accel_var); +} + +TEST_F(ZeroOrderHoverThrustEkfTest, testHoverThrustJump) +{ + // GIVEN: a vehicle hovering, the estimator starts with the wrong estimate, the measurements + // and the input thrust are noisy + const float accel_noise = 2.f; + const float accel_var = accel_noise * accel_noise; + const float thr_noise = 0.01f; + float thrust = 0.8; // At hover + float hover_thrust_true = 0.8f; + float t_sim = 10.f; + + // WHEN: we input noisy accel and thrust data, and run the filter + ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise); + // THEN: change the hover thrust and the current thrust (the velocity controller responds quickly) + // Note that this is an extreme jump in hover thrust + thrust = 0.3; + hover_thrust_true = 0.3f; + t_sim = 10.f; + status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise); + + // THEN: the estimate should converge to the new hover thrust and the accel noise variance should + // be close to the true noise value + EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f); + EXPECT_NEAR(status.hover_thrust_var, 0.f, 1e-3f); + // After a recovery, the noise variance estimate takes more time to converge back to the true value + EXPECT_NEAR(status.accel_noise_var, accel_var, 2.f * accel_var); }