flow: add unit test for yaw motion (PX4#950)

NicolasM0 · Dec 25, 2020 · 18f334f · 18f334f
1 parent df7f261
commit 18f334f
Show file tree

Hide file tree

Showing 3 changed files with 110 additions and 15 deletions.
diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp
@@ -95,6 +95,11 @@ bool EkfWrapper::isIntendingFlowFusion() const
 	return _ekf->control_status_flags().opt_flow;
 }
 
+void EkfWrapper::setFlowOffset(const Vector3f &offset)
+{
+	_ekf_params->flow_pos_body = offset;
+}
+
 void EkfWrapper::enableExternalVisionPositionFusion()
 {
 	_ekf_params->fusion_mode |= MASK_USE_EVPOS;

diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h
@@ -71,6 +71,7 @@ class EkfWrapper
 	void enableFlowFusion();
 	void disableFlowFusion();
 	bool isIntendingFlowFusion() const;
+	void setFlowOffset(const matrix::Vector3f &offset);
 
 	void enableExternalVisionPositionFusion();
 	void disableExternalVisionPositionFusion();

diff --git a/test/test_EKF_flow.cpp b/test/test_EKF_flow.cpp
@@ -71,18 +71,43 @@ class EkfFlowTest : public ::testing::Test {
 	void TearDown() override
 	{
 	}
+
+	void startRangeFinderFusion(float distance);
+	void startZeroFlowFusion();
+	void setFlowFromHorizontalVelocityAndDistance(flowSample &flow_sample, const Vector2f &simulated_horz_velocity, float estimated_distance_to_ground);
 };
 
+void EkfFlowTest::startRangeFinderFusion(float distance)
+{
+	_sensor_simulator._rng.setData(distance, 100);
+	_sensor_simulator._rng.setLimits(0.1f, 9.f);
+	_sensor_simulator.startRangeFinder();
+}
+
+void EkfFlowTest::startZeroFlowFusion()
+{
+	// Start fusing zero flow data
+	_sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest());
+	_ekf_wrapper.enableFlowFusion();
+	_sensor_simulator.startFlow();
+}
+
+void EkfFlowTest::setFlowFromHorizontalVelocityAndDistance(flowSample &flow_sample, const Vector2f &simulated_horz_velocity, float estimated_distance_to_ground)
+{
+	flow_sample.flow_xy_rad =
+		Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
+			 -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
+}
+
 TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
 {
 	ResetLoggingChecker reset_logging_checker(_ekf);
 
 	// WHEN: simulate being 5m above ground
 	const float simulated_distance_to_ground = 5.f;
-	_sensor_simulator._rng.setData(simulated_distance_to_ground, 100);
-	_sensor_simulator._rng.setLimits(0.1f, 9.f);
-	_sensor_simulator.startRangeFinder();
+	startRangeFinderFusion(simulated_distance_to_ground);
 	_ekf->set_in_air_status(true);
+
 	_sensor_simulator.runSeconds(5.f);
 
 	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
@@ -93,9 +118,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
 	// WHEN: start fusing flow data
 	const Vector2f simulated_horz_velocity(0.5f, -0.2f);
 	flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
-	flow_sample.flow_xy_rad =
-		Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-			 -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
+	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground);
 	_sensor_simulator._flow.setData(flow_sample);
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
@@ -152,20 +175,17 @@ TEST_F(EkfFlowTest, inAirConvergence)
 {
 	// WHEN: simulate being 5m above ground
 	const float simulated_distance_to_ground = 5.f;
-	_sensor_simulator._rng.setData(simulated_distance_to_ground, 100);
-	_sensor_simulator._rng.setLimits(0.1f, 9.f);
-	_sensor_simulator.startRangeFinder();
+	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();
-	flow_sample.flow_xy_rad =
-		Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-			 -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
+	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground);
 	_sensor_simulator._flow.setData(flow_sample);
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
@@ -182,9 +202,7 @@ TEST_F(EkfFlowTest, inAirConvergence)
 
 	// AND: when the velocity changes
 	simulated_horz_velocity = Vector2f(0.8f, -0.5f);
-	flow_sample.flow_xy_rad =
-		Vector2f( simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-			 -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
+	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, estimated_distance_to_ground);
 	_sensor_simulator._flow.setData(flow_sample);
 	_sensor_simulator.runSeconds(5.0);
 
@@ -196,3 +214,74 @@ TEST_F(EkfFlowTest, inAirConvergence)
 	EXPECT_NEAR(estimated_horz_velocity(1), simulated_horz_velocity(1), 0.05f)
 		<< estimated_horz_velocity(1);
 }
+
+TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
+{
+	// WHEN: fusing range finder and optical flow data in air
+	const float simulated_distance_to_ground = 5.f;
+	startRangeFinderFusion(simulated_distance_to_ground);
+	startZeroFlowFusion();
+	_ekf->set_in_air_status(true);
+
+	_sensor_simulator.runSeconds(5.f);
+
+	// AND WHEN: there is a pure yaw rotation
+	const Vector3f body_rate(0.f, 0.f, 3.14159f);
+	const Vector3f flow_offset(0.15, -0.05f, 0.2f);
+	_ekf_wrapper.setFlowOffset(flow_offset);
+
+	const Vector2f simulated_horz_velocity(body_rate % flow_offset);
+	flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
+	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
+
+	// use autopilot gyro data
+	flow_sample.gyro_xyz.setAll(NAN);
+
+	_sensor_simulator._flow.setData(flow_sample);
+	_sensor_simulator._imu.setGyroData(body_rate);
+	_sensor_simulator.runSeconds(10.f);
+
+	// THEN: the flow due to the yaw rotation and the offsets is canceled
+	// and the velocity estimate stays 0
+	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
+	EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
+		<< "estimated vel = " << estimated_horz_velocity(0);
+	EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
+		<< "estimated vel = " << estimated_horz_velocity(1);
+}
+
+TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
+{
+	// WHEN: fusing range finder and optical flow data in air
+	const float simulated_distance_to_ground = 5.f;
+	startRangeFinderFusion(simulated_distance_to_ground);
+	startZeroFlowFusion();
+	_ekf->set_in_air_status(true);
+
+	_sensor_simulator.runSeconds(5.f);
+
+	// AND WHEN: there is a pure yaw rotation
+	const Vector3f body_rate(0.f, 0.f, 3.14159f);
+	const Vector3f flow_offset(-0.15, 0.05f, 0.2f);
+	_ekf_wrapper.setFlowOffset(flow_offset);
+
+	const Vector2f simulated_horz_velocity(body_rate % flow_offset);
+	flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
+	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
+
+	// use flow sensor gyro data
+	// for clarification of the sign, see definition of flowSample
+	flow_sample.gyro_xyz = -body_rate * flow_sample.dt;
+
+	_sensor_simulator._flow.setData(flow_sample);
+	_sensor_simulator._imu.setGyroData(body_rate);
+	_sensor_simulator.runSeconds(10.f);
+
+	// THEN: the flow due to the yaw rotation and the offsets is canceled
+	// and the velocity estimate stays 0
+	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
+	EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
+		<< "estimated vel = " << estimated_horz_velocity(0);
+	EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
+		<< "estimated vel = " << estimated_horz_velocity(1);
+}