From 340a2caa8ee42c4742a8266c92da0f457081b1ee Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 21 Oct 2021 14:10:54 +0200 Subject: [PATCH] ekf2_test: use motion_planning library The VelocitySmoothing class from the motion_planning library is used to generate trajectories in order to test the EKF convergence during motion --- .../ekf2/test/sensor_simulator/CMakeLists.txt | 2 +- .../sensor_simulator/sensor_simulator.cpp | 90 ++++++++++++++++++- .../test/sensor_simulator/sensor_simulator.h | 9 ++ src/modules/ekf2/test/test_EKF_flow.cpp | 58 ++++++------ 4 files changed, 126 insertions(+), 33 deletions(-) diff --git a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt index efdb9a60bf..58bdcee733 100644 --- a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt +++ b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt @@ -48,4 +48,4 @@ set(SRCS ) add_library(ecl_sensor_sim ${SRCS}) -target_link_libraries(ecl_sensor_sim ecl_EKF) +target_link_libraries(ecl_sensor_sim ecl_EKF motion_planning) diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp index 86d7379850..ae0d2e4c95 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp @@ -285,6 +285,89 @@ void SensorSimulator::setSingleReplaySample(const sensor_info &sample) } } +void SensorSimulator::setTrajectoryTargetVelocity(const Vector3f &velocity_target) +{ + for (int i = 0; i < 3; i++) { + _trajectory[i].updateDurations(velocity_target(i)); + } + + VelocitySmoothing::timeSynchronization(_trajectory, 3); +} + +void SensorSimulator::runTrajectorySeconds(float duration_seconds) +{ + runTrajectoryMicroseconds(uint32_t(duration_seconds * 1e6f)); +} + +void SensorSimulator::runTrajectoryMicroseconds(uint32_t duration) +{ + // simulate in 1000us steps + const uint64_t start_time = _time; + + for (; _time < start_time + duration; _time += 1000) { + + for (int i = 0; i < 3; i++) { + _trajectory[i].updateTraj(1e-3f); + } + + setSensorDataFromTrajectory(); + + bool update_imu = _imu.should_send(_time); + updateSensors(); + + if (update_imu) { + _ekf->update(); + } + } +} + +void SensorSimulator::setSensorDataFromTrajectory() +{ + const Vector3f accel_world{_trajectory[0].getCurrentAcceleration(), + _trajectory[1].getCurrentAcceleration(), + _trajectory[2].getCurrentAcceleration()}; + const Vector3f vel_world{_trajectory[0].getCurrentVelocity(), + _trajectory[1].getCurrentVelocity(), + _trajectory[2].getCurrentVelocity()}; + + // IMU + const Vector3f earth_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G}; + const Dcmf R_world_to_body = _R_body_to_world.transpose(); + const Vector3f specific_force = R_world_to_body * (accel_world + earth_gravity); + const Vector3f gyro{}; + + _imu.setData(specific_force, gyro); + + // Magnetometer + if (_mag.isRunning()) { + const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f}; + const Vector3f mag_field_body = R_world_to_body * world_mag_field; + _mag.setData(mag_field_body); + } + + // Baro + /* if (_baro.isRunning()) { */ + /* _baro.setData(..); */ + /* } */ + + // Range finder + const float distance_to_ground = -_trajectory[2].getCurrentPosition() / _R_body_to_world(2, 2); + + if (_rng.isRunning()) { + _rng.setData(distance_to_ground, -1); + } + + // Optical flow + if (_flow.isRunning()) { + flowSample flow_sample = _flow.dataAtRest(); + const Vector3f vel_body = R_world_to_body * vel_world; + flow_sample.flow_xy_rad = + Vector2f(vel_body(1) * flow_sample.dt / distance_to_ground, + -vel_body(0) * flow_sample.dt / distance_to_ground); + _flow.setData(flow_sample); + } +} + void SensorSimulator::setGpsLatitude(const double latitude) { int32_t lat = static_cast(latitude * 1e7); @@ -311,11 +394,12 @@ void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias) void SensorSimulator::simulateOrientation(Quatf orientation) { + _R_body_to_world = Dcmf(orientation); + const Vector3f world_sensed_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G}; const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f}; - const Dcmf R_bodyToWorld(orientation); - const Vector3f sensed_gravity_body = R_bodyToWorld.transpose() * world_sensed_gravity; - const Vector3f body_mag_field = R_bodyToWorld.transpose() * world_mag_field; + const Vector3f sensed_gravity_body = _R_body_to_world.transpose() * world_sensed_gravity; + const Vector3f body_mag_field = _R_body_to_world.transpose() * world_mag_field; _imu.setData(sensed_gravity_body, Vector3f{0.0f, 0.0f, 0.0f}); _mag.setData(body_mag_field); diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h index aaf6cb65e2..b639a91af3 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h @@ -49,6 +49,7 @@ #include #include #include +#include #include "imu.h" #include "mag.h" @@ -83,6 +84,10 @@ public: void runReplaySeconds(float duration_seconds); void runReplayMicroseconds(uint32_t duration); + void setTrajectoryTargetVelocity(const Vector3f &velocity_target); + void runTrajectorySeconds(float duration_seconds); + void runTrajectoryMicroseconds(uint32_t duration); + void startBaro() { _baro.start(); } void stopBaro() { _baro.stop(); } @@ -120,11 +125,14 @@ public: RangeFinder _rng; Vio _vio; + VelocitySmoothing _trajectory[3]; + private: void setSensorDataToDefault(); void setSensorDataFromReplayData(); void setSensorRateToDefault(); void setSingleReplaySample(const sensor_info &sample); + void setSensorDataFromTrajectory(); void startBasicSensor(); void updateSensors(); @@ -137,5 +145,6 @@ private: uint64_t _current_replay_data_index{0}; uint64_t _time{0}; // microseconds + Dcmf _R_body_to_world{}; }; #endif // !EKF_SENSOR_SIMULATOR_H diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 2532c60620..a3e0ca8a53 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -108,6 +108,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) // WHEN: simulate being 5m above ground const float simulated_distance_to_ground = 5.f; + _sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground); startRangeFinderFusion(simulated_distance_to_ground); _ekf->set_in_air_status(true); @@ -119,22 +120,23 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) reset_logging_checker.capturePreResetState(); // WHEN: start fusing flow data - const Vector2f simulated_horz_velocity(0.5f, -0.2f); - flowSample flow_sample = _sensor_simulator._flow.dataAtRest(); - setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground); - _sensor_simulator._flow.setData(flow_sample); + const Vector3f simulated_velocity(0.5f, -0.2f, 0.f); + _sensor_simulator._trajectory[0].setCurrentVelocity(simulated_velocity(0)); + _sensor_simulator._trajectory[1].setCurrentVelocity(simulated_velocity(1)); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); + // Let it reset but not fuse more measurements. We actually need to send 2 // samples to get a reset because the first one cannot be used as the gyro // compensation needs to be accumulated between two samples. - _sensor_simulator.runSeconds(0.14); + _sensor_simulator.runTrajectorySeconds(0.14); // THEN: estimated velocity should match simulated velocity - const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); - EXPECT_TRUE(isEqual(estimated_horz_velocity, simulated_horz_velocity)) - << "estimated vel = " << estimated_horz_velocity(0) << ", " - << estimated_horz_velocity(1); + const Vector3f estimated_velocity = _ekf->getVelocity(); + EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity)) + << "estimated vel = " << estimated_velocity(0) << ", " + << estimated_velocity(1); // AND: the reset in velocity should be saved correctly reset_logging_checker.capturePostResetState(); @@ -178,44 +180,42 @@ TEST_F(EkfFlowTest, inAirConvergence) { // WHEN: simulate being 5m above ground const float simulated_distance_to_ground = 5.f; + _sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground); startRangeFinderFusion(simulated_distance_to_ground); _ekf->set_in_air_status(true); _sensor_simulator.runSeconds(5.f); - const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); - // WHEN: start fusing flow data - Vector2f simulated_horz_velocity(0.5f, -0.2f); - flowSample flow_sample = _sensor_simulator._flow.dataAtRest(); - setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground); - _sensor_simulator._flow.setData(flow_sample); + Vector3f simulated_velocity(0.5f, -0.2f, 0.f); + _sensor_simulator._trajectory[0].setCurrentVelocity(simulated_velocity(0)); + _sensor_simulator._trajectory[1].setCurrentVelocity(simulated_velocity(1)); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); // Let it reset but not fuse more measurements. We actually need to send 2 // samples to get a reset because the first one cannot be used as the gyro // compensation needs to be accumulated between two samples. - _sensor_simulator.runSeconds(0.14); + _sensor_simulator.runTrajectorySeconds(0.14); // THEN: estimated velocity should match simulated velocity - Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); - EXPECT_TRUE(isEqual(estimated_horz_velocity, simulated_horz_velocity)) - << "estimated vel = " << estimated_horz_velocity(0) << ", " - << estimated_horz_velocity(1); + Vector3f estimated_velocity = _ekf->getVelocity(); + EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity)) + << "estimated vel = " << estimated_velocity(0) << ", " + << estimated_velocity(1); // AND: when the velocity changes - simulated_horz_velocity = Vector2f(0.8f, -0.5f); - setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground); - _sensor_simulator._flow.setData(flow_sample); - _sensor_simulator.runSeconds(5.0); + simulated_velocity = Vector3f(1.8f, -1.5f, -0.5f); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); + _sensor_simulator.runTrajectorySeconds(_sensor_simulator._trajectory[0].getTotalTime()); // THEN: estimated velocity should converge to the simulated velocity // This takes a bit of time because the data is inconsistent with IMU measurements - estimated_horz_velocity = Vector2f(_ekf->getVelocity()); - EXPECT_NEAR(estimated_horz_velocity(0), simulated_horz_velocity(0), 0.05f) - << "estimated vel = " << estimated_horz_velocity(0); - EXPECT_NEAR(estimated_horz_velocity(1), simulated_horz_velocity(1), 0.05f) - << estimated_horz_velocity(1); + estimated_velocity = _ekf->getVelocity(); + EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 0.05f) + << "estimated vel = " << estimated_velocity(0); + EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 0.05f) + << estimated_velocity(1); } TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)