diff --git a/msg/VehicleOpticalFlowVel.msg b/msg/VehicleOpticalFlowVel.msg
index d92806803a..947131da4d 100644
--- a/msg/VehicleOpticalFlowVel.msg
+++ b/msg/VehicleOpticalFlowVel.msg
@@ -4,14 +4,12 @@ uint64 timestamp_sample                # the timestamp of the raw data (microsec
 float32[2] vel_body                    # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
 float32[2] vel_ne                      # same as vel_body but in local frame (m/s)
 
-float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad)
-float32[2] flow_compensated_integral   # integrated optical flow measurement compensated for angular motion (rad)
+float32[2] flow_rate_uncompensated     # integrated optical flow measurement (rad/s)
+float32[2] flow_rate_compensated       # integrated optical flow measurement compensated for angular motion (rad/s)
 
 float32[3] gyro_rate                   # gyro measurement synchronized with flow measurements (rad/s)
-float32[3] gyro_rate_integral          # gyro measurement integrated to flow rate and synchronized with flow measurements (rad)
 
 float32[3] gyro_bias
 float32[3] ref_gyro
-float32[3] meas_gyro
 
 # TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index a6b20b3978..aba810ceb4 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -225,11 +225,10 @@ struct airspeedSample {
 };
 
 struct flowSample {
-	uint64_t    time_us{};     ///< timestamp of the integration period leading edge (uSec)
-	Vector2f    flow_xy_rad{}; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
-	Vector3f    gyro_xyz{};    ///< 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)
-	uint8_t     quality{};     ///< quality indicator between 0 and 255
+	uint64_t    time_us{};   ///< timestamp of the integration period midpoint (uSec)
+	Vector2f    flow_rate{}; ///< measured angular rate of the image about the X and Y body axes (rad/s), RH rotation is positive
+	Vector3f    gyro_rate{}; ///< measured angular rate of the inertial frame about the body axes obtained from rate gyro measurements (rad/s), RH rotation is positive
+	uint8_t     quality{};   ///< quality indicator between 0 and 255
 };
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 768b24d438..7da797f6c1 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -132,14 +132,12 @@ public:
 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
 	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
 
-	const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; }
-	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; }
+	const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; }
+	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; }
 
-	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
-	const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
+	const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_rate; }
 	const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; }
 	const Vector3f &getRefBodyRate() const { return _ref_body_rate; }
-	const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 	float getHeadingInnov() const
@@ -600,14 +598,9 @@ private:
 	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)
-	Vector3f _imu_del_ang_of{};	///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
 	Vector3f _ref_body_rate{};
-	Vector3f _measured_body_rate{};
 
-	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)
-	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
+	Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
 
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 #endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -879,6 +872,8 @@ private:
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion(const imuSample &imu_delayed);
+	void startFlowFusion();
+	void resetFlowFusion();
 	void stopFlowFusion();
 
 	void updateOnGroundMotionForOpticalFlowChecks();
@@ -888,8 +883,7 @@ private:
 	float calcOptFlowMeasVar(const flowSample &flow_sample);
 
 	// calculate optical flow body angular rate compensation
-	// returns false if bias corrected body rate data is unavailable
-	bool calcOptFlowBodyRateComp();
+	void calcOptFlowBodyRateComp(const imuSample &imu_delayed);
 
 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index 4d4cae818b..067d8b123c 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -120,7 +120,7 @@ public:
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
+	// if optical flow sensor gyro delta angles are not available, set gyro_rate vector fields to NaN and the EKF will use its internal gyro data instead
 	void setOpticalFlowData(const flowSample &flow);
 
 	// set sensor limitations reported by the optical flow sensor
diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp
index ef0810b865..626f0917cd 100644
--- a/src/modules/ekf2/EKF/optflow_fusion.cpp
+++ b/src/modules/ekf2/EKF/optflow_fusion.cpp
@@ -59,7 +59,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	// 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;
+	const Vector2f opt_flow_rate = _flow_rate_compensated;
 
 	// 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;
@@ -182,8 +182,8 @@ Vector2f Ekf::predictFlowVelBody()
 	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
 
 	// calculate the velocity of the sensor relative to the imu in body frame
-	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
-	const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt - _flow_gyro_bias)) % pos_offset_body;
+	// Note: _flow_sample_delayed.gyro_rate is the negative of the body angular velocity, thus use minus sign
+	const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_rate - _flow_gyro_bias)) % pos_offset_body;
 
 	// calculate the velocity of the sensor in the earth frame
 	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
@@ -192,43 +192,6 @@ Vector2f Ekf::predictFlowVelBody()
 	return _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
 }
 
-
-// calculate optical flow body angular rate compensation
-// returns false if bias corrected body rate data is unavailable
-bool Ekf::calcOptFlowBodyRateComp()
-{
-	bool is_body_rate_comp_available = false;
-
-	if ((_delta_time_of > FLT_EPSILON)
-	    && (_flow_sample_delayed.dt > FLT_EPSILON)) {
-		const Vector3f reference_body_rate = -_imu_del_ang_of / _delta_time_of; // flow gyro has opposite sign convention
-		_ref_body_rate = reference_body_rate;
-
-		if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1))) {
-			_flow_sample_delayed.gyro_xyz = reference_body_rate * _flow_sample_delayed.dt;
-
-		} else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) {
-			// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
-			_flow_sample_delayed.gyro_xyz(2) = reference_body_rate(2) * _flow_sample_delayed.dt;
-		}
-
-		const Vector3f measured_body_rate = _flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt;
-		_measured_body_rate = measured_body_rate;
-
-		if (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) {
-			// 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;
-		}
-
-		is_body_rate_comp_available = true;
-	}
-
-	// reset the accumulators
-	_imu_del_ang_of.setZero();
-	_delta_time_of = 0.0f;
-	return is_body_rate_comp_available;
-}
-
 // calculate the measurement variance for the optical flow sensor (rad/sec)^2
 float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample)
 {
diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp
index d4b2dc1466..f8218e883b 100644
--- a/src/modules/ekf2/EKF/optical_flow_control.cpp
+++ b/src/modules/ekf2/EKF/optical_flow_control.cpp
@@ -41,237 +41,118 @@
 void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 {
 	if (_flow_buffer) {
-		// We don't fuse flow data immediately because 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, 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 && !_hagl_sensor_status.flags.flow)) {
-			_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
-		}
-	}
-
-	// 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_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt));
-	if (_delta_time_of < 0.2f) {
-		_imu_del_ang_of += delta_angle;
-		_delta_time_of += imu_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_delayed.delta_ang_dt;
-	}
-
-	if (_flow_data_ready) {
-		int32_t min_quality = _params.flow_qual_min;
-		if (!_control_status.flags.in_air) {
-			min_quality = _params.flow_qual_min_gnd;
-		}
-
-		const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
-		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_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_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_delayed.delta_ang_dt);
-			}
-		}
-
-		const bool is_body_rate_comp_available = calcOptFlowBodyRateComp();
-
-		// don't allow invalid flow gyro_xyz to propagate
-		if (!_flow_sample_delayed.gyro_xyz.isAllFinite()) {
-			_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();
+		_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
 	}
 
 	// 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) {
+		const int32_t min_quality = _control_status.flags.in_air
+					    ? _params.flow_qual_min
+					    : _params.flow_qual_min_gnd;
+
+		const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
+		const bool is_magnitude_good = _flow_sample_delayed.flow_rate.isAllFinite()
+					       && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
+		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
+
+		if (is_quality_good
+		    && is_magnitude_good
+		    && is_tilt_good) {
+			// compensate for body motion to give a LOS rate
+			calcOptFlowBodyRateComp(imu_delayed);
+			_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
+
+		} else {
+			// don't use this flow data and wait for the next data to arrive
+			_flow_data_ready = false;
+			_flow_rate_compensated.setZero();
+		}
+	}
+
+	if (_flow_data_ready) {
+		updateOptFlow(_aid_src_optical_flow);
 
 		// Check if we are in-air and require optical flow to control position drift
 		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.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
+					|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
 
-#if defined(CONFIG_EKF2_GNSS)
-		// check if using GPS, but GPS is bad
-		if (_control_status.flags.gps) {
-			if (_control_status.flags.in_air && !is_flow_required) {
-				// 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;
+		// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
+		// use a relaxed time criteria to enable it to coast through bad range finder data
+		const bool terrain_available = isTerrainEstimateValid() || isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6);
 
-				if (_gps_error_norm > gps_err_norm_lim) {
-					is_flow_required = true;
-				}
-			}
-		}
-#endif // CONFIG_EKF2_GNSS
+		const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
+							   && _control_status.flags.tilt_align
+							   && (terrain_available || is_flow_required);
 
-		// 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
-						     && ((_time_delayed_us > (_time_good_motion_us + (uint64_t)1E5))
-								     || (_time_delayed_us < (_time_bad_motion_us + (uint64_t)5E6)));
-		const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
+		const bool starting_conditions_passing = continuing_conditions_passing
+							 && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
 
-		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.flow_ctrl == 1)
-			    || inhibit_flow_use) {
+			if (continuing_conditions_passing) {
+				fuseOptFlow();
 
+				// 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.no_aid_timeout_max)) {
+					if (is_flow_required) {
+						resetFlowFusion();
+
+					} else {
+						stopFlowFusion();
+					}
+				}
+
+			} else {
 				stopFlowFusion();
-				return;
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				startFlowFusion();
 			}
 		}
 
-		// optical flow fusion mode selection logic
-		if ((_params.flow_ctrl == 1) // 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()) {
-				ECL_INFO("reset velocity to flow");
-				_information_events.flags.reset_vel_to_flow = true;
-				resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
-
-				// reset position, estimate is relative to initial position in this mode, so we start with zero error
-				if (!_control_status.flags.in_air) {
-					ECL_INFO("reset position to zero");
-					resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
-					_last_known_pos.xy() = _state.pos.xy();
-
-				} else {
-					_information_events.flags.reset_pos_to_last_known = true;
-					ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
-					resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
-				}
-			}
-
-			_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
-			_control_status.flags.opt_flow = true;
-
-			return;
-		}
-
-		if (_control_status.flags.opt_flow) {
-			// Wait until the midpoint of the flow sample has fallen behind the fusion time horizon
-			if (_time_delayed_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(_aid_src_terrain_range_finder.time_last_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.no_aid_timeout_max)
-			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
-
-				ECL_INFO("reset velocity to flow");
-				_information_events.flags.reset_vel_to_flow = true;
-				resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
-
-				// reset position, estimate is relative to initial position in this mode, so we start with zero error
-				ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
-				_information_events.flags.reset_pos_to_last_known = true;
-				resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
-
-				_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
-			}
-		}
-
-	} else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) {
-
+	} else if (_control_status.flags.opt_flow && isTimedOut(_flow_sample_delayed.time_us, _params.reset_timeout_max)) {
 		stopFlowFusion();
 	}
 }
 
-void Ekf::updateOnGroundMotionForOpticalFlowChecks()
+void Ekf::startFlowFusion()
 {
-	// 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();
+	ECL_INFO("starting optical flow fusion");
 
-	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 (!_aid_src_optical_flow.innovation_rejected && isHorizontalAidingActive()) {
+		// Consistent with the current velocity state, simply fuse the data without reset
+		fuseOptFlow();
+		_control_status.flags.opt_flow = true;
 
-	if (motion_is_excessive) {
-		_time_bad_motion_us = _time_delayed_us;
+	} else if (!isHorizontalAidingActive()) {
+		resetFlowFusion();
+		_control_status.flags.opt_flow = true;
 
 	} else {
-		_time_good_motion_us = _time_delayed_us;
+		ECL_INFO("optical flow fusion failed to start");
+		_control_status.flags.opt_flow = false;
 	}
 }
 
-void Ekf::resetOnGroundMotionForOpticalFlowChecks()
+void Ekf::resetFlowFusion()
 {
-	_time_bad_motion_us = 0;
-	_time_good_motion_us = _time_delayed_us;
+	ECL_INFO("reset velocity to flow");
+	_information_events.flags.reset_vel_to_flow = true;
+	resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
+
+	// reset position, estimate is relative to initial position in this mode, so we start with zero error
+	if (!_control_status.flags.in_air) {
+		ECL_INFO("reset position to zero");
+		resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
+	}
+
+	updateOptFlow(_aid_src_optical_flow);
+	_innov_check_fail_status.flags.reject_optflow_X = false;
+	_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 }
 
 void Ekf::stopFlowFusion()
@@ -283,3 +164,24 @@ void Ekf::stopFlowFusion()
 		resetEstimatorAidStatus(_aid_src_optical_flow);
 	}
 }
+
+void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
+{
+	if (imu_delayed.delta_ang_dt > FLT_EPSILON) {
+		_ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias(); // flow gyro has opposite sign convention
+
+	} else {
+		_ref_body_rate.zero();
+	}
+
+	if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_rate(1))) {
+		_flow_sample_delayed.gyro_rate = _ref_body_rate;
+
+	} else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(2))) {
+		// Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro
+		_flow_sample_delayed.gyro_rate(2) = _ref_body_rate(2);
+	}
+
+	// calculate the bias estimate using  a combined LPF and spike filter
+	_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f, 0.1f) * 0.01f;
+}
diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp
index 62cf8367c3..19e12d8f75 100644
--- a/src/modules/ekf2/EKF/terrain_estimator.cpp
+++ b/src/modules/ekf2/EKF/terrain_estimator.cpp
@@ -58,6 +58,14 @@ void Ekf::initHagl()
 
 	// use the ground clearance value as our uncertainty
 	_terrain_var = sq(_params.rng_gnd_clearance);
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 }
 
 void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
@@ -419,17 +427,33 @@ void Ekf::controlHaglFakeFusion()
 	    && !_hagl_sensor_status.flags.range_finder
 	    && !_hagl_sensor_status.flags.flow) {
 
-		initHagl();
+		bool recent_terrain_aiding = false;
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+		recent_terrain_aiding |= isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)1e6);
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+		recent_terrain_aiding |= isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)1e6);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+		if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
+			initHagl();
+		}
 	}
 }
 
 bool Ekf::isTerrainEstimateValid() const
 {
+	bool valid = false;
+
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 
 	// we have been fusing range finder measurements in the last 5 seconds
-	if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
-		return true;
+	if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
+		if (_hagl_sensor_status.flags.range_finder || !_control_status.flags.in_air) {
+			valid = true;
+		}
 	}
 
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -439,12 +463,12 @@ bool Ekf::isTerrainEstimateValid() const
 	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
 	// this can only be the case if the main filter does not fuse optical flow
 	if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) {
-		return true;
+		valid = true;
 	}
 
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-	return false;
+	return valid;
 }
 
 void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 074d4112f2..bafb94bab5 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -2002,15 +2002,13 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 		_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
 
-		_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
-		_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
+		_ekf.getFlowUncompensated().copyTo(flow_vel.flow_rate_uncompensated);
+		_ekf.getFlowCompensated().copyTo(flow_vel.flow_rate_compensated);
 
 		_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate);
-		_ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral);
 
 		_ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias);
 		_ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro);
-		_ekf.getMeasuredBodyRate().copyTo(flow_vel.meas_gyro);
 
 		flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
@@ -2361,17 +2359,30 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 			perf_count(_msg_missed_optical_flow_perf);
 		}
 
-		flowSample flow {
-			.time_us = optical_flow.timestamp_sample,
+		const float dt = 1e-6f * (float)optical_flow.integration_timespan_us;
+		Vector2f flow_rate;
+		Vector3f gyro_rate;
+
+		if (dt > FLT_EPSILON) {
 			// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
 			// is produced by a RH rotation of the image about the sensor axis.
-			.flow_xy_rad = Vector2f{-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]},
-			.gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]},
-			.dt = 1e-6f * (float)optical_flow.integration_timespan_us,
-			.quality = optical_flow.quality,
+			flow_rate = Vector2f(-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]) / dt;
+			gyro_rate = Vector3f(-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]) / dt;
+
+		} else if (optical_flow.quality == 0) {
+			// handle special case of SITL and PX4Flow where dt is forced to zero when the quaity is 0
+			flow_rate.zero();
+			gyro_rate.zero();
+		}
+
+		flowSample flow {
+			.time_us = optical_flow.timestamp_sample - optical_flow.integration_timespan_us / 2, // correct timestamp to midpoint of integration interval as the data is converted to rates
+			.flow_rate = flow_rate,
+			.gyro_rate = gyro_rate,
+			.quality = optical_flow.quality
 		};
 
-		if (Vector2f(optical_flow.pixel_flow).isAllFinite() && flow.dt < 1) {
+		if (Vector2f(optical_flow.pixel_flow).isAllFinite() && optical_flow.integration_timespan_us < 1e6) {
 
 			// Save sensor limits reported by the optical flow sensor
 			_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
diff --git a/src/modules/ekf2/test/sensor_simulator/flow.cpp b/src/modules/ekf2/test/sensor_simulator/flow.cpp
index 400cb66b70..929a57a096 100644
--- a/src/modules/ekf2/test/sensor_simulator/flow.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/flow.cpp
@@ -28,9 +28,8 @@ void Flow::setData(const flowSample &flow)
 flowSample Flow::dataAtRest()
 {
 	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.flow_rate = Vector2f{0.0f, 0.0f};
+	flow_at_rest.gyro_rate = Vector3f{0.0f, 0.0f, 0.0f};
 	flow_at_rest.quality = 255;
 	return flow_at_rest;
 }
diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
index b3d481b383..d9b60b538f 100644
--- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
@@ -266,11 +266,11 @@ void SensorSimulator::setSingleReplaySample(const sensor_info &sample)
 
 	} else if (sample.sensor_type == sensor_info::measurement_t::FLOW) {
 		flowSample flow_sample;
-		flow_sample.flow_xy_rad = Vector2f(sample.sensor_data[0],
-						   sample.sensor_data[1]);
-		flow_sample.gyro_xyz = Vector3f(sample.sensor_data[2],
-						sample.sensor_data[3],
-						sample.sensor_data[4]);
+		flow_sample.flow_rate = Vector2f(sample.sensor_data[0],
+						 sample.sensor_data[1]);
+		flow_sample.gyro_rate = Vector3f(sample.sensor_data[2],
+						 sample.sensor_data[3],
+						 sample.sensor_data[4]);
 		flow_sample.quality = sample.sensor_data[5];
 		_flow.setData(flow_sample);
 
@@ -373,9 +373,9 @@ void SensorSimulator::setSensorDataFromTrajectory()
 	if (_flow.isRunning()) {
 		flowSample flow_sample = _flow.dataAtRest();
 		const Vector3f vel_body = R_world_to_body * vel_world;
-		flow_sample.flow_xy_rad =
-			Vector2f(vel_body(1) * flow_sample.dt / distance_to_ground,
-				 -vel_body(0) * flow_sample.dt / distance_to_ground);
+		flow_sample.flow_rate =
+			Vector2f(vel_body(1) / distance_to_ground,
+				 -vel_body(0) / distance_to_ground);
 		_flow.setData(flow_sample);
 	}
 
diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp
index 0302849dd4..0c541b0df4 100644
--- a/src/modules/ekf2/test/test_EKF_flow.cpp
+++ b/src/modules/ekf2/test/test_EKF_flow.cpp
@@ -102,9 +102,9 @@ void EkfFlowTest::startZeroFlowFusion()
 void EkfFlowTest::setFlowFromHorizontalVelocityAndDistance(flowSample &flow_sample,
 		const Vector2f &simulated_horz_velocity, float estimated_distance_to_ground)
 {
-	flow_sample.flow_xy_rad =
-		Vector2f(simulated_horz_velocity(1) * flow_sample.dt / estimated_distance_to_ground,
-			 -simulated_horz_velocity(0) * flow_sample.dt / estimated_distance_to_ground);
+	flow_sample.flow_rate =
+		Vector2f(simulated_horz_velocity(1) / estimated_distance_to_ground,
+			 -simulated_horz_velocity(0) / estimated_distance_to_ground);
 }
 
 TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
@@ -140,6 +140,8 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir)
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector3f estimated_velocity = _ekf->getVelocity();
+	estimated_velocity.print();
+	simulated_velocity.print();
 	EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity))
 			<< "estimated vel = " << estimated_velocity(0) << ", "
 			<< estimated_velocity(1);
@@ -163,11 +165,11 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 
 	// 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.startRangeFinder();
 	_sensor_simulator.runSeconds(1.0);
 
 	// THEN: estimated velocity should match simulated velocity
@@ -248,7 +250,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
 	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
 
 	// use autopilot gyro data
-	flow_sample.gyro_xyz.setAll(NAN);
+	flow_sample.gyro_rate.setAll(NAN);
 
 	_sensor_simulator._flow.setData(flow_sample);
 	_sensor_simulator._imu.setGyroData(body_rate);
@@ -286,7 +288,7 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
 
 	// use flow sensor gyro data
 	// for clarification of the sign, see definition of flowSample
-	flow_sample.gyro_xyz = -body_rate * flow_sample.dt;
+	flow_sample.gyro_rate = -body_rate;
 
 	_sensor_simulator._flow.setData(flow_sample);
 	_sensor_simulator._imu.setGyroData(body_rate);
diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp
index 0bafab37f3..0c5cf174b8 100644
--- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp
+++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp
@@ -183,6 +183,7 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow)
 	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.startRangeFinder();
 	_ekf_wrapper.enableFlowFusion();
 
 	_ekf->set_in_air_status(true);
diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp
index a18c3fdf02..b157e014e1 100644
--- a/src/modules/ekf2/test/test_EKF_gps.cpp
+++ b/src/modules/ekf2/test/test_EKF_gps.cpp
@@ -192,6 +192,7 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
 	_sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest());
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
+	_sensor_simulator.startRangeFinder();
 
 	_ekf_wrapper.enableGpsHeightFusion();
 
diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
index 72d0016160..5336afe15e 100644
--- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
+++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
@@ -98,9 +98,9 @@ public:
 
 		// Configure optical flow simulator data
 		flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
-		flow_sample.flow_xy_rad =
-			Vector2f(simulated_velocity(1) * flow_sample.dt / flow_height,
-				 -simulated_velocity(0) * flow_sample.dt / flow_height);
+		flow_sample.flow_rate =
+			Vector2f(simulated_velocity(1) / flow_height,
+				 -simulated_velocity(0) / flow_height);
 		_sensor_simulator._flow.setData(flow_sample);
 		const float max_flow_rate = 5.f;
 		const float min_ground_distance = 0.f;
diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
index 2e36e7cc46..e629095be6 100644
--- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
+++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
@@ -290,12 +290,12 @@ void VehicleOpticalFlow::Run()
 				// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
 				// is produced by a RH rotation of the image about the sensor axis.
 				const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]};
-				const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
+				const Vector3f gyro_rate_integral{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]};
 
 				const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us;
 
 				// compensate for body motion to give a LOS rate
-				const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy();
+				const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_rate_integral.xy();
 
 				Vector3f vel_optflow_body;
 				vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt;
@@ -320,24 +320,19 @@ void VehicleOpticalFlow::Run()
 					flow_vel.vel_ne[1] = flow_vel_ne(1);
 				}
 
-				// flow_uncompensated_integral
-				flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral);
+				const Vector2f flow_rate(flow_xy_rad * (1.f / flow_dt));
+				flow_rate.copyTo(flow_vel.flow_rate_uncompensated);
 
-				// flow_compensated_integral
-				flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral);
+				const Vector2f flow_rate_compensated(flow_compensated_XY_rad * (1.f / flow_dt));
+				flow_rate_compensated.copyTo(flow_vel.flow_rate_compensated);
 
-				const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt));
+				const Vector3f measured_body_rate(gyro_rate_integral * (1.f / flow_dt));
 
 				// gyro_rate
 				flow_vel.gyro_rate[0] = measured_body_rate(0);
 				flow_vel.gyro_rate[1] = measured_body_rate(1);
 				flow_vel.gyro_rate[2] = measured_body_rate(2);
 
-				// gyro_rate_integral
-				flow_vel.gyro_rate_integral[0] = gyro_xyz(0);
-				flow_vel.gyro_rate_integral[1] = gyro_xyz(1);
-				flow_vel.gyro_rate_integral[2] = gyro_xyz(2);
-
 				flow_vel.timestamp = hrt_absolute_time();
 
 				_vehicle_optical_flow_vel_pub.publish(flow_vel);