dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
bresch 2020-01-31 17:36:30 +01:00 committed by Mathieu Bresciani
parent e15e94b00a
commit 7ca13a67b3
1 changed files with 49 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp
View File

@ -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<float> 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<float> _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);
}