dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensor_simulator: update EKF at IMU rate 

This is how it is also done in ekf2_main. Otherwise, this leads to
multiple integration of the same IMU data due to asynchronous sensor
updates triggering a prediction step between IMU updates.

Fix unit tests that broke because of this fix
This commit is contained in:
bresch 2020-06-03 11:09:18 +02:00 committed by Mathieu Bresciani
parent 59183f70ba
commit 6126c190b2
8 changed files with 60 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
test/sensor_simulator/imu.cpp
View File

@ -15,11 +15,12 @@ Imu::~Imu()
void Imu::send(uint64_t time)
{
const float dt = float((time - _time_last_data_sent) * 1.e-6f);
imuSample imu_sample{};
imu_sample.time_us = time;
imu_sample.delta_ang_dt = (time - _time_last_data_sent) * 1.e-6f;
imu_sample.delta_ang_dt = dt;
imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt;
imu_sample.delta_vel_dt = (time - _time_last_data_sent) * 1.e-6f;
imu_sample.delta_vel_dt = dt;
imu_sample.delta_vel = _accel_data * imu_sample.delta_vel_dt;
_ekf->setIMUData(imu_sample);

6
test/sensor_simulator/sensor_simulator.cpp
View File

@ -146,9 +146,13 @@ void SensorSimulator::runMicroseconds(uint32_t duration)
for(;_time < start_time + (uint64_t)duration; _time+=1000)
{
bool update_imu = _imu.should_send(_time);
updateSensors();
_ekf->update();
if (update_imu) {
// Update at IMU rate
_ekf->update();
}
}
}

4
test/test_EKF_airspeed.cpp
View File

@ -61,7 +61,7 @@ class EkfAirspeedTest : public ::testing::Test {
{
_ekf->init(0);
_sensor_simulator.simulateOrientation(_quat_sim);
_sensor_simulator.runSeconds(2);
_sensor_simulator.runSeconds(7);
}
// Use this method to clean up any memory, network etc. after each test
@ -113,6 +113,6 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
const float expected_height_after_pressure_correction = height_before_pressure_correction -
expected_height_difference;
EXPECT_NEAR(height_after_pressure_correction, expected_height_after_pressure_correction, 1e-5f);
EXPECT_NEAR(height_after_pressure_correction, expected_height_after_pressure_correction, 1e-3f);
}

25
test/test_EKF_basics.cpp
View File

@ -51,7 +51,7 @@ class EkfBasicsTest : public ::testing::Test {
// Duration of initalization with only providing baro,mag and IMU
const uint32_t _init_duration_s{3};
const uint32_t _init_duration_s{7};
// Setup the Ekf with synthetic measurements
void SetUp() override
@ -164,27 +164,32 @@ TEST_F(EkfBasicsTest, gpsFusion)
EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z);
}
TEST_F(EkfBasicsTest, accleBiasEstimation)
TEST_F(EkfBasicsTest, accelBiasEstimation)
{
// GIVEN: initialized EKF with default IMU, baro and mag input for 3s
// GIVEN: initialized EKF with default IMU, baro and mag input
// WHEN: Added more sensor measurements with accel bias and gps measurements
const Vector3f accel_bias_sim = {0.0f,0.0f,0.1f};
_sensor_simulator.startGps();
_sensor_simulator.setImuBias(accel_bias_sim, Vector3f{0.0f,0.0f,0.0f});
_sensor_simulator.runSeconds(10);
_sensor_simulator.setImuBias(accel_bias_sim, Vector3f(0.0f,0.0f,0.0f));
_ekf->set_min_required_gps_health_time(1e6);
_sensor_simulator.runSeconds(30);
const Vector3f pos = _ekf->getPosition();
const Vector3f vel = _ekf->getVelocity();
const Vector3f accel_bias = _ekf->getAccelBias();
const Vector3f gyro_bias = _ekf->getGyroBias();
const Vector3f zero{0.0f, 0.0f, 0.0f};
const Vector3f zero = {0.0f, 0.0f, 0.0f};
// THEN: EKF should stay or converge to zero
EXPECT_TRUE(matrix::isEqual(pos, zero, 0.01f));
EXPECT_TRUE(matrix::isEqual(vel, zero, 0.005f));
EXPECT_TRUE(matrix::isEqual(accel_bias, accel_bias_sim, 0.001f));
EXPECT_TRUE(matrix::isEqual(gyro_bias, zero, 0.001f));
EXPECT_TRUE(matrix::isEqual(pos, zero, 0.05f))
<< "pos = " << pos(0) << ", " << pos(1) << ", " << pos(2);
EXPECT_TRUE(matrix::isEqual(vel, zero, 0.02f))
<< "vel = " << vel(0) << ", " << vel(1) << ", " << vel(2);
EXPECT_TRUE(matrix::isEqual(accel_bias, accel_bias_sim, 0.01f))
<< "accel_bias = " << accel_bias(0) << ", " << accel_bias(1) << ", " << accel_bias(2);
EXPECT_TRUE(matrix::isEqual(gyro_bias, zero, 0.001f))
<< "gyro_bias = " << gyro_bias(0) << ", " << gyro_bias(1) << ", " << gyro_bias(2);
}
// TODO: Add sampling tests

20
test/test_EKF_externalVision.cpp
View File

@ -55,6 +55,8 @@ class EkfExternalVisionTest : public ::testing::Test {
SensorSimulator _sensor_simulator;
EkfWrapper _ekf_wrapper;
static constexpr float _tilt_align_time = 7.f;
// Setup the Ekf with synthetic measurements
void SetUp() override
{
@ -69,7 +71,7 @@ class EkfExternalVisionTest : public ::testing::Test {
TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
{
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time); // Let the tilt align
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(2);
@ -104,7 +106,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
TEST_F(EkfExternalVisionTest, visionVelocityReset)
{
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
@ -130,7 +132,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset)
TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
{
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
ResetLoggingChecker reset_logging_checker(_ekf);
reset_logging_checker.capturePreResetState();
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
@ -166,7 +168,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
{
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
_sensor_simulator._vio.setPosition(simulated_position);
@ -181,7 +183,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
{
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
// Heading of drone in EKF frame is 0°
@ -206,7 +208,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
TEST_F(EkfExternalVisionTest, visionVarianceCheck)
{
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
const Vector3f velVar_init = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
@ -221,7 +223,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck)
TEST_F(EkfExternalVisionTest, visionAlignment)
{
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
// GIVEN: Drone is pointing north, and we use mag (ROTATE_EV)
// Heading of drone in EKF frame is 0°
@ -256,7 +258,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody)
// GIVEN: Drone is turned 90 degrees
const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
_sensor_simulator.simulateOrientation(quat_sim);
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
// Without any measurement x and y velocity variance are close
const Vector3f velVar_init = _ekf->getVelocityVariance();
@ -291,7 +293,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
// GIVEN: Drone is turned 90 degrees
const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
_sensor_simulator.simulateOrientation(quat_sim);
_sensor_simulator.runSeconds(3);
_sensor_simulator.runSeconds(_tilt_align_time);
// Without any measurement x and y velocity variance are close
const Vector3f velVar_init = _ekf->getVelocityVariance();

14
test/test_EKF_flow.cpp
View File

@ -59,7 +59,7 @@ class EkfFlowTest : public ::testing::Test {
void SetUp() override
{
_ekf->init(0);
_sensor_simulator.runSeconds(2);
_sensor_simulator.runSeconds(7);
}
// Use this method to clean up any memory, network etc. after each test
@ -78,7 +78,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
_sensor_simulator._rng.setLimits(0.1f, 9.f);
_sensor_simulator.startRangeFinder();
_ekf->set_in_air_status(true);
_sensor_simulator.runSeconds(1.5f);
_sensor_simulator.runSeconds(5.f);
const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground);
@ -93,12 +93,18 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
_sensor_simulator._flow.setData(flow_sample);
_ekf_wrapper.enableFlowFusion();
const float max_flow_rate = 5.f;
const float min_ground_distance = 0.f;
const float max_ground_distance = 50.f;
_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
_sensor_simulator.startFlow();
_sensor_simulator.runSeconds(0.2);
_sensor_simulator.runSeconds(0.12); // Let it reset but not fuse more measurements
// THEN: estimated velocity should match simulated velocity
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
EXPECT_FALSE(isEqual(estimated_horz_velocity, simulated_horz_velocity)); // TODO: This needs to change
EXPECT_FALSE(isEqual(estimated_horz_velocity, simulated_horz_velocity))
<< "estimated vel = " << estimated_horz_velocity(0) << ", "
<< estimated_horz_velocity(1); // TODO: This needs to change (reset is always 0)
// AND: the reset in velocity should be saved correctly
reset_logging_checker.capturePostResetState();

2
test/test_EKF_fusionLogic.cpp
View File

@ -58,7 +58,7 @@ class EkfFusionLogicTest : public ::testing::Test {
void SetUp() override
{
_ekf->init(0);
_sensor_simulator.runSeconds(2);
_sensor_simulator.runSeconds(7);
}
// Use this method to clean up any memory, network etc. after each test

18
test/test_EKF_initialization.cpp
View File

@ -65,7 +65,12 @@ class EkfInitializationTest : public ::testing::Test {
void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion)
{
const Quatf quat_est = _ekf->getQuaternion();
EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion));
const float precision = 0.0002f; // TODO: this is only required for the pitch90 test to pass
EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion, precision))
<< "quat est = " << quat_est(0) << ", " << quat_est(1) << ", "
<< quat_est(2) << ", " << quat_est(3)
<< "\nquat true = " << true_quaternion(0) << ", " << true_quaternion(1) << ", "
<< true_quaternion(2) << ", " << true_quaternion(3);
}
void validStateAfterOrientationInitialization()
@ -89,8 +94,10 @@ class EkfInitializationTest : public ::testing::Test {
const Vector3f pos = _ekf->getPosition();
const Vector3f vel = _ekf->getVelocity();
EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f));
EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.001f));
EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f))
<< "pos = " << pos(0) << ", " << pos(1) << ", " << pos(2);
EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.002f))
<< "vel = " << vel(0) << ", " << vel(1) << ", " << vel(2);
}
void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization()
@ -118,7 +125,7 @@ class EkfInitializationTest : public ::testing::Test {
for (int i = 0; i < 3; i++){
if (fabsf(R_to_earth(2, i)) > 0.8f) {
// Highly observable, the variance decreases
EXPECT_LT(dvel_bias_var(i), 2.0e-6f) << "axis " << i;
EXPECT_LT(dvel_bias_var(i), 4.0e-6f) << "axis " << i;
} else {
// Poorly observable, the variance is set to 0
@ -192,7 +199,8 @@ TEST_F(EkfInitializationTest, initializeWithPitch90)
_sensor_simulator.runSeconds(_init_tilt_period);
initializedOrienationIsMatchingGroundTruth(quat_sim);
// TODO: Quaternion Variance is smaller in this case than in the other cases
// TODO: Quaternion Variance is smaller and vel x is larger
// in this case than in the other cases
validStateAfterOrientationInitialization();
_sensor_simulator.runSeconds(1.f);