Expand IMU Down Sampling tests

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)
 {

test/test_EKF_imuSampling.cpp

@@ -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));
+}