flow: restructure optical flow control logic (#928)

- extract motion checks performed on ground
- move all non-timing check to controlOpticalFlowFusion. This simplifies and standardirzes the setOpticalFlowData function. Furthermore, in some cases (SITL, PX4Flow), the dt is forced to 0 when the quality is 0 (which is usual on ground) so the ekf needs to ignore those values on ground to initialize properly the flow fusion.
- add filter convergence test
- move check for dt time max in setOpticalFlowData
- in the simulator, do not update dt as this is the sensor integration
period.

test/sensor_simulator/flow.cpp

@@ -15,7 +15,6 @@ Flow::~Flow()
 
 void Flow::send(uint64_t time)
 {
-	_flow_data.dt = static_cast<float>(time - _time_last_data_sent) * 1e-6f;
 	_flow_data.time_us = time;
 	_ekf->setOpticalFlowData(_flow_data);
 }
@@ -28,12 +27,12 @@ void Flow::setData(const flowSample& flow)
 
 flowSample Flow::dataAtRest()
 {
-	flowSample _flow_at_rest;
-	_flow_at_rest.dt = 0.02f;
-	_flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f};
-	_flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f};
-	_flow_at_rest.quality = 255;
-	return _flow_at_rest;
+	flowSample flow_at_rest;
+	flow_at_rest.dt = static_cast<float>(_update_period) * 1e-6f;
+	flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f};
+	flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f};
+	flow_at_rest.quality = 255;
+	return flow_at_rest;
 }
 
 } // namespace sensor

test/sensor_simulator/sensor_simulator.cpp

@@ -12,8 +12,8 @@ _rng(ekf),
 _vio(ekf),
 _airspeed(ekf)
 {
-	setSensorDataToDefault();
 	setSensorRateToDefault();
+	setSensorDataToDefault();
 	startBasicSensor();
 }
 

test/test_EKF_flow.cpp

@@ -58,6 +58,11 @@ class EkfFlowTest : public ::testing::Test {
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
+		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);
+
 		_ekf->init(0);
 		_sensor_simulator.runSeconds(7);
 	}
@@ -93,18 +98,17 @@ 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.12); // Let it reset but not fuse more measurements
+	 // 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);
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
-	EXPECT_FALSE(isEqual(estimated_horz_velocity, simulated_horz_velocity))
+	EXPECT_TRUE(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)
+		<< estimated_horz_velocity(1);
 
 	// AND: the reset in velocity should be saved correctly
 	reset_logging_checker.capturePostResetState();
@@ -124,13 +128,18 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	reset_logging_checker.capturePreResetState();
 
 	// WHEN: start fusing flow data
+	flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
+	flow_sample.dt = 0.f; // some sensors force dt to zero when quality is low
+	flow_sample.quality = 0;
+	_sensor_simulator._flow.setData(flow_sample);
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
 	_sensor_simulator.runSeconds(1.0);
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
-	EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f)));
+	EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f)))
+	<< estimated_horz_velocity(0) << ", " << estimated_horz_velocity(1);
 
 	// AND: the reset in velocity should be saved correctly
 	reset_logging_checker.capturePostResetState();
@@ -138,3 +147,52 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
 	EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-9f));
 }
+
+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();
+	_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);
+	_sensor_simulator._flow.setData(flow_sample);
+	_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);
+
+	// 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);
+
+	// 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);
+	_sensor_simulator._flow.setData(flow_sample);
+	_sensor_simulator.runSeconds(5.0);
+
+	// 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);
+}

test/test_EKF_fusionLogic.cpp

@@ -164,6 +164,10 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
 	// GIVEN: a tilt and heading aligned filter
 	// WHEN: sending flow data without having the flow fusion enabled
 	//       flow measurement fusion should not be intended.
+	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(4);
 
@@ -198,10 +202,10 @@ TEST_F(EkfFusionLogicTest, doFlowFusion)
 
 	// WHEN: Stop sending flow data
 	_sensor_simulator.stopFlow();
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(11);
 
 	// THEN: EKF should not intend to fuse flow measurements
-	EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); // TODO: change to false
+	EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());
 	// THEN: Local and global position should not be valid
 	EXPECT_FALSE(_ekf->local_position_is_valid());
 	EXPECT_FALSE(_ekf->global_position_is_valid());