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.

EKF/control.cpp

@@ -144,12 +144,13 @@ void Ekf::controlFusionModes()
 	// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
 	// in this case we need to empty the buffer
 	if (!_flow_data_ready || !_control_status.flags.opt_flow) {
-		_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
-				   && (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
+		_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed);
 	}
 
 	// check if we should fuse flow data for terrain estimation
 	if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
+		// TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion
+		// due to some checks failing
 		// only fuse flow for terrain if range data hasn't been fused for 5 seconds
 		_flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6);
 		// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
@@ -343,33 +344,56 @@ void Ekf::controlExternalVisionFusion()
 
 void Ekf::controlOpticalFlowFusion()
 {
-	// TODO: These motion checks run all the time. Pull them out of this function
 	// Check if on ground motion is un-suitable for use of optical flow
 	if (!_control_status.flags.in_air) {
-		// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
-		const float accel_norm = _accel_vec_filt.norm();
-
-		const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
-					    || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
-					    || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
-					    || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
-
-		if (motion_is_excessive) {
-			_time_bad_motion_us = _imu_sample_delayed.time_us;
-
-		} else {
-			_time_good_motion_us = _imu_sample_delayed.time_us;
-		}
+		updateOnGroundMotionForOpticalFlowChecks();
 
 	} else {
-		_time_bad_motion_us = 0;
-		_time_good_motion_us = _imu_sample_delayed.time_us;
+		resetOnGroundMotionForOpticalFlowChecks();
 	}
 
 	// Accumulate autopilot gyro data across the same time interval as the flow sensor
 	_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
 	_delta_time_of += _imu_sample_delayed.delta_ang_dt;
 
+	if (_flow_data_ready) {
+		const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
+		const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate);
+		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
+
+		const float delta_time_min = fmaxf(0.8f * _delta_time_of, 0.001f);
+		const float delta_time_max = fminf(1.2f * _delta_time_of, 0.2f);
+		const bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min
+		                                && _flow_sample_delayed.dt <= delta_time_max;
+		const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
+
+		if (is_quality_good
+		    && is_magnitude_good
+		    && is_tilt_good
+		    && is_body_rate_comp_available
+		    && is_delta_time_good) {
+			// compensate for body motion to give a LOS rate
+			_flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
+
+		} else if (!_control_status.flags.in_air && is_body_rate_comp_available) {
+
+			if (!is_delta_time_good) {
+				// handle special case of SITL and PX4Flow where dt is forced to
+				// zero when the quaity is 0
+				_flow_sample_delayed.dt = delta_time_min;
+			}
+
+			// when on the ground with poor flow quality,
+			// assume zero ground relative velocity and LOS rate
+			_flow_compensated_XY_rad.setZero();
+
+		} else {
+			// don't use this flow data and wait for the next data to arrive
+			_flow_data_ready = false;
+			_flow_for_terrain_data_ready = false; // TODO: find a better place
+		}
+	}
+
 	// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
 	if (_flow_data_ready) {
 		// Inhibit flow use if motion is un-suitable or we have good quality GPS
@@ -377,47 +401,38 @@ void Ekf::controlOpticalFlowFusion()
 		const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
 
 		// Check if we are in-air and require optical flow to control position drift
-		bool flow_required = _control_status.flags.in_air &&
-				(_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
-				  || (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow))
-				  || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
+		const bool is_flow_required = _control_status.flags.in_air
+		                              && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
+		                                  || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
+		                                  || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
 
-		if (!_inhibit_flow_use && _control_status.flags.opt_flow) {
-			// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
-			bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5);
-			bool flight_motion_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
-			if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) {
-				_inhibit_flow_use = true;
-			}
-		} else if (_inhibit_flow_use && !_control_status.flags.opt_flow){
-			// allow use of optical flow if motion is suitable or we are reliant on it for flight navigation
-			bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6);
-			bool flight_motion_ok = _control_status.flags.in_air && isRangeAidSuitable();
-			if (preflight_motion_ok || flight_motion_ok || flow_required) {
-				_inhibit_flow_use = false;
-			}
-		}
 
-		// Handle cases where we are using optical flow but are no longer able to because data is old
-		// or its use has been inhibited.
+		// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
+		const bool preflight_motion_not_ok = !_control_status.flags.in_air
+		                                     && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
+		                                         || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
+		const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
+
+		_inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
+		                    || !_control_status.flags.tilt_align;
+
+		// Handle cases where we are using optical flow but we should not use it anymore
 		if (_control_status.flags.opt_flow) {
-			if (_inhibit_flow_use) {
-				stopFlowFusion();
-				_time_last_of_fuse = 0;
+			if (!(_params.fusion_mode & MASK_USE_OF)
+			    || _inhibit_flow_use) {
 
-			} else if (isTimedOut(_time_last_of_fuse, (uint64_t)_params.reset_timeout_max)) {
 				stopFlowFusion();
+				return;
 			}
 		}
 
 		// optical flow fusion mode selection logic
 		if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
 			&& !_control_status.flags.opt_flow // we are not yet using flow data
-			&& _control_status.flags.tilt_align // we know our tilt attitude
-			&& !_inhibit_flow_use
-			&& isTerrainEstimateValid())
+			&& !_inhibit_flow_use)
 		{
 			// If the heading is not aligned, reset the yaw and magnetic field states
+			// TODO: ekf2 should always try to align itself if not already aligned
 			if (!_control_status.flags.yaw_align) {
 				_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
 			}
@@ -430,61 +445,65 @@ void Ekf::controlOpticalFlowFusion()
 
 				// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
 				const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
+
 				if (flow_aid_only) {
-					// TODO: Issue: First time we want to reset the optical flow velocity
-					// we will use _flow_compensated_XY_rad in the resetVelocity() function,
-					// but _flow_compensated_XY_rad is this zero as it gets updated for the first time below here.
 					resetVelocity();
 					resetHorizontalPosition();
 				}
 			}
-
-		} else if (!(_params.fusion_mode & MASK_USE_OF)) {
-			_control_status.flags.opt_flow = false;
 		}
 
-		// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
-		if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
+		if (_control_status.flags.opt_flow) {
+			// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
+			if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
+				// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
+				// but use a relaxed time criteria to enable it to coast through bad range finder data
+				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
+					fuseOptFlow();
+					_last_known_posNE = _state.pos.xy();
+				}
 
-			bool do_reset = isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
+				_flow_data_ready = false;
+			}
+
+			// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
+			if (isTimedOut(_time_last_of_fuse, _params.reset_timeout_max)
+			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
 
-			if (do_reset) {
 				resetVelocity();
 				resetHorizontalPosition();
 			}
 		}
 
-		// Only fuse optical flow if valid body rate compensation data is available
-		if (calcOptFlowBodyRateComp()) {
-
-			bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
-
-			if (!flow_quality_good && !_control_status.flags.in_air) {
-				// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
-				_flow_compensated_XY_rad.setZero();
-			} else {
-				// compensate for body motion to give a LOS rate
-				_flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
-			}
-		} else {
-			// don't use this flow data and wait for the next data to arrive
-			_flow_data_ready = false;
-		}
+	} else if (_control_status.flags.opt_flow && (_imu_sample_delayed.time_us >  _flow_sample_delayed.time_us + (uint64_t)10e6)) {
+		stopFlowFusion();
 	}
+}
 
-	// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
-	if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
-		// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
-		// but use a relaxed time criteria to enable it to coast through bad range finder data
-		if (_control_status.flags.opt_flow && isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
-			fuseOptFlow();
-			_last_known_posNE = _state.pos.xy();
-		}
+void Ekf::updateOnGroundMotionForOpticalFlowChecks()
+{
+	// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
+	const float accel_norm = _accel_vec_filt.norm();
 
-		_flow_data_ready = false;
+	const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
+				    || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
+				    || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
+				    || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
+
+	if (motion_is_excessive) {
+		_time_bad_motion_us = _imu_sample_delayed.time_us;
+
+	} else {
+		_time_good_motion_us = _imu_sample_delayed.time_us;
 	}
 }
 
+void Ekf::resetOnGroundMotionForOpticalFlowChecks()
+{
+	_time_bad_motion_us = 0;
+	_time_good_motion_us = _imu_sample_delayed.time_us;
+}
+
 void Ekf::controlGpsFusion()
 {
 	// Check for new GPS data that has fallen behind the fusion time horizon

EKF/ekf.h

@@ -753,6 +753,8 @@ private:
 
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion();
+	void updateOnGroundMotionForOpticalFlowChecks();
+	void resetOnGroundMotionForOpticalFlowChecks();
 
 	// control fusion of GPS observations
 	void controlGpsFusion();

EKF/ekf_helper.cpp

@@ -79,7 +79,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
 	// calculate absolute distance from focal point to centre of frame assuming a flat earth
 	const float range = heightAboveGndEst / _range_sensor.getCosTilt();
 
-	if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
+	if ((range - _params.rng_gnd_clearance) > 0.3f) {
 		// we should have reliable OF measurements so
 		// calculate X and Y body relative velocities from OF measurements
 		Vector3f vel_optflow_body;

EKF/estimator_interface.cpp

@@ -354,47 +354,14 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 
 	// limit data rate to prevent data being lost
 	if ((flow.time_us - _time_last_optflow) > _min_obs_interval_us) {
-		// check if enough integration time and fail if integration time is less than 50%
-		// of min arrival interval because too much data is being lost
-		float delta_time = flow.dt; // in seconds
-		const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us;
-		const bool delta_time_good = delta_time >= delta_time_min;
+		_time_last_optflow = flow.time_us;
 
-		bool flow_magnitude_good = true;
+		flowSample optflow_sample_new = flow;
 
-		if (delta_time_good) {
-			// check magnitude is within sensor limits
-			// use this to prevent use of a saturated flow sensor
-			// when there are other aiding sources available
-			const float flow_rate_magnitude = flow.flow_xy_rad.norm() / delta_time;
-			flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
+		optflow_sample_new.time_us -= _params.flow_delay_ms * 1000;
+		optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
-		} else {
-			// protect against overflow caused by division with very small delta_time
-			delta_time = delta_time_min;
-		}
-
-		const bool relying_on_flow = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow);
-
-		const bool flow_quality_good = (flow.quality >= _params.flow_qual_min);
-
-		// Check data validity and write to buffers
-		// Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion()
-		bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow);
-
-		if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) {
-
-			_time_last_optflow = flow.time_us;
-
-			flowSample optflow_sample_new = flow;
-
-			optflow_sample_new.time_us -= _params.flow_delay_ms * 1000;
-			optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
-
-			optflow_sample_new.dt = delta_time;
-
-			_flow_buffer.push(optflow_sample_new);
-		}
+		_flow_buffer.push(optflow_sample_new);
 	}
 }
 

EKF/optflow_fusion.cpp

@@ -108,13 +108,8 @@ void Ekf::fuseOptFlow()
 	_flow_vel_body(1) = opt_flow_rate(0) * range;
 	_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
 
-	if (opt_flow_rate.norm() < _flow_max_rate) {
-		_flow_innov(0) =  vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
-		_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
-
-	} else {
-		return;
-	}
+	_flow_innov(0) =  vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
+	_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
 
 	// The derivation allows for an arbitrary body to flow sensor frame rotation which is
 	// currently not supported by the EKF, so assume sensor frame is aligned with the
@@ -350,7 +345,9 @@ bool Ekf::calcOptFlowBodyRateComp()
 
 		// if accumulation time differences are not excessive and accumulation time is adequate
 		// compare the optical flow and and navigation rate data and calculate a bias error
-		if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
+		if ((_delta_time_of > FLT_EPSILON)
+		    && (_flow_sample_delayed.dt > FLT_EPSILON)
+		    && (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f)) {
 
 			const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of));
 

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