From cd9e81539cc17c49fa0f5184b1245381dac3c0c5 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Fri, 3 Jan 2020 14:55:00 +0100 Subject: [PATCH] Expand IMU Down Sampling tests --- EKF/imu_down_sampler.cpp | 1 + test/test_EKF_imuSampling.cpp | 139 +++++++++++++++++++++++++++++----- 2 files changed, 123 insertions(+), 17 deletions(-) diff --git a/EKF/imu_down_sampler.cpp b/EKF/imu_down_sampler.cpp index 4d44cbb080..31880d72c3 100644 --- a/EKF/imu_down_sampler.cpp +++ b/EKF/imu_down_sampler.cpp @@ -13,6 +13,7 @@ ImuDownSampler::~ImuDownSampler() } // integrate imu samples until target dt reached +// assumes that dt of the gyroscope is close to the dt of the accelerometer // returns true if target dt is reached bool ImuDownSampler::update(imuSample imu_sample_new) { diff --git a/test/test_EKF_imuSampling.cpp b/test/test_EKF_imuSampling.cpp index 6e3dc0711d..c20c4048cc 100644 --- a/test/test_EKF_imuSampling.cpp +++ b/test/test_EKF_imuSampling.cpp @@ -34,8 +34,9 @@ #include #include #include "EKF/ekf.h" +#include "EKF/imu_down_sampler.hpp" -class EkfSamplingTestParametrized : public ::testing::TestWithParam> +class EkfImuSamplingTest : public ::testing::TestWithParam> { public: @@ -56,7 +57,7 @@ class EkfSamplingTestParametrized : public ::testing::TestWithParam(GetParam()) * (_ekf.FILTER_UPDATE_PERIOD_MS * 1000); uint32_t expected_dt_us = std::get<1>(GetParam()) * (_ekf.FILTER_UPDATE_PERIOD_MS * 1000); - Vector3f ang_vel{0.0f,0.0f,0.0f}; - Vector3f accel{-0.46f,0.87f,0.0f}; + Vector3f ang_vel= std::get<2>(GetParam()); + Vector3f accel = std::get<3>(GetParam()); imuSample imu_sample; - imu_sample.delta_ang_dt = dt_us * 1.e-6f; + imu_sample.delta_ang_dt = dt_us * 1.0e-6f; imu_sample.delta_ang = ang_vel * imu_sample.delta_ang_dt; - imu_sample.delta_vel_dt = dt_us * 1.e-6f; + imu_sample.delta_vel_dt = dt_us * 1.0e-6f; imu_sample.delta_vel = accel * imu_sample.delta_vel_dt; // The higher the imu rate is the more measurements we have to set before reaching the FILTER_UPDATE_PERIOD @@ -89,19 +90,123 @@ TEST_P(EkfSamplingTestParametrized, imuSamplingAtMultipleRates) // WHEN: downsampling the imu measurement // THEN: the delta vel should be accumulated correctly - EXPECT_NEAR(ang_vel(0), imu_sample_buffered.delta_ang(0)/imu_sample_buffered.delta_ang_dt, 1e-3f); - EXPECT_NEAR(ang_vel(1), imu_sample_buffered.delta_ang(1)/imu_sample_buffered.delta_ang_dt, 1e-3f); - EXPECT_NEAR(ang_vel(2), imu_sample_buffered.delta_ang(2)/imu_sample_buffered.delta_ang_dt, 1e-3f); - EXPECT_NEAR(accel(0), imu_sample_buffered.delta_vel(0)/imu_sample_buffered.delta_vel_dt, 1e-3f); - EXPECT_NEAR(accel(1), imu_sample_buffered.delta_vel(1)/imu_sample_buffered.delta_vel_dt, 1e-3f); - EXPECT_NEAR(accel(2), imu_sample_buffered.delta_vel(2)/imu_sample_buffered.delta_vel_dt, 1e-3f); - + EXPECT_TRUE(matrix::isEqual(ang_vel, imu_sample_buffered.delta_ang/imu_sample_buffered.delta_ang_dt, 1e-7f)); + EXPECT_TRUE(matrix::isEqual(accel, imu_sample_buffered.delta_vel/imu_sample_buffered.delta_vel_dt, 1e-7f)); } INSTANTIATE_TEST_CASE_P(imuSamplingAtMultipleRates, - EkfSamplingTestParametrized, + EkfImuSamplingTest, ::testing::Values( - std::make_tuple(1.0f,1.0f), - std::make_tuple(1.6f,1.6f), - std::make_tuple(0.333f,1.0f) + std::make_tuple(1.0f, 1.0f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}), + std::make_tuple(0.5f, 1.0f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}), + std::make_tuple(1.6f, 1.6f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}), + std::make_tuple(0.333f,1.0f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}), + std::make_tuple(1.0f, 1.0f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(0.5f, 1.0f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(1.6f, 1.6f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(0.333f,1.0f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(1.0f, 1.0f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(0.5f, 1.0f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(1.6f, 1.6f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(0.333f,1.0f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(1.0f, 1.0f,Vector3f{0.0f,0.0f,1.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(0.5f, 1.0f,Vector3f{0.0f,0.0f,1.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(1.6f, 1.6f,Vector3f{0.0f,0.0f,1.0f},Vector3f{0.0f,0.0f,0.0f}), + std::make_tuple(0.333f,1.0f,Vector3f{0.0f,0.0f,1.0f},Vector3f{0.0f,0.0f,0.0f}) )); + +TEST_F(EkfImuSamplingTest, accelDownSampling) +{ + ImuDownSampler sampler(0.008f); + + Vector3f ang_vel{0.0f,0.0f,0.0f}; + Vector3f accel{-0.46f,0.87f,0.0f}; + imuSample input_sample; + input_sample.delta_ang_dt = 0.004f; + input_sample.delta_ang = ang_vel * input_sample.delta_ang_dt; + input_sample.delta_vel_dt = 0.004f; + input_sample.delta_vel = accel * input_sample.delta_vel_dt; + input_sample.time_us = 0; + + // WHEN: adding samples at the double rate as the target rate + EXPECT_FALSE(sampler.update(input_sample)); + input_sample.time_us = 4000; + + // THEN: after two samples a first downsampled sample is ready + EXPECT_TRUE(sampler.update(input_sample)); + + // THEN: downsampled sample should fit to input data + imuSample output_sample = sampler.getDownSampledImuAndTriggerReset(); + EXPECT_FLOAT_EQ(output_sample.delta_ang_dt, 0.008f); + EXPECT_FLOAT_EQ(output_sample.delta_vel_dt, 0.008f); + EXPECT_TRUE(matrix::isEqual(ang_vel * 0.008f, output_sample.delta_ang, 1e-10f)); + EXPECT_TRUE(matrix::isEqual(accel * 0.008f, output_sample.delta_vel, 1e-10f)); +} + +TEST_F(EkfImuSamplingTest, gyroDownSampling) +{ + ImuDownSampler sampler(0.008f); + + Vector3f ang_vel{0.0f,0.0f,1.0f}; + Vector3f accel{0.0f,0.0f,0.0f}; + imuSample input_sample; + + input_sample.delta_ang_dt = 0.004f; + input_sample.delta_ang = ang_vel * input_sample.delta_ang_dt; + input_sample.delta_vel_dt = 0.004f; + input_sample.delta_vel = accel * input_sample.delta_vel_dt; + input_sample.time_us = 0; + + // WHEN: adding samples at the double rate as the target rate + EXPECT_FALSE(sampler.update(input_sample)); + input_sample.time_us += 4000; + + // THEN: after two samples a first downsampled sample is ready + EXPECT_TRUE(sampler.update(input_sample)); + input_sample.time_us += 4000; + + // THEN: downsampled sample should fit to input data + imuSample output_sample = sampler.getDownSampledImuAndTriggerReset(); + EXPECT_FLOAT_EQ(output_sample.delta_ang_dt, 0.008f); + EXPECT_FLOAT_EQ(output_sample.delta_vel_dt, 0.008f); + EXPECT_TRUE(matrix::isEqual(ang_vel * 0.008f, output_sample.delta_ang, 1e-10f)); + EXPECT_TRUE(matrix::isEqual(accel * 0.008f, output_sample.delta_vel, 1e-10f)); + + ang_vel = Vector3f{0.0f,1.0f,0.0f}; + input_sample.delta_ang = ang_vel * input_sample.delta_ang_dt; + input_sample.delta_vel = accel * input_sample.delta_vel_dt; + + // WHEN: adding samples at the double rate as the target rate + EXPECT_FALSE(sampler.update(input_sample)); + input_sample.time_us += 4000; + + // THEN: after two more samples a second downsampled sample is ready + EXPECT_TRUE(sampler.update(input_sample)); + input_sample.time_us += 4000; + + // THEN: downsampled sample should fit the adapted input data + output_sample = sampler.getDownSampledImuAndTriggerReset(); + EXPECT_FLOAT_EQ(output_sample.delta_ang_dt, 0.008f); + EXPECT_FLOAT_EQ(output_sample.delta_vel_dt, 0.008f); + EXPECT_TRUE(matrix::isEqual(ang_vel * 0.008f, output_sample.delta_ang, 1e-10f)); + EXPECT_TRUE(matrix::isEqual(accel * 0.008f, output_sample.delta_vel, 1e-10f)); + + ang_vel = Vector3f{1.0f,0.0f,0.0f}; + input_sample.delta_ang = ang_vel * input_sample.delta_ang_dt; + input_sample.delta_vel = accel * input_sample.delta_vel_dt; + + // WHEN: adding samples at the double rate as the target rate + EXPECT_FALSE(sampler.update(input_sample)); + input_sample.time_us += 4000; + + // THEN: after two more samples a second downsampled sample is ready + EXPECT_TRUE(sampler.update(input_sample)); + input_sample.time_us += 4000; + + // THEN: downsampled sample should fit the adapted input data + output_sample = sampler.getDownSampledImuAndTriggerReset(); + EXPECT_FLOAT_EQ(output_sample.delta_ang_dt, 0.008f); + EXPECT_FLOAT_EQ(output_sample.delta_vel_dt, 0.008f); + EXPECT_TRUE(matrix::isEqual(ang_vel * 0.008f, output_sample.delta_ang, 1e-10f)); + EXPECT_TRUE(matrix::isEqual(accel * 0.008f, output_sample.delta_vel, 1e-10f)); +}