From b024cdd282268f5ad82aa1aa0b9c449c3ce3b62c Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Mon, 25 May 2020 11:10:01 +0200 Subject: [PATCH] unit test: check that the accel bias is only learned when observable (#827) --- test/sensor_simulator/ekf_wrapper.cpp | 5 ++++ test/sensor_simulator/ekf_wrapper.h | 2 ++ test/test_EKF_initialization.cpp | 34 +++++++++++++++++++++++++++ 3 files changed, 41 insertions(+) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index 25886a9841..c6ac9b6762 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -200,3 +200,8 @@ int EkfWrapper::getQuaternionResetCounter() const _ekf->get_quat_reset(tmp, &counter); return static_cast(counter); } + +matrix::Vector3f EkfWrapper::getDeltaVelBiasVariance() const +{ + return _ekf->covariances_diagonal().slice<3, 1>(13,0); +} diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index c74763c13a..104424ce69 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -94,6 +94,8 @@ public: matrix::Vector getQuaternionVariance() const; int getQuaternionResetCounter() const; + matrix::Vector3f getDeltaVelBiasVariance() const; + private: std::shared_ptr _ekf; diff --git a/test/test_EKF_initialization.cpp b/test/test_EKF_initialization.cpp index a9e80a2a9b..b239ca4d4e 100644 --- a/test/test_EKF_initialization.cpp +++ b/test/test_EKF_initialization.cpp @@ -108,6 +108,25 @@ class EkfInitializationTest : public ::testing::Test { EXPECT_TRUE(vel_var(1) > vel_variance_limit) << "vel_var(1)" << vel_var(1); EXPECT_TRUE(vel_var(2) > vel_variance_limit) << "vel_var(2)" << vel_var(2); } + + void learningCorrectAccelBias() + { + const Dcmf R_to_earth = Dcmf(_ekf->getQuaternion()); + const Vector3f dvel_bias_var = _ekf_wrapper.getDeltaVelBiasVariance(); + const Vector3f accel_bias = _ekf->getAccelBias(); + + for (int i = 0; i < 3; i++){ + if (fabsf(R_to_earth(2, i)) > 0.8f) { + // Highly observable, the variance decreases + EXPECT_LT(dvel_bias_var(i), 2.0e-6f) << "axis " << i; + + } else { + // Poorly observable, the variance is set to 0 + EXPECT_FLOAT_EQ(dvel_bias_var(i), 0.f) << "axis" << i; + EXPECT_FLOAT_EQ(accel_bias(i), 0.f) << "axis" << i; + } + } + } }; TEST_F(EkfInitializationTest, initializeWithZeroTilt) @@ -122,6 +141,9 @@ TEST_F(EkfInitializationTest, initializeWithZeroTilt) initializedOrienationIsMatchingGroundTruth(quat_sim); validStateAfterOrientationInitialization(); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); } TEST_F(EkfInitializationTest, initializeHeadingWithZeroTilt) @@ -137,6 +159,9 @@ TEST_F(EkfInitializationTest, initializeHeadingWithZeroTilt) initializedOrienationIsMatchingGroundTruth(quat_sim); validStateAfterOrientationInitialization(); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); } TEST_F(EkfInitializationTest, initializeWithTilt) @@ -151,6 +176,9 @@ TEST_F(EkfInitializationTest, initializeWithTilt) initializedOrienationIsMatchingGroundTruth(quat_sim); validStateAfterOrientationInitialization(); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); } TEST_F(EkfInitializationTest, initializeWithPitch90) @@ -166,6 +194,9 @@ TEST_F(EkfInitializationTest, initializeWithPitch90) initializedOrienationIsMatchingGroundTruth(quat_sim); // TODO: Quaternion Variance is smaller in this case than in the other cases validStateAfterOrientationInitialization(); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); } TEST_F(EkfInitializationTest, initializeWithRoll90) @@ -180,4 +211,7 @@ TEST_F(EkfInitializationTest, initializeWithRoll90) initializedOrienationIsMatchingGroundTruth(quat_sim); validStateAfterOrientationInitialization(); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); }