diff --git a/msg/estimator_aid_source_2d.msg b/msg/estimator_aid_source_2d.msg
index 53157c46cc..ada6be79d9 100644
--- a/msg/estimator_aid_source_2d.msg
+++ b/msg/estimator_aid_source_2d.msg
@@ -20,4 +20,4 @@ bool fused                   # true if the sample was successfully fused
 
 # TOPICS estimator_aid_source_2d
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos
-# TOPICS estimator_aid_src_aux_vel
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt
index fee770ddba..a976d00831 100644
--- a/src/modules/ekf2/CMakeLists.txt
+++ b/src/modules/ekf2/CMakeLists.txt
@@ -104,6 +104,7 @@ px4_add_module(
 		EKF/imu_down_sampler.cpp
 		EKF/mag_control.cpp
 		EKF/mag_fusion.cpp
+		EKF/optical_flow_control.cpp
 		EKF/optflow_fusion.cpp
 		EKF/range_finder_consistency_check.cpp
 		EKF/range_height_control.cpp
diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt
index 2a71b85056..06b4b017bc 100644
--- a/src/modules/ekf2/EKF/CMakeLists.txt
+++ b/src/modules/ekf2/EKF/CMakeLists.txt
@@ -55,6 +55,7 @@ add_library(ecl_EKF
 	imu_down_sampler.cpp
 	mag_control.cpp
 	mag_fusion.cpp
+	optical_flow_control.cpp
 	optflow_fusion.cpp
 	range_finder_consistency_check.cpp
 	range_height_control.cpp
diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index 53e8af191a..e89209a611 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -317,7 +317,7 @@ void Ekf::controlExternalVisionFusion()
 				// check if we have been deadreckoning too long
 				if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) {
 					// only reset velocity if we have no another source of aiding constraining it
-					if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
+					if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
 					    isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) {
 
 						if (_control_status.flags.ev_vel) {
@@ -348,7 +348,7 @@ void Ekf::controlExternalVisionFusion()
 			// check if we have been deadreckoning too long
 			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
 				// only reset velocity if we have no another source of aiding constraining it
-				if (isTimedOut(_time_last_of_fuse, (uint64_t)1E6) &&
+				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) &&
 				    isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) {
 					resetVelocityToVision();
 				}
@@ -414,171 +414,6 @@ void Ekf::controlExternalVisionFusion()
 	}
 }
 
-void Ekf::controlOpticalFlowFusion()
-{
-	// Check if on ground motion is un-suitable for use of optical flow
-	if (!_control_status.flags.in_air) {
-		updateOnGroundMotionForOpticalFlowChecks();
-
-	} else {
-		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.7f * _delta_time_of, 0.001f);
-		const float delta_time_max = fminf(1.3f * _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) {
-
-			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;
-			}
-
-			// don't allow invalid flow gyro_xyz to propagate
-			if (!is_body_rate_comp_available) {
-				if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
-					_flow_sample_delayed.gyro_xyz.zero();
-				}
-			}
-
-			// 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_compensated_XY_rad.setZero();
-		}
-	} else {
-		_flow_compensated_XY_rad.setZero();
-	}
-
-	// 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
-		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
-		const bool is_flow_required = _control_status.flags.in_air
-					      && (_control_status.flags.inertial_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
-
-
-		// 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 (!(_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW)
-			    || _inhibit_flow_use) {
-
-				stopFlowFusion();
-				return;
-			}
-		}
-
-		// optical flow fusion mode selection logic
-		if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
-		    && !_control_status.flags.opt_flow // we are not yet using flow data
-		    && !_inhibit_flow_use) {
-			// If the heading is valid and use is not inhibited , start using optical flow aiding
-			if (_control_status.flags.yaw_align || _params.mag_fusion_type == MagFuseType::NONE) {
-				// set the flag and reset the fusion timeout
-				ECL_INFO("starting optical flow fusion");
-				_control_status.flags.opt_flow = true;
-				_time_last_of_fuse = _imu_sample_delayed.time_us;
-
-				// 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) {
-					resetHorizontalVelocityToOpticalFlow();
-					resetHorizontalPositionToOpticalFlow();
-				}
-			}
-		}
-
-		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_pos.xy() = _state.pos.xy();
-				}
-
-				_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)) {
-
-				resetHorizontalVelocityToOpticalFlow();
-				resetHorizontalPositionToOpticalFlow();
-			}
-		}
-
-	} else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) {
-
-		stopFlowFusion();
-	}
-}
-
-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();
-
-	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::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
 {
 	if (!(_params.gnss_ctrl & GnssCtrl::YAW)
@@ -792,5 +627,5 @@ bool Ekf::hasHorizontalAidingTimedOut() const
 {
 	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
 	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
+	       && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
 }
diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
index 5442e9f151..4cc769f253 100644
--- a/src/modules/ekf2/EKF/ekf.cpp
+++ b/src/modules/ekf2/EKF/ekf.cpp
@@ -206,7 +206,6 @@ bool Ekf::initialiseFilter()
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
 	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
 	_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
-	_time_last_of_fuse = _imu_sample_delayed.time_us;
 
 	// reset the output predictor state history to match the EKF initial values
 	alignOutputFilter();
@@ -239,7 +238,7 @@ bool Ekf::initialiseTilt()
 void Ekf::predictState()
 {
 	// apply imu bias corrections
-	const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_ang_dt;
+	const Vector3f delta_ang_bias_scaled = getGyroBias() * _imu_sample_delayed.delta_ang_dt;
 	Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - delta_ang_bias_scaled;
 
 	// subtract component of angular rate due to earth rotation
@@ -252,7 +251,7 @@ void Ekf::predictState()
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// Calculate an earth frame delta velocity
-	const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt;
+	const Vector3f delta_vel_bias_scaled = getAccelBias() * _imu_sample_delayed.delta_vel_dt;
 	const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled;
 	const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
 
@@ -306,7 +305,7 @@ void Ekf::calculateOutputStates(const imuSample &imu)
 	// Use full rate IMU data at the current time horizon
 
 	// correct delta angles for bias offsets
-	const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * imu.delta_ang_dt;
+	const Vector3f delta_ang_bias_scaled = getGyroBias() * imu.delta_ang_dt;
 
 	// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
 	const Vector3f delta_angle(imu.delta_ang - delta_ang_bias_scaled + _delta_angle_corr);
@@ -517,7 +516,7 @@ Quatf Ekf::calculate_quaternion() const
 {
 	// Correct delta angle data for bias errors using bias state estimates from the EKF and also apply
 	// corrections required to track the EKF quaternion states
-	const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _newest_high_rate_imu_sample.delta_ang_dt;
+	const Vector3f delta_ang_bias_scaled = getGyroBias() * _newest_high_rate_imu_sample.delta_ang_dt;
 
 	const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - delta_ang_bias_scaled + _delta_angle_corr};
 
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 767f1db7c8..ebaefdecb8 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -101,9 +101,9 @@ public:
 	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
 	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
 
-	void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); }
-	void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); }
-	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; }
+	void getFlowInnov(float flow_innov[2]) const;
+	void getFlowInnovVar(float flow_innov_var[2]) const;
+	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
 
 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
 	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
@@ -437,6 +437,8 @@ public:
 
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 
+	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
+
 private:
 
 	// set the internal states and status to their default value
@@ -497,7 +499,6 @@ private:
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
 	uint64_t _time_last_heading_fuse{0};
 
-	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 	uint64_t _time_last_healthy_rng_data{0};
@@ -540,8 +541,6 @@ private:
 	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
 
 	// optical flow processing
-	Vector2f _flow_innov{};		///< flow measurement innovation (rad/sec)
-	Vector2f _flow_innov_var{};	///< flow innovation variance ((rad/sec)**2)
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
 	Vector2f _flow_vel_body{};	///< velocity from corrected flow measurement (body frame)(m/s)
 	Vector2f _flow_vel_ne{};		///< velocity from corrected flow measurement (local frame) (m/s)
@@ -550,7 +549,6 @@ private:
 	float _delta_time_of{0.0f};	///< time in sec that _imu_del_ang_of was accumulated over (sec)
 	uint64_t _time_bad_motion_us{0};	///< last system time that on-ground motion exceeded limits (uSec)
 	uint64_t _time_good_motion_us{0};	///< last system time that on-ground motion was within limits (uSec)
-	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
 	estimator_aid_source_1d_s _aid_src_baro_hgt{};
@@ -576,6 +574,8 @@ private:
 
 	estimator_aid_source_2d_s _aid_src_aux_vel{};
 
+	estimator_aid_source_2d_s _aid_src_optical_flow{};
+
 	// output predictor states
 	Vector3f _delta_angle_corr{};	///< delta angle correction vector (rad)
 	Vector3f _vel_err_integ{};	///< integral of velocity tracking error (m)
@@ -708,7 +708,7 @@ private:
 	void resetVerticalVelocityTo(float new_vert_vel);
 
 	void resetVelocityToGps(const gpsSample &gps_sample);
-	void resetHorizontalVelocityToOpticalFlow();
+	void resetHorizontalVelocityToOpticalFlow(const flowSample &flow_sample);
 	void resetVelocityToVision();
 	void resetHorizontalVelocityToZero();
 
@@ -732,6 +732,7 @@ private:
 	void resetVerticalVelocityToZero();
 
 	// fuse optical flow line of sight rate measurements
+	void updateOptFlow(estimator_aid_source_2d_s &aid_src);
 	void fuseOptFlow();
 
 	// horizontal and vertical position aid source
@@ -992,7 +993,7 @@ private:
 	float getRngHeightVariance() const;
 
 	// calculate the measurement variance for the optical flow sensor
-	float calcOptFlowMeasVar();
+	float calcOptFlowMeasVar(const flowSample &flow_sample);
 
 	// rotate quaternion covariances into variances for an equivalent rotation vector
 	Vector3f calcRotVecVariances();
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 18bdd7e010..deb3c205ab 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -52,7 +52,7 @@ void Ekf::resetVelocityToGps(const gpsSample &gps_sample)
 	P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample.sacc));
 }
 
-void Ekf::resetHorizontalVelocityToOpticalFlow()
+void Ekf::resetHorizontalVelocityToOpticalFlow(const flowSample &flow_sample)
 {
 	_information_events.flags.reset_vel_to_flow = true;
 	ECL_INFO("reset velocity to flow");
@@ -66,8 +66,8 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
 		// we should have reliable OF measurements so
 		// calculate X and Y body relative velocities from OF measurements
 		Vector3f vel_optflow_body;
-		vel_optflow_body(0) = - range * _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt;
-		vel_optflow_body(1) =   range * _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt;
+		vel_optflow_body(0) = - range * _flow_compensated_XY_rad(1) / flow_sample.dt;
+		vel_optflow_body(1) =   range * _flow_compensated_XY_rad(0) / flow_sample.dt;
 		vel_optflow_body(2) = 0.0f;
 
 		// rotate from body to earth frame
@@ -80,7 +80,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow()
 	}
 
 	// reset the horizontal velocity variance using the optical flow noise variance
-	P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar());
+	P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar(flow_sample));
 }
 
 void Ekf::resetVelocityToVision()
@@ -172,13 +172,14 @@ void Ekf::resetHorizontalPositionToVision()
 void Ekf::resetHorizontalPositionToOpticalFlow()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
-	ECL_INFO("reset position to last known position");
 
 	if (!_control_status.flags.in_air) {
+		ECL_INFO("reset horizontal position to (0, 0)");
 		// we are likely starting OF for the first time so reset the horizontal position
 		resetHorizontalPositionTo(Vector2f(0.f, 0.f));
 
 	} else {
+		ECL_INFO("reset horizontal position to last known");
 		resetHorizontalPositionTo(_last_known_pos.xy());
 	}
 
@@ -623,6 +624,18 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
 	aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
 }
 
+void Ekf::getFlowInnov(float flow_innov[2]) const
+{
+	flow_innov[0] = _aid_src_optical_flow.innovation[0];
+	flow_innov[1] = _aid_src_optical_flow.innovation[1];
+}
+
+void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
+{
+	flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
+	flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
+}
+
 // get the state vector at the delayed time horizon
 matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 {
@@ -761,7 +774,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm();
+			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
 		}
 
 		if (_control_status.flags.gps) {
@@ -927,7 +940,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	}
 
 	if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
-		float of_vel = sqrtf(_optflow_test_ratio);
+		float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
 		vel = math::max(of_vel, FLT_MIN);
 	}
 
@@ -1040,7 +1053,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
 				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
 
-	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
+	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
 
 	const bool airDataAiding = _control_status.flags.wind &&
 				   isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
@@ -1618,9 +1631,8 @@ void Ekf::stopFlowFusion()
 	if (_control_status.flags.opt_flow) {
 		ECL_INFO("stopping optical flow fusion");
 		_control_status.flags.opt_flow = false;
-		_flow_innov.setZero();
-		_flow_innov_var.setZero();
-		_optflow_test_ratio = 0.0f;
+
+		resetEstimatorAidStatus(_aid_src_optical_flow);
 	}
 }
 
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index e42bba901e..1f84f348c2 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -342,7 +342,6 @@ protected:
 	// innovation consistency check monitoring ratios
 	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 
-	float _optflow_test_ratio{};		// Optical flow innovation consistency check ratio
 	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio
 	Vector2f _drag_test_ratio{};		// drag innovation consistency check ratio
 	innovation_fault_status_u _innov_check_fail_status{};
diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp
index 164ac75ed5..7d508bdd2b 100644
--- a/src/modules/ekf2/EKF/fake_pos_control.cpp
+++ b/src/modules/ekf2/EKF/fake_pos_control.cpp
@@ -79,6 +79,9 @@ void Ekf::controlFakePosFusion()
 				}
 			}
 		}
+
+	} else if (_control_status.flags.fake_pos && isHorizontalAidingActive()) {
+		stopFakePosFusion();
 	}
 }
 
diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp
index 683fb35cd6..90d9960cb7 100644
--- a/src/modules/ekf2/EKF/optflow_fusion.cpp
+++ b/src/modules/ekf2/EKF/optflow_fusion.cpp
@@ -47,23 +47,10 @@
 #include <float.h>
 #include "utils.hpp"
 
-void Ekf::fuseOptFlow()
+void Ekf::updateOptFlow(estimator_aid_source_2d_s &aid_src)
 {
-	float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f);
-
-	// get latest estimated orientation
-	const float q0 = _state.quat_nominal(0);
-	const float q1 = _state.quat_nominal(1);
-	const float q2 = _state.quat_nominal(2);
-	const float q3 = _state.quat_nominal(3);
-
-	// get latest velocity in earth frame
-	const float vn = _state.vel(0);
-	const float ve = _state.vel(1);
-	const float vd = _state.vel(2);
-
-	// calculate the optical flow observation variance
-	const float R_LOS = calcOptFlowMeasVar();
+	resetEstimatorAidStatus(aid_src);
+	aid_src.timestamp_sample = _flow_sample_delayed.time_us;
 
 	// get rotation matrix from earth to body
 	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
@@ -81,35 +68,64 @@ void Ekf::fuseOptFlow()
 	// rotate into body frame
 	const Vector3f vel_body = earth_to_body * vel_rel_earth;
 
-	// height above ground of the IMU
-	float heightAboveGndEst = _terrain_vpos - _state.pos(2);
-
 	// calculate the sensor position relative to the IMU in earth frame
 	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	heightAboveGndEst -= pos_offset_earth(2);
-
-	// constrain minimum height above ground
-	heightAboveGndEst = math::max(heightAboveGndEst, gndclearance);
+	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
 
 	// calculate range from focal point to centre of image
-	const float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view
+	const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
 
 	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
 	// correct for gyro bias errors in the data used to do the motion compensation
 	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
-	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
+	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
 
-	// compute the velocities in body and local frames from corrected optical flow measurement
-	// for logging only
+	// compute the velocities in body and local frames from corrected optical flow measurement for logging only
 	_flow_vel_body(0) = -opt_flow_rate(1) * range;
-	_flow_vel_body(1) = opt_flow_rate(0) * range;
+	_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));
 
-	_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
+	aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
+	aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
+
+	aid_src.innovation[0] =  (vel_body(1) / range) - aid_src.observation[0];
+	aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1];
+
+	// calculate the optical flow observation variance
+	const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
+	aid_src.observation_variance[0] = R_LOS;
+	aid_src.observation_variance[1] = R_LOS;
+}
+
+void Ekf::fuseOptFlow()
+{
+	_aid_src_optical_flow.fusion_enabled = true;
+
+	const float R_LOS = _aid_src_optical_flow.observation_variance[0];
+
+	// get latest estimated orientation
+	const float q0 = _state.quat_nominal(0);
+	const float q1 = _state.quat_nominal(1);
+	const float q2 = _state.quat_nominal(2);
+	const float q3 = _state.quat_nominal(3);
+
+	// get latest velocity in earth frame
+	const float vn = _state.vel(0);
+	const float ve = _state.vel(1);
+	const float vd = _state.vel(2);
+
+	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
+	// a positive offset in earth frame leads to a smaller height above the ground.
+	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
+
+	// calculate range from focal point to centre of image
+	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
+	const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
 
 	// 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
@@ -170,14 +186,14 @@ void Ekf::fuseOptFlow()
 	// const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
 
 	// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
-	_flow_innov_var(0) = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
+	_aid_src_optical_flow.innovation_variance[0] = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
 
-	if (_flow_innov_var(0) < R_LOS) {
+	if (_aid_src_optical_flow.innovation_variance[0] < R_LOS) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
 		return;
 	}
-	const float HK50 = HK4/_flow_innov_var(0);
+	const float HK50 = HK4 / _aid_src_optical_flow.innovation_variance[0];
 
 	const float HK51 = Tbs(0,1)*q1;
 	const float HK52 = Tbs(0,2)*q0;
@@ -226,110 +242,102 @@ void Ekf::fuseOptFlow()
 	// const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
 
 	// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
-	_flow_innov_var(1) = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
-	if (_flow_innov_var(1) < R_LOS) {
+	_aid_src_optical_flow.innovation_variance[1] = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
+	if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
 		return;
 	}
-	const float HK95 = HK4/_flow_innov_var(1);
 
+	const float HK95 = HK4 / _aid_src_optical_flow.innovation_variance[1];
 
 	// run the innovation consistency check and record result
-	bool all_innovation_checks_passed = true;
-	float test_ratio[2];
-	test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
-	test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
-	_optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]);
+	setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
 
-	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
-		const bool innov_check_fail = (test_ratio[obs_index] > 1.0f);
-
-		if (innov_check_fail) {
-			all_innovation_checks_passed = false;
-		}
-
-		if (obs_index == 0) {
-			_innov_check_fail_status.flags.reject_optflow_X = innov_check_fail;
-
-		} else {
-			_innov_check_fail_status.flags.reject_optflow_Y = innov_check_fail;
-		}
-	}
+	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
+	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
 
 	// if either axis fails we abort the fusion
-	if (!all_innovation_checks_passed) {
+	if (_aid_src_optical_flow.innovation_rejected) {
 		return;
-
 	}
 
+	bool fused[2] {false, false};
+
 	// fuse observation axes sequentially
-	SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
-	Vector24f Kfusion; // Optical flow Kalman gains
+	{
+		// Optical flow observation Jacobians - axis 0
+		SparseVector24f<0,1,2,3,4,5,6> Hfusion;
+		Hfusion.at<0>() = HK3*HK5;
+		Hfusion.at<1>() = HK5*HK7;
+		Hfusion.at<2>() = HK5*HK8;
+		Hfusion.at<3>() = HK5*HK9;
+		Hfusion.at<4>() = HK25*HK4;
+		Hfusion.at<5>() = HK33*HK4;
+		Hfusion.at<6>() = HK37*HK4;
 
-	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
+		// Optical flow Kalman gains - axis 0
+		Vector24f Kfusion;
+		Kfusion(0) = HK42*HK50;
+		Kfusion(1) = HK49*HK50;
+		Kfusion(2) = HK47*HK50;
+		Kfusion(3) = HK48*HK50;
+		Kfusion(4) = HK46*HK50;
+		Kfusion(5) = HK45*HK50;
+		Kfusion(6) = HK44*HK50;
 
-		// calculate observation Jocobians and Kalman gains
-		if (obs_index == 0) {
-			// Observation Jacobians - axis 0
-			Hfusion.at<0>() = HK3*HK5;
-			Hfusion.at<1>() = HK5*HK7;
-			Hfusion.at<2>() = HK5*HK8;
-			Hfusion.at<3>() = HK5*HK9;
-			Hfusion.at<4>() = HK25*HK4;
-			Hfusion.at<5>() = HK33*HK4;
-			Hfusion.at<6>() = HK37*HK4;
+		for (unsigned row = 7; row <= 23; row++) {
+			Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
+		}
 
-			// Kalman gains - axis 0
-			Kfusion(0) = HK42*HK50;
-			Kfusion(1) = HK49*HK50;
-			Kfusion(2) = HK47*HK50;
-			Kfusion(3) = HK48*HK50;
-			Kfusion(4) = HK46*HK50;
-			Kfusion(5) = HK45*HK50;
-			Kfusion(6) = HK44*HK50;
-
-			for (unsigned row = 7; row <= 23; row++) {
-				Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
-			}
+		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) {
+			fused[0] = true;
+			_fault_status.flags.bad_optflow_X = false;
 
 		} else {
-			// Observation Jacobians - axis 1
-			Hfusion.at<0>() = -HK5*HK63;
-			Hfusion.at<1>() = -HK5*HK66;
-			Hfusion.at<2>() = -HK5*HK68;
-			Hfusion.at<3>() = -HK5*HK70;
-			Hfusion.at<4>() = -HK4*HK74;
-			Hfusion.at<5>() = -HK4*HK77;
-			Hfusion.at<6>() = -HK4*HK79;
+			_fault_status.flags.bad_optflow_X = true;
+			return;
+		}
+	}
 
-			// Kalman gains - axis 1
-			Kfusion(0) = -HK87*HK95;
-			Kfusion(1) = -HK94*HK95;
-			Kfusion(2) = -HK91*HK95;
-			Kfusion(3) = -HK93*HK95;
-			Kfusion(4) = -HK90*HK95;
-			Kfusion(5) = -HK89*HK95;
-			Kfusion(6) = -HK88*HK95;
+	{
+		// Optical flow observation Jacobians - axis 1
+		SparseVector24f<0,1,2,3,4,5,6> Hfusion;
+		Hfusion.at<0>() = -HK5*HK63;
+		Hfusion.at<1>() = -HK5*HK66;
+		Hfusion.at<2>() = -HK5*HK68;
+		Hfusion.at<3>() = -HK5*HK70;
+		Hfusion.at<4>() = -HK4*HK74;
+		Hfusion.at<5>() = -HK4*HK77;
+		Hfusion.at<6>() = -HK4*HK79;
 
-			for (unsigned row = 7; row <= 23; row++) {
-				Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
-			}
+		// Optical flow Kalman gains - axis 1
+		Vector24f Kfusion;
+		Kfusion(0) = -HK87*HK95;
+		Kfusion(1) = -HK94*HK95;
+		Kfusion(2) = -HK91*HK95;
+		Kfusion(3) = -HK93*HK95;
+		Kfusion(4) = -HK90*HK95;
+		Kfusion(5) = -HK89*HK95;
+		Kfusion(6) = -HK88*HK95;
 
+		for (unsigned row = 7; row <= 23; row++) {
+			Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
 		}
 
-		const bool is_fused = measurementUpdate(Kfusion, Hfusion, _flow_innov(obs_index));
+		if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[1])) {
+			fused[1] = true;
+			_fault_status.flags.bad_optflow_Y = false;
 
-		if (obs_index == 0) {
-			_fault_status.flags.bad_optflow_X = !is_fused;
-
-		} else if (obs_index == 1) {
-			_fault_status.flags.bad_optflow_Y = !is_fused;
+		} else {
+			_fault_status.flags.bad_optflow_Y = true;
+			return;
 		}
+	}
 
-		if (is_fused) {
-			_time_last_of_fuse = _imu_sample_delayed.time_us;
-		}
+	if (fused[0] && fused[1]) {
+		_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
+		_aid_src_optical_flow.fused = true;
 	}
 }
 
@@ -337,13 +345,6 @@ void Ekf::fuseOptFlow()
 // returns false if bias corrected body rate data is unavailable
 bool Ekf::calcOptFlowBodyRateComp()
 {
-	// reset the accumulators if the time interval is too large
-	if (_delta_time_of > 1.0f) {
-		_imu_del_ang_of.setZero();
-		_delta_time_of = 0.0f;
-		return false;
-	}
-
 	bool is_body_rate_comp_available = false;
 	const bool use_flow_sensor_gyro = PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) && PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2));
 
@@ -362,6 +363,9 @@ bool Ekf::calcOptFlowBodyRateComp()
 			// calculate the bias estimate using  a combined LPF and spike filter
 			_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f;
 
+			// apply gyro bias
+			_flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt);
+
 			is_body_rate_comp_available = true;
 		}
 
@@ -370,6 +374,7 @@ bool Ekf::calcOptFlowBodyRateComp()
 		// for clarification of the sign see definition of flowSample and imuSample in common.h
 		if ((_delta_time_of > FLT_EPSILON)
 		    && (_flow_sample_delayed.dt > FLT_EPSILON)) {
+
 			_flow_sample_delayed.gyro_xyz = -_imu_del_ang_of / _delta_time_of * _flow_sample_delayed.dt;
 			_flow_gyro_bias.zero();
 
@@ -384,25 +389,24 @@ bool Ekf::calcOptFlowBodyRateComp()
 }
 
 // calculate the measurement variance for the optical flow sensor (rad/sec)^2
-float Ekf::calcOptFlowMeasVar()
+float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample)
 {
 	// calculate the observation noise variance - scaling noise linearly across flow quality range
 	const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
 	const float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
 
 	// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
-	float weighting = (255.0f - (float)_params.flow_qual_min);
+	float weighting = (255.f - (float)_params.flow_qual_min);
 
-	if (weighting >= 1.0f) {
-		weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
-					    1.0f);
+	if (weighting >= 1.f) {
+		weighting = math::constrain((float)(flow_sample.quality - _params.flow_qual_min) / weighting, 0.f, 1.f);
 
 	} else {
 		weighting = 0.0f;
 	}
 
 	// take the weighted average of the observation noise for the best and wort flow quality
-	const float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
+	const float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.f - weighting));
 
 	return R_LOS;
 }
diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp
new file mode 100644
index 0000000000..fdd740a6b1
--- /dev/null
+++ b/src/modules/ekf2/EKF/optical_flow_control.cpp
@@ -0,0 +1,230 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file optical_flow_control.cpp
+ * Control functions for optical flow fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlOpticalFlowFusion()
+{
+	// Check if on ground motion is un-suitable for use of optical flow
+	if (!_control_status.flags.in_air) {
+		updateOnGroundMotionForOpticalFlowChecks();
+
+	} else {
+		resetOnGroundMotionForOpticalFlowChecks();
+	}
+
+	// Accumulate autopilot gyro data across the same time interval as the flow sensor
+	const Vector3f delta_angle(_imu_sample_delayed.delta_ang - (getGyroBias() * _imu_sample_delayed.delta_ang_dt));
+	if (_delta_time_of < 0.1f) {
+		_imu_del_ang_of += delta_angle;
+		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
+
+	} else {
+		// reset the accumulators if the time interval is too large
+		_imu_del_ang_of = delta_angle;
+		_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.7f * _delta_time_of, 0.001f);
+		const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f);
+		bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max;
+
+		if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) {
+
+			if (fabsf(_imu_sample_delayed.delta_ang_dt - _flow_sample_delayed.dt) < 0.1f) {
+				// reset accumulators to current IMU
+				_imu_del_ang_of = delta_angle;
+				_delta_time_of = _imu_sample_delayed.delta_ang_dt;
+
+				is_delta_time_good = true;
+			 }
+
+			if (is_quality_good && !is_delta_time_good) {
+				ECL_DEBUG("Optical flow: bad delta time: OF dt %.6f s (min: %.3f, max: %.3f), IMU dt %.6f s",
+					(double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max, (double)_imu_sample_delayed.delta_ang_dt);
+			}
+		}
+
+		const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
+
+		// don't allow invalid flow gyro_xyz to propagate
+		if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
+
+			_flow_sample_delayed.gyro_xyz.zero();
+		}
+
+		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) {
+
+			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_compensated_XY_rad.setZero();
+		}
+
+		updateOptFlow(_aid_src_optical_flow);
+
+	} else {
+		_flow_compensated_XY_rad.setZero();
+	}
+
+	// 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
+		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
+		const bool is_flow_required = _control_status.flags.in_air
+					      && (_control_status.flags.inertial_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
+
+		// 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();
+
+		const bool 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 (!(_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW)
+			    || inhibit_flow_use) {
+
+				stopFlowFusion();
+				return;
+			}
+		}
+
+		// optical flow fusion mode selection logic
+		if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
+		    && !_control_status.flags.opt_flow // we are not yet using flow data
+		    && !inhibit_flow_use) {
+
+			// set the flag and reset the fusion timeout
+			ECL_INFO("starting optical flow fusion");
+
+			// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
+			if (!isHorizontalAidingActive()) {
+				resetHorizontalVelocityToOpticalFlow(_flow_sample_delayed);
+				resetHorizontalPositionToOpticalFlow();
+			}
+
+			_control_status.flags.opt_flow = true;
+			_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
+
+			return;
+		}
+
+		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_pos.xy() = _state.pos.xy();
+				}
+
+				_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(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)
+			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
+
+				resetHorizontalVelocityToOpticalFlow(_flow_sample_delayed);
+				resetHorizontalPositionToOpticalFlow();
+			}
+		}
+
+	} else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) {
+
+		stopFlowFusion();
+	}
+}
+
+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();
+
+	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;
+}
diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp
index 218b993abb..7ee99736e9 100644
--- a/src/modules/ekf2/EKF/terrain_estimator.cpp
+++ b/src/modules/ekf2/EKF/terrain_estimator.cpp
@@ -193,11 +193,15 @@ void Ekf::resetHaglRng()
 
 void Ekf::stopHaglRngFusion()
 {
-	_hagl_sensor_status.flags.range_finder = false;
-	_hagl_innov = 0.f;
-	_hagl_innov_var = 0.f;
-	_hagl_test_ratio = 0.f;
-	_innov_check_fail_status.flags.reject_hagl = false;
+	if (_hagl_sensor_status.flags.range_finder) {
+
+		_hagl_innov = 0.f;
+		_hagl_innov_var = 0.f;
+		_hagl_test_ratio = 0.f;
+		_innov_check_fail_status.flags.reject_hagl = false;
+
+		_hagl_sensor_status.flags.range_finder = false;
+	}
 }
 
 void Ekf::fuseHaglRng()
@@ -277,7 +281,7 @@ void Ekf::controlHaglFlowFusion()
 		}
 
 	} else if (_hagl_sensor_status.flags.flow
-		   && (_imu_sample_delayed.time_us >  _flow_sample_delayed.time_us + (uint64_t)5e6)) {
+		   && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
 		// No data anymore. Stop until it comes back.
 		stopHaglFlowFusion();
 	}
@@ -293,11 +297,10 @@ void Ekf::startHaglFlowFusion()
 
 void Ekf::stopHaglFlowFusion()
 {
-	_hagl_sensor_status.flags.flow = false;
-	_hagl_innov = 0.f;
-	_hagl_innov_var = 0.f;
-	_hagl_test_ratio = 0.f;
-	_innov_check_fail_status.flags.reject_hagl = false;
+	if (_hagl_sensor_status.flags.flow) {
+
+		_hagl_sensor_status.flags.flow = false;
+	}
 }
 
 void Ekf::resetHaglFlow()
@@ -310,10 +313,12 @@ void Ekf::resetHaglFlow()
 
 void Ekf::fuseFlowForTerrain()
 {
+	_aid_src_optical_flow.fusion_enabled = true;
+
 	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
 	// correct for gyro bias errors in the data used to do the motion compensation
 	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
-	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
+	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
 
 	// get latest estimated orientation
 	const float q0 = _state.quat_nominal(0);
@@ -322,7 +327,7 @@ void Ekf::fuseFlowForTerrain()
 	const float q3 = _state.quat_nominal(3);
 
 	// calculate the optical flow observation variance
-	const float R_LOS = calcOptFlowMeasVar();
+	const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
 
 	// get rotation matrix from earth to body
 	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
@@ -340,72 +345,64 @@ void Ekf::fuseFlowForTerrain()
 	// rotate into body frame
 	const Vector3f vel_body = earth_to_body * vel_rel_earth;
 
-	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
-
 	// constrain terrain to minimum allowed value and predict height above ground
 	_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
 	const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2));
 
-	// Calculate observation matrix for flow around the vehicle x axis
-	const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
+	// calculate prediced optical flow
+	const float pred_flow_x =  vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
+	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
+
+	// calculate flow innovation
+	_aid_src_optical_flow.innovation[0] = pred_flow_x - opt_flow_rate(0);
+	_aid_src_optical_flow.innovation[1] = pred_flow_y - opt_flow_rate(1);
+
+	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+
+	// Calculate observation matrix for flow around
+	const float Hx =  vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv;
+	const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
 
 	// Constrain terrain variance to be non-negative
-	_terrain_var = fmaxf(_terrain_var, 0.0f);
+	_terrain_var = fmaxf(_terrain_var, sq(0.01f));
 
 	// Cacluate innovation variance
-	_flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS;
+	_aid_src_optical_flow.innovation_variance[0] = Hx * Hx * _terrain_var + R_LOS;
+	_aid_src_optical_flow.innovation_variance[1] = Hy * Hy * _terrain_var + R_LOS;
 
-	// calculate the kalman gain for the flow x measurement
-	const float Kx = _terrain_var * Hx / _flow_innov_var(0);
+	// run the innovation consistency check and record result
+	setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
 
-	// calculate prediced optical flow about x axis
-	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
+	// do not perform measurement update if badly conditioned
+	if (_aid_src_optical_flow.innovation_rejected) {
+		return;
+	}
 
-	// calculate flow innovation (x axis)
-	_flow_innov(0) = pred_flow_x - opt_flow_rate(0);
+	// calculate the kalman gain for the flow measurement
+	const float Kx = _terrain_var * Hx / _aid_src_optical_flow.innovation_variance[0];
 
 	// calculate correction term for terrain variance
 	const float KxHxP =  Kx * Hx * _terrain_var;
 
-	// innovation consistency check
-	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
-	float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0));
+	_terrain_vpos += Kx * _aid_src_optical_flow.innovation[0];
+	// guard against negative variance
+	_terrain_var = fmaxf(_terrain_var - KxHxP, sq(0.01f));
 
-	// do not perform measurement update if badly conditioned
-	if (flow_test_ratio <= 1.0f) {
-		_terrain_vpos += Kx * _flow_innov(0);
-		// guard against negative variance
-		_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
-		_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
-	}
 
-	// Calculate observation matrix for flow around the vehicle y axis
-	const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
-
-	// Calculuate innovation variance
-	_flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS;
-
-	// calculate the kalman gain for the flow y measurement
-	const float Ky = _terrain_var * Hy / _flow_innov_var(1);
-
-	// calculate prediced optical flow about y axis
-	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
-
-	// calculate flow innovation (y axis)
-	_flow_innov(1) = pred_flow_y - opt_flow_rate(1);
+	// calculate the kalman gain for the flow measurement
+	const float Ky = _terrain_var * Hy / _aid_src_optical_flow.innovation_variance[1];
 
 	// calculate correction term for terrain variance
 	const float KyHyP =  Ky * Hy * _terrain_var;
 
-	// innovation consistency check
-	flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1));
+	_terrain_vpos += Ky * _aid_src_optical_flow.innovation_variance[1];
+	// guard against negative variance
+	_terrain_var = fmaxf(_terrain_var - KyHyP, sq(0.01f));
 
-	if (flow_test_ratio <= 1.0f) {
-		_terrain_vpos += Ky * _flow_innov(1);
-		// guard against negative variance
-		_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
-		_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
-	}
+
+	_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
+	//_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us; // TODO: separate aid source status for OF terrain?
+	_aid_src_optical_flow.fused = true;
 }
 
 void Ekf::controlHaglFakeFusion()
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 4af4f9a00e..2a8b2a86aa 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -747,6 +747,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 
 	// aux velocity
 	PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
+
+	// optical flow
+	PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
 }
 
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -1531,9 +1534,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 
 void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 {
-	if (_ekf.getFlowCompensated().longerThan(0.f)) {
+	const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample;
+
+	if ((timestamp_sample != 0) && (timestamp_sample > _status_optical_flow_pub_last)) {
+
 		vehicle_optical_flow_vel_s flow_vel{};
-		flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
+		flow_vel.timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample;
 
 		_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index a28d24f928..f582d7fb2f 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -285,6 +285,8 @@ private:
 
 	hrt_abstime _status_aux_vel_pub_last{0};
 
+	hrt_abstime _status_optical_flow_pub_last{0};
+
 	float _last_baro_bias_published{};
 	float _last_gnss_hgt_bias_published{};
 	float _last_rng_hgt_bias_published{};
@@ -374,6 +376,8 @@ private:
 
 	uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
 
+	uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
+
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index cb00c1ebb6..8878673c5a 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -191,6 +191,7 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
+	// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
 
 	// log all raw sensors at minimal rate (at least 1 Hz)
@@ -281,6 +282,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES);
 
 #endif /* CONFIG_ARCH_BOARD_PX4_SITL */