EKF: Fix half frame offset in flow gyro compensation.

The gyro data accumulation needs to be across the same integration period as the flow sensor. The previous code didn't sample the accumulation until the midpoint of the flow data had fallen behind the fusion time horizon.

This PR changes the optical flow time stamp definition so that flow data is retrieved when the leading edge of the flow accumulation period falls behind the fusion time horizon. This enables the accumulated gyro data to be sampled at the correct time. Fusion is then delayed until the mid sample time has fallen behind the fusion time horizon.

EKF/common.h

@@ -141,7 +141,7 @@ struct flowSample {
 	Vector2f flowRadXY;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
 	Vector3f gyroXYZ;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
 	float    dt;		///< amount of integration time (sec)
-	uint64_t time_us;	///< timestamp of the integration period mid-point (uSec)
+	uint64_t time_us;	///< timestamp of the integration period leading edge (uSec)
 };
 
 struct extVisionSample {

EKF/control.cpp

@@ -113,8 +113,13 @@ void Ekf::controlFusionModes()
 
 	checkForStuckRange();
 
-	_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
-			   && (_R_to_earth(2, 2) > 0.7071f);
+	// We don't fuse flow data immediately becasue we have to wait for the mid integration point to fall behind the fusion time horizon.
+	// This means we stop looking for new data until the old data has been fused.
+	if (!_flow_data_ready) {
+		_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
+				   && (_R_to_earth(2, 2) > 0.7071f);
+	}
+
 	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
 	_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
 
@@ -358,7 +363,11 @@ void Ekf::controlOpticalFlowFusion()
 		_time_good_motion_us = _imu_sample_delayed.time_us;
 	}
 
-	// New optical flow data has fallen behind the fusion time horizon and is ready to be fused
+	// Accumulate autopilot gyro data across the same time interval as the flow sensor
+	_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
+	_delta_time_of += _imu_sample_delayed.delta_ang_dt;
+
+	// 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
 		// Apply hysteresis to prevent rapid mode switching
@@ -404,10 +413,6 @@ void Ekf::controlOpticalFlowFusion()
 			}
 		}
 
-		// Accumulate autopilot gyro data across the same time interval as the flow sensor
-		_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
-		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
-
 		// 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
@@ -463,23 +468,29 @@ void Ekf::controlOpticalFlowFusion()
 			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
 				_flowRadXYcomp.zero();
-
 			} else {
 				// compensate for body motion to give a LOS rate
 				_flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0);
 				_flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1);
 			}
-
-
-			// Fuse optical flow LOS rate observations into the main filter 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 && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
-				fuseOptFlow();
-				_last_known_posNE(0) = _state.pos(0);
-				_last_known_posNE(1) = _state.pos(1);
-			}
+		} else {
+			// don't use this flow data and wait for the next data to arrive
+			_flow_data_ready = false;
 		}
 	}
 
+	// 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 && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
+			fuseOptFlow();
+			_last_known_posNE(0) = _state.pos(0);
+			_last_known_posNE(1) = _state.pos(1);
+		}
+
+		_flow_data_ready = false;
+	}
 }
 
 void Ekf::controlGpsFusion()

EKF/ekf.h

@@ -282,7 +282,7 @@ private:
 	bool _mag_data_ready{false};	///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused
 	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
 	bool _range_data_ready{false};	///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
-	bool _flow_data_ready{false};	///< true when new optical flow data has fallen behind the fusion time horizon and is available to be fused
+	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 

EKF/estimator_interface.cpp

@@ -396,8 +396,8 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 		// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
 		if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) {
 			flowSample optflow_sample_new;
-			// calculate the system time-stamp for the mid point of the integration period
-			optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;
+			// calculate the system time-stamp for the leading edge of the flow data integration period
+			optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000;
 
 			// copy the quality metric returned by the PX4Flow sensor
 			optflow_sample_new.quality = flow->quality;