From 7d3814b1d8f1d26b0e18bc558cf0a812d911840e Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 17 Dec 2019 18:52:18 +0100 Subject: [PATCH] Add orientation initialization tests --- test/CMakeLists.txt | 1 + test/sensor_simulator/ekf_wrapper.cpp | 32 ++++ test/sensor_simulator/ekf_wrapper.h | 7 +- test/sensor_simulator/sensor_simulator.cpp | 12 ++ test/sensor_simulator/sensor_simulator.h | 1 + test/test_EKF_basics.cpp | 14 +- test/test_EKF_initialization.cpp | 177 +++++++++++++++++++++ 7 files changed, 236 insertions(+), 8 deletions(-) create mode 100644 test/test_EKF_initialization.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index aa3ca08a58..d3a8a9acfe 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -42,6 +42,7 @@ set(SRCS test_EKF_imuSampling.cpp test_AlphaFilter.cpp test_EKF_fusionLogic.cpp + test_EKF_initialization.cpp ) add_executable(ECL_GTESTS ${SRCS}) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index 9658e78f00..8e474bacaa 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -69,3 +69,35 @@ Vector3f EkfWrapper::getGyroBias() const _ekf->get_gyro_bias(temp); return Vector3f(temp); } + +Quatf EkfWrapper::getQuaternion() const +{ + return _ekf->get_quaternion(); +} + +Eulerf EkfWrapper::getEulerAngles() const +{ + return Eulerf(getQuaternion()); +} + +matrix::Vector EkfWrapper::getState() const +{ + float state[24]; + _ekf->get_state_delayed(state); + return matrix::Vector{state}; +} + +matrix::Vector EkfWrapper::getQuaternionVariance() const +{ + return matrix::Vector(_ekf->orientation_covariances().diag()); +} + +Vector3f EkfWrapper::getPositionVariance() const +{ + return Vector3f(_ekf->position_covariances().diag()); +} + +Vector3f EkfWrapper::getVelocityVariance() const +{ + return Vector3f(_ekf->velocity_covariances().diag()); +} diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h index b8bf2cae5a..522a1761dd 100644 --- a/test/sensor_simulator/ekf_wrapper.h +++ b/test/sensor_simulator/ekf_wrapper.h @@ -63,7 +63,12 @@ public: Vector3f getVelocity() const; Vector3f getAccelBias() const; Vector3f getGyroBias() const; - + Quatf getQuaternion() const; + Eulerf getEulerAngles() const; + matrix::Vector getState() const; + matrix::Vector getQuaternionVariance() const; + Vector3f getPositionVariance() const; + Vector3f getVelocityVariance() const; private: std::shared_ptr _ekf; diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp index 4134156a36..77ad62a3de 100644 --- a/test/sensor_simulator/sensor_simulator.cpp +++ b/test/sensor_simulator/sensor_simulator.cpp @@ -74,3 +74,15 @@ void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias) _imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias, Vector3f{0.0f,0.0f,0.0f} + gyro_bias); } + +void SensorSimulator::simulateOrientation(Quatf orientation) +{ + const Vector3f world_sensed_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G}; + const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f}; + const Dcmf R_bodyToWorld(orientation); + const Vector3f sensed_gravity_body = R_bodyToWorld.transpose() * world_sensed_gravity; + const Vector3f body_mag_field = R_bodyToWorld.transpose() * world_mag_field; + + _imu.setData(sensed_gravity_body, Vector3f{0.0f,0.0f,0.0f}); + _mag.setData(body_mag_field); +} diff --git a/test/sensor_simulator/sensor_simulator.h b/test/sensor_simulator/sensor_simulator.h index 8078c8791c..0242e9c3ea 100644 --- a/test/sensor_simulator/sensor_simulator.h +++ b/test/sensor_simulator/sensor_simulator.h @@ -83,6 +83,7 @@ public: void stopRangeFinder(){ _rng.stop(); } void setImuBias(Vector3f accel_bias, Vector3f gyro_bias); + void simulateOrientation(Quatf orientation); Imu _imu; Mag _mag; diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp index 3743456b7a..ac36b61fa5 100644 --- a/test/test_EKF_basics.cpp +++ b/test/test_EKF_basics.cpp @@ -38,9 +38,9 @@ #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" -class EkfInitializationTest : public ::testing::Test { +class EkfBasicsTest : public ::testing::Test { public: - EkfInitializationTest(): ::testing::Test(), + EkfBasicsTest(): ::testing::Test(), _ekf{std::make_shared()}, _sensor_simulator(_ekf), _ekf_wrapper(_ekf) {}; @@ -67,14 +67,14 @@ class EkfInitializationTest : public ::testing::Test { }; -TEST_F(EkfInitializationTest, tiltAlign) +TEST_F(EkfBasicsTest, tiltAlign) { // GIVEN: reasonable static sensor data for some duration // THEN: EKF should tilt align EXPECT_TRUE(_ekf->attitude_valid()); } -TEST_F(EkfInitializationTest, initialControlMode) +TEST_F(EkfBasicsTest, initialControlMode) { // GIVEN: reasonable static sensor data for some duration // THEN: EKF control status should be reasonable @@ -108,7 +108,7 @@ TEST_F(EkfInitializationTest, initialControlMode) EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z); } -TEST_F(EkfInitializationTest, convergesToZero) +TEST_F(EkfBasicsTest, convergesToZero) { // GIVEN: initialized EKF with default IMU, baro and mag input _sensor_simulator.runSeconds(4); @@ -126,7 +126,7 @@ TEST_F(EkfInitializationTest, convergesToZero) EXPECT_TRUE(matrix::isEqual(gyro_bias, ref, 0.001f)); } -TEST_F(EkfInitializationTest, gpsFusion) +TEST_F(EkfBasicsTest, gpsFusion) { // GIVEN: initialized EKF with default IMU, baro and mag input for // WHEN: setting GPS measurements for 11s, minimum GPS health time is set to 10 sec @@ -164,7 +164,7 @@ TEST_F(EkfInitializationTest, gpsFusion) EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z); } -TEST_F(EkfInitializationTest, accleBiasEstimation) +TEST_F(EkfBasicsTest, accleBiasEstimation) { // GIVEN: initialized EKF with default IMU, baro and mag input for 2s // WHEN: Added more sensor measurements with accel bias and gps measurements diff --git a/test/test_EKF_initialization.cpp b/test/test_EKF_initialization.cpp new file mode 100644 index 0000000000..f53bf2d8ae --- /dev/null +++ b/test/test_EKF_initialization.cpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + +class EkfInitializationTest : public ::testing::Test { + public: + EkfInitializationTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + const float _init_tilt_period = 0.5; // seconds + + // GTests is calling this + void SetUp() override + { + _ekf->init(0); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } + + void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion) + { + Quatf quat_est = _ekf_wrapper.getQuaternion(); + EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion)); + } + + void validStateAfterOrientationInitialization() + { + quaternionVarianceBigEnoughAfterOrientationInitialization(); + velocityAndPositionCloseToZero(); + velocityAndPositionVarianceBigEnoughAfterOrientationInitialization(); + } + + void quaternionVarianceBigEnoughAfterOrientationInitialization() + { + const matrix::Vector quat_variance = _ekf_wrapper.getQuaternionVariance(); + const float quat_variance_limit = 0.0001f; + EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1)" << quat_variance(1); + EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2)" << quat_variance(2); + EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3)" << quat_variance(3); + } + + void velocityAndPositionCloseToZero() + { + Vector3f pos = _ekf_wrapper.getPosition(); + Vector3f vel = _ekf_wrapper.getVelocity(); + + EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f)); + EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.001f)); + } + + void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization() + { + Vector3f pos_var = _ekf_wrapper.getPositionVariance(); + Vector3f vel_var = _ekf_wrapper.getVelocityVariance(); + + const float pos_variance_limit = 0.2f; + EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(1)" << pos_var(0); + EXPECT_TRUE(pos_var(1) > pos_variance_limit) << "pos_var(2)" << pos_var(1); + EXPECT_TRUE(pos_var(2) > pos_variance_limit) << "pos_var(3)" << pos_var(2); + + const float vel_variance_limit = 0.4f; + EXPECT_TRUE(vel_var(0) > vel_variance_limit) << "vel_var(1)" << vel_var(0); + EXPECT_TRUE(vel_var(1) > vel_variance_limit) << "vel_var(2)" << vel_var(1); + EXPECT_TRUE(vel_var(2) > vel_variance_limit) << "vel_var(3)" << vel_var(2); + } +}; + +TEST_F(EkfInitializationTest, initializeWithZeroTilt) +{ + const float pitch = math::radians(0.0f); + const float roll = math::radians(0.0f); + const Eulerf euler_angles_sim(roll, pitch, 0.0f); + const Quatf quat_sim(euler_angles_sim); + + _sensor_simulator.simulateOrientation(quat_sim); + _sensor_simulator.runSeconds(_init_tilt_period); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + validStateAfterOrientationInitialization(); +} + +TEST_F(EkfInitializationTest, initializeHeadingWithZeroTilt) +{ + const float pitch = math::radians(0.0f); + const float roll = math::radians(0.0f); + const float yaw = math::radians(90.0f); + const Eulerf euler_angles_sim(roll, pitch, yaw); + const Quatf quat_sim(euler_angles_sim); + + _sensor_simulator.simulateOrientation(quat_sim); + _sensor_simulator.runSeconds(_init_tilt_period); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + validStateAfterOrientationInitialization(); +} + +TEST_F(EkfInitializationTest, initializeWithTilt) +{ + const float pitch = math::radians(30.0f); + const float roll = math::radians(60.0f); + const Eulerf euler_angles_sim(roll, pitch, 0.0f); + const Quatf quat_sim(euler_angles_sim); + + _sensor_simulator.simulateOrientation(quat_sim); + _sensor_simulator.runSeconds(_init_tilt_period); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + validStateAfterOrientationInitialization(); +} + +TEST_F(EkfInitializationTest, initializeWithPitch90) +{ + const Quatf quat_sim(0.0f, 0.7071068f, 0.0f, 0.7071068f); + + _sensor_simulator.simulateOrientation(quat_sim); + _sensor_simulator.runSeconds(_init_tilt_period); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + // TODO: Quaternion Variance is smaller in this case than in the other cases + validStateAfterOrientationInitialization(); +} + +TEST_F(EkfInitializationTest, initializeWithRoll90) +{ + const Quatf quat_sim(0.7071068f, 0.7071068f, 0.0f, 0.0f); + + _sensor_simulator.simulateOrientation(quat_sim); + _sensor_simulator.runSeconds(_init_tilt_period); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + validStateAfterOrientationInitialization(); +}