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

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)

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<int32_t>(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);

src/modules/ekf2/test/sensor_simulator/sensor_simulator.h

@@ -49,6 +49,7 @@
 #include <sstream>
 #include <vector>
 #include <array>
+#include <motion_planning/VelocitySmoothing.hpp>
 
 #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

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)