fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 20:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Expand IMU Down Sampling tests

Browse Source
This commit is contained in:
kamilritz
2020-01-03 14:55:00 +01:00
committed by Paul Riseborough
parent de58ab9167
commit cd9e81539c
2 changed files with 123 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/imu_down_sampler.cpp
+1
View File
@@ -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)
{
test/test_EKF_imuSampling.cpp
+122 -17
View File
@@ -34,8 +34,9 @@
#include <gtest/gtest.h>
#include <math.h>
#include "EKF/ekf.h"
#include "EKF/imu_down_sampler.hpp"
class EkfSamplingTestParametrized : public ::testing::TestWithParam<std::tuple<float,float>>
class EkfImuSamplingTest : public ::testing::TestWithParam<std::tuple<float,float,Vector3f, Vector3f>>
{
public:
@@ -56,7 +57,7 @@ class EkfSamplingTestParametrized : public ::testing::TestWithParam<std::tuple<f
}
};
TEST_P(EkfSamplingTestParametrized, imuSamplingAtMultipleRates)
TEST_P(EkfImuSamplingTest, imuSamplingAtMultipleRates)
{
// WHEN: adding imu samples at a higher rate than the update loop
// THEN: imu sample should be down sampled
@@ -66,12 +67,12 @@ TEST_P(EkfSamplingTestParametrized, imuSamplingAtMultipleRates)
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};
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<float,float>(1.0f,1.0f),
std::make_tuple<float,float>(1.6f,1.6f),
std::make_tuple<float,float>(0.333f,1.0f)
std::make_tuple<float,float,Vector3f,Vector3f>(1.0f, 1.0f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}),
std::make_tuple<float,float,Vector3f,Vector3f>(0.5f, 1.0f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}),
std::make_tuple<float,float,Vector3f,Vector3f>(1.6f, 1.6f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}),
std::make_tuple<float,float,Vector3f,Vector3f>(0.333f,1.0f,Vector3f{0.0f,0.0f,0.0f},Vector3f{-0.46f,0.87f,0.20f}),
std::make_tuple<float,float,Vector3f,Vector3f>(1.0f, 1.0f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(0.5f, 1.0f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(1.6f, 1.6f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(0.333f,1.0f,Vector3f{1.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(1.0f, 1.0f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(0.5f, 1.0f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(1.6f, 1.6f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(0.333f,1.0f,Vector3f{0.0f,1.0f,0.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(1.0f, 1.0f,Vector3f{0.0f,0.0f,1.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(0.5f, 1.0f,Vector3f{0.0f,0.0f,1.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(1.6f, 1.6f,Vector3f{0.0f,0.0f,1.0f},Vector3f{0.0f,0.0f,0.0f}),
std::make_tuple<float,float,Vector3f,Vector3f>(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));
}