diff --git a/EKF/control.cpp b/EKF/control.cpp
index 0a3bd8f504..28d92b271f 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -514,16 +514,7 @@ void Ekf::controlGpsFusion()
 
 				// If the heading is valid start using gps aiding
 				if (_control_status.flags.yaw_align) {
-					resetHorizontalPositionToGps();
-
-					// when adding with optical flow,
-					// velocity reset is not necessary
-					if (!_control_status.flags.opt_flow) {
-						resetVelocityToGps();
-					}
-
-					ECL_INFO_TIMESTAMPED("starting GPS fusion");
-					_control_status.flags.gps = true;
+					startGpsFusion();
 				}
 			}
 
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 6b3dc9231c..c4664ab9f2 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -847,10 +847,9 @@ private:
 		return sensor_timestamp + acceptance_interval > _time_last_imu;
 	}
 
+	void startGpsFusion();
 	void stopGpsFusion();
-
 	void stopGpsPosFusion();
-
 	void stopGpsVelFusion();
 
 	void startGpsYawFusion();
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 8d65a84fce..8999280d2f 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -1530,6 +1530,20 @@ void Ekf::loadMagCovData()
 	P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
 }
 
+void Ekf::startGpsFusion()
+{
+	resetHorizontalPositionToGps();
+
+	// when using optical flow,
+	// velocity reset is not necessary
+	if (!_control_status.flags.opt_flow) {
+		resetVelocityToGps();
+	}
+
+	ECL_INFO_TIMESTAMPED("starting GPS fusion");
+	_control_status.flags.gps = true;
+}
+
 void Ekf::stopGpsFusion()
 {
 	stopGpsPosFusion();