From 29cf7884c35b94fc3a981d9bf3ab230243867766 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 14 Jan 2020 15:02:06 +0100 Subject: [PATCH] Test baro downsampling --- test/CMakeLists.txt | 1 + test/test_EKF_measurementSampling.cpp | 78 +++++++++++++++++++ test/test_EKF_sampling.cpp | 107 -------------------------- 3 files changed, 79 insertions(+), 107 deletions(-) create mode 100644 test/test_EKF_measurementSampling.cpp delete mode 100644 test/test_EKF_sampling.cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 78e1237fb5..783da7873f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -39,6 +39,7 @@ set(SRCS main.cpp test_EKF_basics.cpp test_EKF_ringbuffer.cpp + test_EKF_measurementSampling.cpp test_EKF_imuSampling.cpp test_AlphaFilter.cpp test_EKF_fusionLogic.cpp diff --git a/test/test_EKF_measurementSampling.cpp b/test/test_EKF_measurementSampling.cpp new file mode 100644 index 0000000000..4e37a0315d --- /dev/null +++ b/test/test_EKF_measurementSampling.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 "EKF/ekf.h" + +class EkfMeasurementSamplingTest : public ::testing::Test +{ + public: + EkfMeasurementSamplingTest(): ::testing::Test(), + _ekf{std::make_shared()}{}; + + std::shared_ptr _ekf; + + void SetUp() override + { + _ekf->init(0); + } +}; + +TEST_F(EkfMeasurementSamplingTest, baroDownSampling) +{ + // Need to send imu samples to initialise + imuSample imu_sample; + + // WHEN: baro data = {1, -1 , 1, -1, ...} being set at high rate + int baro_rate_Hz = 10000; + uint64_t time = 0; + float baro_data = 1.0f; + for(int i = 0; i < 2 * baro_rate_Hz; i++) + { + if(i % 100 == 0) + { + // send imu data at a 100 times lower rate + imu_sample.time_us = time; + _ekf->setIMUData(imu_sample); + } + _ekf->setBaroData(time, baro_data); + baro_data *= -1.0f; + time += 1000000 / baro_rate_Hz; + } + _ekf->update(); + + // THEN: average and buffered baro dato should be close to zero + baroSample baro_sample_from_buffer = _ekf->get_baro_sample_delayed(); + EXPECT_NEAR(baro_sample_from_buffer.hgt, 0.0f, 0.01f); +} diff --git a/test/test_EKF_sampling.cpp b/test/test_EKF_sampling.cpp deleted file mode 100644 index a007070f4a..0000000000 --- a/test/test_EKF_sampling.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** - * - * 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 "EKF/ekf.h" - -class EkfImuSamplingTestParametrized : public ::testing::TestWithParam> -{ - public: - - Ekf _ekf{}; - - uint32_t _t_us{0}; - - // Setup the Ekf with synthetic measurements - void SetUp() override - { - _ekf.init(0); - - } - - void TearDown() override - { - - } -}; - -TEST_P(EkfImuSamplingTestParametrized, imuSamplingAtMultipleRates) -{ - // WHEN: adding imu samples at a higher rate than the update loop - // THEN: imu sample should be down sampled - // WHEN: adding imu samples at a same or lower rate than the update loop - // THEN: imu sample should reach buffer unchanged - - uint32_t dt_us = std::get<0>(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}; - imuSample imu_sample; - imu_sample.delta_ang_dt = dt_us * 1.e-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 = 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 - for(int i = 0; i<(int)20/std::get<0>(GetParam()); ++i) - { - imu_sample.time_us = _t_us; - _ekf.setIMUData(imu_sample); - _t_us += dt_us; - } - - // Get the imu sample that was put into the buffer - imuSample imu_sample_buffered = _ekf.get_imu_sample_delayed(); - EXPECT_NEAR(expected_dt_us / 1e6f, imu_sample_buffered.delta_ang_dt, 1e-5f); - EXPECT_NEAR(expected_dt_us / 1e6f, imu_sample_buffered.delta_vel_dt, 1e-5f); - - // 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); - -} - -INSTANTIATE_TEST_CASE_P(imuSamplingAtMultipleRates, - EkfImuSamplingTestParametrized, - ::testing::Values( - std::make_tuple(1.0f,1.0f), - std::make_tuple(1.6f,1.6f), - std::make_tuple(0.333f,1.0f) - ));