diff --git a/EKF/control.cpp b/EKF/control.cpp
index 9266315cec..3188311189 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -552,6 +552,11 @@ void Ekf::controlHeightAiding()
 		_control_status.flags.ev_hgt = true;
 
 	}
+
+	// If we are on ground, store the local position and time to use as a reference for takeoff checks
+	if (!_control_status.flags.in_air) {
+		_last_on_ground_posD = _state.pos(2);
+	}
 }
 
 void Ekf::controlMagAiding()
@@ -564,23 +569,28 @@ void Ekf::controlMagAiding()
 	// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
 	// or the more accurate 3-axis fusion
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
+		// start 3D fusion if in-flight and height has increased sufficiently
+		// to be away from ground magnetic anomalies
+		// don't switch back to heading fusion until we are back on the ground
+		bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
+		bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved);
 
-	if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-		// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
-		if (!_control_status.flags.mag_3D) {
-			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+		if (use_3D_fusion && _control_status.flags.tilt_align) {
+			// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
+			if (!_control_status.flags.mag_3D) {
+				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+			}
+
+			// use 3D mag fusion when airborne
+			_control_status.flags.mag_hdg = false;
+			_control_status.flags.mag_3D = true;
+
+		} else {
+			// use heading fusion when on the ground
+			_control_status.flags.mag_hdg = true;
+			_control_status.flags.mag_3D = false;
 		}
 
-		// use 3D mag fusion when airborne
-		_control_status.flags.mag_hdg = false;
-		_control_status.flags.mag_3D = true;
-
-	} else {
-		// use heading fusion when on the ground
-		_control_status.flags.mag_hdg = true;
-		_control_status.flags.mag_3D = false;
-	}
-
 	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
 		// always use heading fusion
 		_control_status.flags.mag_hdg = true;
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 35488673d1..b4f09b02fb 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -98,6 +98,8 @@ Ekf::Ekf():
 	_ev_counter(0),
 	_time_last_mag(0),
 	_hgt_sensor_offset(0.0f),
+	_baro_hgt_offset(0.0f),
+	_last_on_ground_posD(0.0f),
 	_terrain_vpos(0.0f),
 	_terrain_var(1.e4f),
 	_hagl_innov(0.0f),
@@ -106,7 +108,6 @@ Ekf::Ekf():
 	_baro_hgt_faulty(false),
 	_gps_hgt_faulty(false),
 	_rng_hgt_faulty(false),
-	_baro_hgt_offset(0.0f),
 	_time_bad_vert_accel(0)
 {
 	_state = {};
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 88237bd3f3..17aa1bf28a 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -248,6 +248,10 @@ private:
 	Vector3f _mag_filt_state;	// filtered magnetometer measurement
 	Vector3f _delVel_sum;		// summed delta velocity
 	float _hgt_sensor_offset;	// set as necessary if desired to maintain the same height after a height reset (m)
+	float _baro_hgt_offset;		// baro height reading at the local NED origin (m)
+
+	// Variables used to control activation of post takeoff functionality
+	float _last_on_ground_posD; // last vertical position when the in_air status was false (m)
 
 	gps_check_fail_status_u _gps_check_fail_status;
 
@@ -265,8 +269,6 @@ private:
 	bool _rng_hgt_faulty;		// true if valid rnage finder height data is unavailable for use
 	int _primary_hgt_source;	// priary source of height data set at initialisation
 
-	float _baro_hgt_offset;		// baro height reading at the local NED origin (m)
-
 	// imu fault status
 	uint64_t _time_bad_vert_accel;	// last time a bad vertical accel was detected (usec)