diff --git a/EKF/control.cpp b/EKF/control.cpp
index 2f6083ce5c..fd3cc7d8bd 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -41,6 +41,7 @@
 
 #include "../ecl.h"
 #include "ekf.h"
+#include "mathlib.h"
 
 void Ekf::controlFusionModes()
 {
@@ -60,228 +61,351 @@ void Ekf::controlFusionModes()
 		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
 			_control_status.flags.tilt_align = true;
 			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+			ECL_INFO("EKF alignment complete");
+
 		}
+
 	}
 
-	// control use of various external sources for position and velocity aiding
-	controlExternalVisionAiding();
-	controlOpticalFlowAiding();
-	controlGpsAiding();
-	controlHeightAiding();
-	controlMagAiding();
+	// check for arrival of new sensor data at the fusion time horizon
+	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
+	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
+	_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
+	_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
+			&& (_R_to_earth(2, 2) > 0.7071f);
+	_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
+			&&  (_R_to_earth(2, 2) > 0.7071f);
+	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
+	_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
+
+	// check for height sensor timeouts and reset and change sensor if necessary
+	controlHeightSensorTimeouts();
+
+	// control use of observations for aiding
+	controlMagFusion();
+	controlExternalVisionFusion();
+	controlOpticalFlowFusion();
+	controlGpsFusion();
+	controlBaroFusion();
+	controlRangeFinderFusion();
+	controlAirDataFusion();
+
+	// for efficiency, fusion of direct state observations for position ad velocity is performed sequentially
+	// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
+	controlVelPosFusion();
 
 }
 
-void Ekf::controlExternalVisionAiding()
+void Ekf::controlExternalVisionFusion()
 {
-	// external vision position aiding selection logic
-	if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
-		// check for a exernal vision measurement that has fallen behind the fusion time horizon
-		if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
-			// turn on use of external vision measurements for position and height
-			_control_status.flags.ev_pos = true;
-			ECL_INFO("EKF switching to external vision position fusion");
-			// turn off other forms of height aiding
+	// Check for new exernal vision data
+	if (_ev_data_ready) {
+
+		// external vision position aiding selection logic
+		if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
+			// check for a exernal vision measurement that has fallen behind the fusion time horizon
+			if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
+				// turn on use of external vision measurements for position and height
+				_control_status.flags.ev_pos = true;
+				ECL_INFO("EKF switching to external vision position fusion");
+				// turn off other forms of height aiding
+				_control_status.flags.baro_hgt = false;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = false;
+				// reset the position, height and velocity
+				resetPosition();
+				resetVelocity();
+				resetHeight();
+			}
+		}
+
+		// external vision yaw aiding selection logic
+		if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
+			// check for a exernal vision measurement that has fallen behind the fusion time horizon
+			if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
+				// reset the yaw angle to the value from the observaton quaternion
+				// get the roll, pitch, yaw estimates from the quaternion states
+				matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
+							    _state.quat_nominal(3));
+				matrix::Euler<float> euler_init(q_init);
+
+				// get initial yaw from the observation quaternion
+				extVisionSample ev_newest = _ext_vision_buffer.get_newest();
+				matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
+				matrix::Euler<float> euler_obs(q_obs);
+				euler_init(2) = euler_obs(2);
+
+				// save a copy of the quaternion state for later use in calculating the amount of reset change
+				Quaternion quat_before_reset = _state.quat_nominal;
+
+				// calculate initial quaternion states for the ekf
+				_state.quat_nominal = Quaternion(euler_init);
+
+				// calculate the amount that the quaternion has changed by
+				_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
+
+				// add the reset amount to the output observer buffered data
+				outputSample output_states;
+				unsigned output_length = _output_buffer.get_length();
+				for (unsigned i=0; i < output_length; i++) {
+					output_states = _output_buffer.get_from_index(i);
+					output_states.quat_nominal *= _state_reset_status.quat_change;
+					_output_buffer.push_to_index(i,output_states);
+				}
+
+				// capture the reset event
+				_state_reset_status.quat_counter++;
+
+				// flag the yaw as aligned
+				_control_status.flags.yaw_align = true;
+
+				// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
+				_control_status.flags.ev_yaw = true;
+				_control_status.flags.mag_hdg = false;
+				_control_status.flags.mag_3D = false;
+				_control_status.flags.mag_dec = false;
+
+				ECL_INFO("EKF switching to external vision yaw fusion");
+			}
+		}
+
+		// determine if we should use the height observation
+		if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
 			_control_status.flags.baro_hgt = false;
 			_control_status.flags.gps_hgt = false;
 			_control_status.flags.rng_hgt = false;
-			// reset the position, height and velocity
-			resetPosition();
-			resetVelocity();
-			resetHeight();
-		}
-	}
+			_control_status.flags.ev_hgt = true;
+			_fuse_height = true;
 
-	// external vision yaw aiding selection logic
-	if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
-		// check for a exernal vision measurement that has fallen behind the fusion time horizon
-		if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
-			// reset the yaw angle to the value from the observaton quaternion
-			// get the roll, pitch, yaw estimates from the quaternion states
-			matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
-						    _state.quat_nominal(3));
-			matrix::Euler<float> euler_init(q_init);
-
-			// get initial yaw from the observation quaternion
-			extVisionSample ev_newest = _ext_vision_buffer.get_newest();
-			matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
-			matrix::Euler<float> euler_obs(q_obs);
-			euler_init(2) = euler_obs(2);
-
-			// save a copy of the quaternion state for later use in calculating the amount of reset change
-			Quaternion quat_before_reset = _state.quat_nominal;
-
-			// calculate initial quaternion states for the ekf
-			_state.quat_nominal = Quaternion(euler_init);
-
-			// calculate the amount that the quaternion has changed by
-			_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
-
-			// add the reset amount to the output observer buffered data
-			outputSample output_states;
-			unsigned output_length = _output_buffer.get_length();
-			for (unsigned i=0; i < output_length; i++) {
-				output_states = _output_buffer.get_from_index(i);
-				output_states.quat_nominal *= _state_reset_status.quat_change;
-				_output_buffer.push_to_index(i,output_states);
-			}
-
-			// capture the reset event
-			_state_reset_status.quat_counter++;
-
-			// flag the yaw as aligned
-			_control_status.flags.yaw_align = true;
-
-			// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
-			_control_status.flags.ev_yaw = true;
-			_control_status.flags.mag_hdg = false;
-			_control_status.flags.mag_3D = false;
-			_control_status.flags.mag_dec = false;
-
-			ECL_INFO("EKF switching to external vision yaw fusion");
-		}
-	}
-
-}
-
-void Ekf::controlOpticalFlowAiding()
-{
-	// optical flow fusion mode selection logic
-	// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
-	if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align
-	    && (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) {
-		// If the heading is not aligned, reset the yaw and magnetic field states
-		if (!_control_status.flags.yaw_align) {
-			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 		}
 
-		// If the heading is valid, start using optical flow aiding
-		if (_control_status.flags.yaw_align) {
-			// set the flag and reset the fusion timeout
-			_control_status.flags.opt_flow = true;
-			_time_last_of_fuse = _time_last_imu;
+		// determine if we should use the horizontal position observations
+		if (_control_status.flags.ev_pos) {
+			_fuse_pos = true;
 
-			// if we are not using GPS then the velocity and position states and covariances need to be set
-			if (!_control_status.flags.gps) {
-				// constrain height above ground to be above minimum possible
-				float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
-
-				// calculate absolute distance from focal point to centre of frame assuming a flat earth
-				float range = heightAboveGndEst / _R_to_earth(2, 2);
-
-				if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
-					// 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_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
-					vel_optflow_body(1) =   range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
-					vel_optflow_body(2) = 0.0f;
-
-					// rotate from body to earth frame
-					Vector3f vel_optflow_earth;
-					vel_optflow_earth = _R_to_earth * vel_optflow_body;
-
-					// take x and Y components
-					_state.vel(0) = vel_optflow_earth(0);
-					_state.vel(1) = vel_optflow_earth(1);
-
-				} else {
-					_state.vel(0) = 0.0f;
-					_state.vel(1) = 0.0f;
-				}
-
-				// reset the velocity covariance terms
-				zeroRows(P,4,5);
-				zeroCols(P,4,5);
-
-				// reset the horizontal velocity variance using the optical flow noise variance
-				P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
-
-				if (!_control_status.flags.in_air) {
-					// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
-					_state.pos(0) = 0.0f;
-					_state.pos(1) = 0.0f;
-
-					// reset the corresponding covariances
-					// we are by definition at the origin at commencement so variances are also zeroed
-					zeroRows(P,7,8);
-					zeroCols(P,7,8);
-
-					// align the output observer to the EKF states
-					alignOutputFilter();
-				}
-			}
+			// correct position and height for offset relative to IMU
+			Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
+			Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
+			_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
+			_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
 		}
 
-	} else if (!(_params.fusion_mode & MASK_USE_OF)) {
-		_control_status.flags.opt_flow = false;
-	}
-
-	// handle the case when we are relying on optical flow fusion and lose it
-	if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
-		// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
-		if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
-			// Switch to the non-aiding mode, zero the velocity states
-			// and set the synthetic position to the current estimate
-			_control_status.flags.opt_flow = false;
-			_last_known_posNE(0) = _state.pos(0);
-			_last_known_posNE(1) = _state.pos(1);
-			_state.vel.setZero();
+		// determine if we should use the yaw observation
+		if (_control_status.flags.ev_yaw) {
+			fuseHeading();
 		}
 	}
 }
 
-void Ekf::controlGpsAiding()
+void Ekf::controlOpticalFlowFusion()
 {
-	// GPS fusion mode selection logic
-	// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
-	if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
-		if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
-		    && (_time_last_imu - _last_gps_fail_us > 5e6)) {
+	// Check for new optical flow data that has fallen behind the fusion time horizon
+	if (_flow_data_ready) {
+
+		// optical flow fusion mode selection logic
+		if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
+				&& !_control_status.flags.opt_flow // we are not yet using flow data
+				&& _control_status.flags.tilt_align // we know our tilt attitude
+				&& (_time_last_imu - _time_last_hagl_fuse) < 5e5) // we have a valid distance to ground estimate
+		{
+
 			// If the heading is not aligned, reset the yaw and magnetic field states
 			if (!_control_status.flags.yaw_align) {
 				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 			}
 
-			// If the heading is valid start using gps aiding
+			// If the heading is valid, start using optical flow aiding
 			if (_control_status.flags.yaw_align) {
-				_control_status.flags.gps = true;
-				_time_last_gps = _time_last_imu;
+				// set the flag and reset the fusion timeout
+				_control_status.flags.opt_flow = true;
+				_time_last_of_fuse = _time_last_imu;
 
-				// if we are not already aiding with optical flow, then we need to reset the position and velocity
-				if (!_control_status.flags.opt_flow) {
-					_control_status.flags.gps = resetPosition();
-					_control_status.flags.gps = resetVelocity();
+				// if we are not using GPS then the velocity and position states and covariances need to be set
+				if (!_control_status.flags.gps) {
+					// constrain height above ground to be above minimum possible
+					float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
+
+					// calculate absolute distance from focal point to centre of frame assuming a flat earth
+					float range = heightAboveGndEst / _R_to_earth(2, 2);
+
+					if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
+						// 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_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
+						vel_optflow_body(1) =   range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
+						vel_optflow_body(2) = 0.0f;
+
+						// rotate from body to earth frame
+						Vector3f vel_optflow_earth;
+						vel_optflow_earth = _R_to_earth * vel_optflow_body;
+
+						// take x and Y components
+						_state.vel(0) = vel_optflow_earth(0);
+						_state.vel(1) = vel_optflow_earth(1);
+
+					} else {
+						_state.vel(0) = 0.0f;
+						_state.vel(1) = 0.0f;
+					}
+
+					// reset the velocity covariance terms
+					zeroRows(P,4,5);
+					zeroCols(P,4,5);
+
+					// reset the horizontal velocity variance using the optical flow noise variance
+					P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
+
+					if (!_control_status.flags.in_air) {
+						// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
+						_state.pos(0) = 0.0f;
+						_state.pos(1) = 0.0f;
+
+						// reset the corresponding covariances
+						// we are by definition at the origin at commencement so variances are also zeroed
+						zeroRows(P,7,8);
+						zeroCols(P,7,8);
+
+						// align the output observer to the EKF states
+						alignOutputFilter();
+
+					}
 				}
 			}
+
+		} else if (!(_params.fusion_mode & MASK_USE_OF)) {
+			_control_status.flags.opt_flow = false;
+
 		}
 
-	}  else if (!(_params.fusion_mode & MASK_USE_GPS)) {
-		_control_status.flags.gps = false;
-	}
-
-	// handle the case when we are relying on GPS fusion and lose it
-	if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
-		// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
-		if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
-			if (_time_last_imu - _time_last_gps > 5e5) {
-				// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
-				// and set the synthetic GPS position to the current estimate
-				_control_status.flags.gps = false;
+		// handle the case when we are relying on optical flow fusion and lose it
+		if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
+			// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
+			if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
+				// Switch to the non-aiding mode, zero the velocity states
+				// and set the synthetic position to the current estimate
+				_control_status.flags.opt_flow = false;
 				_last_known_posNE(0) = _state.pos(0);
 				_last_known_posNE(1) = _state.pos(1);
 				_state.vel.setZero();
 
-			} else {
-				// Reset states to the last GPS measurement
-				resetPosition();
-				resetVelocity();
-
-				// Reset the timeout counters
-				_time_last_pos_fuse = _time_last_imu;
-				_time_last_vel_fuse = _time_last_imu;
 			}
 		}
+
+		// fuse the data
+		if (_control_status.flags.opt_flow) {
+			// Update optical flow bias estimates
+			calcOptFlowBias();
+
+			// Fuse optical flow LOS rate observations into the main filter
+			fuseOptFlow();
+			_last_known_posNE(0) = _state.pos(0);
+			_last_known_posNE(1) = _state.pos(1);
+
+		}
+	}
+}
+
+void Ekf::controlGpsFusion()
+{
+	// Check for new GPS data that has fallen behind the fusion time horizon
+	if (_gps_data_ready) {
+
+		// Determine if we should use GPS aiding for velocity and horizontal position
+		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
+		if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
+			if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) {
+				// If the heading is not aligned, reset the yaw and magnetic field states
+				if (!_control_status.flags.yaw_align) {
+					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+
+				}
+
+				// If the heading is valid start using gps aiding
+				if (_control_status.flags.yaw_align) {
+					_control_status.flags.gps = true;
+					_time_last_gps = _time_last_imu;
+
+					// if we are not already aiding with optical flow, then we need to reset the position and velocity
+					if (!_control_status.flags.opt_flow) {
+						if (resetPosition() && resetVelocity()) {
+							_control_status.flags.gps = true;
+
+						} else {
+							_control_status.flags.gps = false;
+
+						}
+					}
+					if (_control_status.flags.gps) {
+						ECL_INFO("EKF commencing GPS aiding");
+
+					}
+				}
+			}
+
+		}  else if (!(_params.fusion_mode & MASK_USE_GPS)) {
+			_control_status.flags.gps = false;
+
+		}
+
+		// handle the case when we are relying on GPS fusion and lose it
+		if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
+			// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
+			if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
+				if (_time_last_imu - _time_last_gps > 5e5) {
+					// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
+					// and set the synthetic GPS position to the current estimate
+					_control_status.flags.gps = false;
+					_last_known_posNE(0) = _state.pos(0);
+					_last_known_posNE(1) = _state.pos(1);
+					_state.vel.setZero();
+					ECL_WARN("EKF GPS fusion timout - stopping GPS aiding");
+
+				} else {
+					// Reset states to the last GPS measurement
+					resetPosition();
+					resetVelocity();
+					ECL_WARN("EKF GPS fusion timout - resetting to GPS");
+
+					// Reset the timeout counters
+					_time_last_pos_fuse = _time_last_imu;
+					_time_last_vel_fuse = _time_last_imu;
+
+				}
+			}
+		}
+
+		// Only use GPS data for position and velocity aiding if enabled
+		if (_control_status.flags.gps) {
+			_fuse_pos = true;
+			_fuse_vert_vel = true;
+			_fuse_hor_vel = true;
+
+			// correct velocity for offset relative to IMU
+			Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
+			Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+			Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body);
+			Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
+			_gps_sample_delayed.vel -= vel_offset_earth;
+
+			// correct position and height for offset relative to IMU
+			Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
+			_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
+			_gps_sample_delayed.hgt += pos_offset_earth(2);
+
+		}
+
+		// Determine if GPS should be used as the height source
+		if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS) || _control_status.flags.gps) && !_gps_hgt_faulty) {
+			_control_status.flags.baro_hgt = false;
+			_control_status.flags.gps_hgt = true;
+			_control_status.flags.rng_hgt = false;
+			_control_status.flags.ev_hgt = false;
+			_fuse_height = true;
+
+		}
 	}
 }
 
@@ -336,31 +460,39 @@ void Ekf::controlHeightSensorTimeouts()
 				// set height sensor health
 				_baro_hgt_faulty = true;
 				_gps_hgt_faulty = false;
+
 				// declare the GPS height healthy
 				_gps_hgt_faulty = false;
+
 				// reset the height mode
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = true;
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = false;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF baro hgt timeout - reset to GPS");
+
 			} else if (reset_to_baro){
 				// set height sensor health
 				_baro_hgt_faulty = false;
+
 				// reset the height mode
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = false;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF baro hgt timeout - reset to baro");
+
 			} else {
 				// we have nothing we can reset to
 				// deny a reset
 				reset_height = false;
+
 			}
 		}
 
@@ -370,6 +502,7 @@ void Ekf::controlHeightSensorTimeouts()
 			gpsSample gps_init = _gps_buffer.get_newest();
 			bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
 			bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+
 			// check the baro height source for consistency and freshness
 			baroSample baro_init = _baro_buffer.get_newest();
 			bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
@@ -389,28 +522,35 @@ void Ekf::controlHeightSensorTimeouts()
 				// set height sensor health
 				_gps_hgt_faulty = true;
 				_baro_hgt_faulty = false;
+
 				// reset the height mode
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = false;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF gps hgt timeout - reset to baro");
+
 			} else if (reset_to_gps) {
 				// set height sensor health
 				_gps_hgt_faulty = false;
+
 				// reset the height mode
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = true;
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = false;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF gps hgt timeout - reset to GPS");
+
 			} else {
 				// we have nothing to reset to
 				reset_height = false;
+
 			}
 		}
 
@@ -419,6 +559,7 @@ void Ekf::controlHeightSensorTimeouts()
 			// check if range finder data is available
 			rangeSample rng_init = _range_buffer.get_newest();
 			bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
+
 			// check if baro data is available
 			baroSample baro_init = _baro_buffer.get_newest();
 			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
@@ -433,28 +574,35 @@ void Ekf::controlHeightSensorTimeouts()
 				// set height sensor health
 				_rng_hgt_faulty = true;
 				_baro_hgt_faulty = false;
+
 				// reset the height mode
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = false;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF rng hgt timeout - reset to baro");
+
 			} else if (reset_to_rng) {
 				// set height sensor health
 				_rng_hgt_faulty = false;
+
 				// reset the height mode
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = true;
 				_control_status.flags.ev_hgt = false;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF rng hgt timeout - reset to rng hgt");
+
 			} else {
 				// we have nothing to reset to
 				reset_height = false;
+
 			}
 		}
 
@@ -463,6 +611,7 @@ void Ekf::controlHeightSensorTimeouts()
 			// check if vision data is available
 			extVisionSample ev_init = _ext_vision_buffer.get_newest();
 			bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
+
 			// check if baro data is available
 			baroSample baro_init = _baro_buffer.get_newest();
 			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
@@ -477,26 +626,32 @@ void Ekf::controlHeightSensorTimeouts()
 				// set height sensor health
 				_rng_hgt_faulty = true;
 				_baro_hgt_faulty = false;
+
 				// reset the height mode
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = false;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF ev hgt timeout - reset to baro");
+
 			} else if (reset_to_ev) {
 				// reset the height mode
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
 				_control_status.flags.ev_hgt = true;
+
 				// request a reset
 				reset_height = true;
 				ECL_INFO("EKF ev hgt timeout - reset to ev hgt");
+
 			} else {
 				// we have nothing to reset to
 				reset_height = false;
+
 			}
 		}
 
@@ -505,127 +660,206 @@ void Ekf::controlHeightSensorTimeouts()
 			resetHeight();
 			// Reset the timout timer
 			_time_last_hgt_fuse = _time_last_imu;
+
 		}
 
 	}
 }
 
-void Ekf::controlHeightAiding()
+void Ekf::controlBaroFusion()
 {
-	// check for height sensor timeouts and reset and change sensor if necessary
-	controlHeightSensorTimeouts();
+	if (_baro_data_ready) {
+		// determine if we should use the baro as our height source
+		uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
+		if (((_params.vdist_sensor_type == VDIST_SENSOR_BARO) || _control_status.flags.baro_hgt) && !_baro_hgt_faulty) {
+			_control_status.flags.baro_hgt = true;
+			_control_status.flags.gps_hgt = false;
+			_control_status.flags.rng_hgt = false;
+			_control_status.flags.ev_hgt = false;
+			_fuse_height = true;
 
-	// Control the source of height measurements for the main filter
-	// do not switch to a sensor if it is unhealthy or the data is stale
-	if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) &&
-			!_baro_hgt_faulty &&
-			(((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) {
+		}
 
-		_control_status.flags.baro_hgt = true;
-		_control_status.flags.gps_hgt = false;
-		_control_status.flags.rng_hgt = false;
-		_control_status.flags.ev_hgt = false;
+		// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
+		if (!_control_status.flags.baro_hgt) {
+			float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
+			local_time_step = math::constrain(local_time_step,0.0f,1.0f);
+			last_baro_time_us = _baro_sample_delayed.time_us;
+			float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
+			_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
 
-	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) &&
-		   !_gps_hgt_faulty &&
-		   (((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) {
-
-		_control_status.flags.baro_hgt = false;
-		_control_status.flags.gps_hgt = true;
-		_control_status.flags.rng_hgt = false;
-		_control_status.flags.ev_hgt = false;
-
-	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) &&
-		   !_rng_hgt_faulty &&
-		    (((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL)  || _control_status.flags.rng_hgt)) {
-
-		_control_status.flags.baro_hgt = false;
-		_control_status.flags.gps_hgt = false;
-		_control_status.flags.rng_hgt = true;
-		_control_status.flags.ev_hgt = false;
-
-	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) &&
-		   (((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL)  || _control_status.flags.ev_hgt)) {
-
-		_control_status.flags.baro_hgt = false;
-		_control_status.flags.gps_hgt = false;
-		_control_status.flags.rng_hgt = false;
-		_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()
+void Ekf::controlRangeFinderFusion()
+{
+	// determine if we should use range finder data for height
+	if (_range_data_ready) {
+		// set the height data source to range if requested
+		if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
+			_control_status.flags.baro_hgt = false;
+			_control_status.flags.gps_hgt = false;
+			_control_status.flags.rng_hgt = true;
+			_control_status.flags.ev_hgt = false;
+
+		}
+
+		// correct the range data for position offset relative to the IMU
+		Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+		Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+		_range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2);
+
+		// always fuse available range finder data into a terrain height estimator if the estimator has been initialised
+		if (_terrain_initialised) {
+			fuseHagl();
+
+		}
+
+		// only use range finder as a height observation in the main filter if specifically enabled
+		if (_control_status.flags.rng_hgt) {
+			_fuse_height = true;
+
+		}
+
+	} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
+		// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
+		// and are on the ground, then synthesise a measurement at the expected on ground value
+		if (!_control_status.flags.in_air) {
+			_range_sample_delayed.rng = _params.rng_gnd_clearance;
+			_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
+
+		}
+
+		_fuse_height = true;
+
+	}
+}
+
+void Ekf::controlAirDataFusion()
+{
+	// TODO This is just to get the logic inside but we will only start fusion once we tested this again
+	//if (_tas_data_ready) {
+	if (false) {
+		fuseAirspeed();
+
+	}
+
+	// control airspeed fusion - TODO move to a function
+	// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
+	if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
+		_control_status.flags.wind = false;
+
+	} else {
+		_control_status.flags.wind = true;
+
+	}
+}
+
+void Ekf::controlMagFusion()
 {
 	// If we are using external vision data for heading then no magnetometer fusion is used
 	if (_control_status.flags.ev_yaw) {
 		return;
 	}
 
-	// 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 we are on ground, store the local position and time to use as a reference
+	if (!_control_status.flags.in_air) {
+		_last_on_ground_posD = _state.pos(2);
 
-		if (use_3D_fusion && _control_status.flags.tilt_align) {
+	}
+
+	// checs for new magnetometer data tath has fallen beind the fusion time horizon
+	if (_mag_data_ready) {
+
+		// 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 (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;
+			}
+
+		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
+			// always use heading fusion
+			_control_status.flags.mag_hdg = true;
+			_control_status.flags.mag_3D = false;
+
+		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
 			// 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
+			// always use 3-axis mag fusion
 			_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;
+			// do no magnetometer fusion at all
+			_control_status.flags.mag_hdg = false;
 			_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;
-		_control_status.flags.mag_3D = false;
+		// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
+		// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
+		if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
+			_control_status.flags.mag_dec = true;
 
-	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
-		// 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);
+		} else {
+			_control_status.flags.mag_dec = false;
 		}
 
-		// always use 3-axis mag fusion
-		_control_status.flags.mag_hdg = false;
-		_control_status.flags.mag_3D = true;
+		// fuse magnetometer data using the selected methods
+		if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
+			fuseMag();
 
-	} else {
-		// do no magnetometer fusion at all
-		_control_status.flags.mag_hdg = false;
-		_control_status.flags.mag_3D = false;
-	}
+			if (_control_status.flags.mag_dec) {
+				fuseDeclination();
+			}
 
-	// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
-	// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
-	if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
-		_control_status.flags.mag_dec = true;
+		} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
+			// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
+			fuseHeading();
+
+		} else {
+			// do no fusion at all
+		}
+	}
+}
+
+void Ekf::controlVelPosFusion()
+{
+	// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
+	// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
+	if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos
+	    && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
+		_fuse_pos = true;
+		_time_last_fake_gps = _time_last_imu;
+
+	}
+
+	// Fuse available NED velocity and position data into the main filter
+	if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
+		fuseVelPosHeight();
+		_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
 
-	} else {
-		_control_status.flags.mag_dec = false;
-	}
-
-	// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
-	if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
-		_control_status.flags.wind = false;
-	} else {
-		_control_status.flags.wind = true;
 	}
 }
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 329fa64938..14a1e26fa0 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -39,6 +39,7 @@
  * @author Paul Riseborough <p_riseborough@live.com.au>
  */
 
+#include "../ecl.h"
 #include "ekf.h"
 #include "mathlib.h"
 
@@ -67,8 +68,13 @@ Ekf::Ekf():
 	_fuse_pos(false),
 	_fuse_hor_vel(false),
 	_fuse_vert_vel(false),
-	_fuse_flow(false),
-	_fuse_hagl_data(false),
+	_gps_data_ready(false),
+	_mag_data_ready(false),
+	_baro_data_ready(false),
+	_range_data_ready(false),
+	_flow_data_ready(false),
+	_ev_data_ready(false),
+	_tas_data_ready(false),
 	_time_last_fake_gps(0),
 	_time_last_pos_fuse(0),
 	_time_last_vel_fuse(0),
@@ -209,166 +215,9 @@ bool Ekf::update()
 			predictHagl();
 		}
 
-		// control logic
+		// control fusion of observation data
 		controlFusionModes();
 
-		// measurement updates
-
-		// Fuse magnetometer data using the selected fusion method and only if angular alignment is complete
-		if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-			if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
-				fuseMag();
-
-				if (_control_status.flags.mag_dec) {
-					fuseDeclination();
-				}
-
-			} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
-				// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
-				fuseHeading();
-
-			} else {
-				// do no fusion at all
-			}
-		}
-
-		// determine if range finder data has fallen behind the fusion time horizon fuse it if we are
-		// not tilted too much to use it
-		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
-		    && (_R_to_earth(2, 2) > 0.7071f)) {
-			// correct the range data for position offset relative to the IMU
-			Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
-			Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-			_range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2);
-
-			// if we have range data we always try to estimate terrain height
-			_fuse_hagl_data = true;
-
-			// only use range finder as a height observation in the main filter if specifically enabled
-			if (_control_status.flags.rng_hgt) {
-				_fuse_height = true;
-			}
-
-		} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
-			// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
-			// and are on the ground, then synthesise a measurement at the expected on ground value
-			if (!_control_status.flags.in_air) {
-				_range_sample_delayed.rng = _params.rng_gnd_clearance;
-				_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
-
-			}
-
-			_fuse_height = true;
-		}
-
-		// determine if baro data has fallen behind the fusion time horizon and fuse it in the main filter if enabled
-		uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
-		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-			if (_control_status.flags.baro_hgt) {
-				_fuse_height = true;
-
-			} else {
-				// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
-				float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
-				local_time_step = math::constrain(local_time_step,0.0f,1.0f);
-				last_baro_time_us = _baro_sample_delayed.time_us;
-				float offset_rate_correction =  0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
-				_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
-			}
-		}
-
-		// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
-		if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
-			// Only use GPS data for position and velocity aiding if enabled
-			if (_control_status.flags.gps) {
-				_fuse_pos = true;
-				_fuse_vert_vel = true;
-				_fuse_hor_vel = true;
-
-				// correct velocity for offset relative to IMU
-				Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
-				Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
-				Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body);
-				Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-				_gps_sample_delayed.vel -= vel_offset_earth;
-
-				// correct position and height for offset relative to IMU
-				Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-				_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
-				_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
-				_gps_sample_delayed.hgt += pos_offset_earth(2);
-
-			}
-
-			// only use gps height observation in the main filter if specifically enabled
-			if (_control_status.flags.gps_hgt) {
-				_fuse_height = true;
-			}
-
-		}
-
-		// If we are using external vision aiding and data has fallen behind the fusion time horizon then fuse it
-		if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
-			// use external vision posiiton and height observations
-			if (_control_status.flags.ev_pos) {
-				_fuse_pos = true;
-				_fuse_height = true;
-				
-				// correct position and height for offset relative to IMU
-				Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-				Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-				_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
-				_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
-				_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
-			}
-			// use external vision yaw observation
-			if (_control_status.flags.ev_yaw) {
-				fuseHeading();
-			}
-		}
-
-		// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
-		if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
-		    && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
-		    && (_R_to_earth(2, 2) > 0.7071f)) {
-			_fuse_flow = true;
-		}
-
-		// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
-		// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
-		if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos
-		    && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
-			_fuse_pos = true;
-			_time_last_fake_gps = _time_last_imu;
-		}
-
-		// fuse available range finder data into a terrain height estimator if it has been initialised
-		if (_fuse_hagl_data && _terrain_initialised) {
-			fuseHagl();
-			_fuse_hagl_data = false;
-		}
-
-		// Fuse available NED velocity and position data into the main filter
-		if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
-			fuseVelPosHeight();
-			_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
-		}
-
-		// Update optical flow bias estimates
-		calcOptFlowBias();
-
-		// Fuse optical flow LOS rate observations into the main filter
-		if (_fuse_flow) {
-			fuseOptFlow();
-			_last_known_posNE(0) = _state.pos(0);
-			_last_known_posNE(1) = _state.pos(1);
-			_fuse_flow = false;
-		}
-
-		// If we are using airspeed measurements and data has fallen behind the fusion time horizon then fuse it
-		if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
-			fuseAirspeed();
-		}
 	}
 
 	// the output observer always runs
@@ -546,11 +395,16 @@ bool Ekf::initialiseFilter(void)
 			baroSample baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt;
 			_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
+			ECL_INFO("EKF using range finder height - commencing alignment");
 
 		} else if (_control_status.flags.ev_hgt) {
 			// if we are using external vision data for height, then the vertical position state needs to be reset
 			// because the initialisation position is not the zero datum
 			resetHeight();
+			ECL_INFO("EKF using vision height - commencing alignment");
+
+		} else if (_control_status.flags.baro_hgt){
+			ECL_INFO("EKF using pressure height - commencing alignment");
 
 		}
 
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 84181828ef..3412d1cf4f 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -187,8 +187,15 @@ private:
 	bool _fuse_pos;			// gps position data should be fused
 	bool _fuse_hor_vel;		// gps horizontal velocity measurement should be fused
 	bool _fuse_vert_vel;		// gps vertical velocity measurement should be fused
-	bool _fuse_flow;		// flow measurement should be fused
-	bool _fuse_hagl_data;		// if true then range data will be fused to estimate terrain height
+
+	// booleans true when fresh sensor data is available at the fusion time horizon
+	bool _gps_data_ready;
+	bool _mag_data_ready;
+	bool _baro_data_ready;
+	bool _range_data_ready;
+	bool _flow_data_ready;
+	bool _ev_data_ready;
+	bool _tas_data_ready;
 
 	uint64_t _time_last_fake_gps;	// last time in us at which we have faked gps measurement for static mode
 
@@ -370,19 +377,28 @@ private:
 	void controlFusionModes();
 
 	// control fusion of external vision observations
-	void controlExternalVisionAiding();
+	void controlExternalVisionFusion();
 
 	// control fusion of optical flow observtions
-	void controlOpticalFlowAiding();
+	void controlOpticalFlowFusion();
 
 	// control fusion of GPS observations
-	void controlGpsAiding();
-
-	// control fusion of height position observations
-	void controlHeightAiding();
+	void controlGpsFusion();
 
 	// control fusion of magnetometer observations
-	void controlMagAiding();
+	void controlMagFusion();
+
+	// control fusion of range finder observations
+	void controlRangeFinderFusion();
+
+	// control fusion of air data observations
+	void controlAirDataFusion();
+
+	// control fusion of pressure altitude observations
+	void controlBaroFusion();
+
+	// control fusion of velocity and position observations
+	void controlVelPosFusion();
 
 	// control for height sensor timeouts, sensor changes and state resets
 	void controlHeightSensorTimeouts();
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 4336ee2ab4..666e60f54e 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -56,21 +56,15 @@ bool Ekf::resetVelocity()
 
 	// reset EKF states
 	if (_control_status.flags.gps) {
-		// if we have a valid GPS measurement use it to initialise velocity states
-		gpsSample gps_newest = _gps_buffer.get_newest();
+		// this reset is only called if we have new gps data at the fusion time horizon
+		_state.vel = _gps_sample_delayed.vel;
 
-		if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) {
-			_state.vel = gps_newest.vel;
-
-		} else {
-			// XXX use the value of the last known velocity
-			return false;
-		}
 	} else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
 		_state.vel.setZero();
 
 	} else {
 		return false;
+
 	}
 
 	// calculate the change in velocity and apply to the output predictor state history
@@ -81,6 +75,7 @@ bool Ekf::resetVelocity()
 		output_states = _output_buffer.get_from_index(index);
 		output_states.vel += velocity_change;
 		_output_buffer.push_to_index(index,output_states);
+
 	}
 
 	// capture the reset event
@@ -103,45 +98,21 @@ bool Ekf::resetPosition()
 	posNE_before_reset(1) = _state.pos(1);
 
 	if (_control_status.flags.gps) {
-		// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
-		gpsSample gps_newest = _gps_buffer.get_newest();
+		// this reset is only called if we have new gps data at the fusion time horizon
+		_state.pos(0) = _gps_sample_delayed.pos(0);
+		_state.pos(1) = _gps_sample_delayed.pos(1);
+		return true;
 
-		float time_delay = 1e-6f * (float)(_imu_sample_delayed.time_us - gps_newest.time_us);
-		float max_time_delay = 1e-6f * (float)GPS_MAX_INTERVAL;
-
-		if (time_delay < max_time_delay) {
-			_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
-			_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
-			return true;
-
-		} else {
-			// XXX use the value of the last known position
-			return false;
-		}
 	} else if (_control_status.flags.opt_flow) {
 		_state.pos(0) = 0.0f;
 		_state.pos(1) = 0.0f;
 		return true;
-	} else if (_control_status.flags.ev_pos) {
-		// if we have fresh data, reset the position to the measurement
-		extVisionSample ev_newest = _ext_vision_buffer.get_newest();
-		if (_time_last_imu - ev_newest.time_us < 2*EV_MAX_INTERVAL) {
-			// use the most recent data if it's time offset from the fusion time horizon is smaller
-			int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
-			int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
-			if (abs(dt_newest) < abs(dt_delayed)) {
-				_state.pos(0) = ev_newest.posNED(0);
-				_state.pos(1) = ev_newest.posNED(1);
-			} else {
-				_state.pos(0) = _ev_sample_delayed.posNED(0);
-				_state.pos(1) = _ev_sample_delayed.posNED(1);
-			}
-			return true;
 
-		} else {
-			// XXX use the value of the last known position
-			return false;
-		}
+	} else if (_control_status.flags.ev_pos) {
+		// this reset is only called if we have new ev data at the fusion time horizon
+		_state.pos(0) = _ev_sample_delayed.posNED(0);
+		_state.pos(1) = _ev_sample_delayed.posNED(1);
+		return true;
 
 	} else {
 		return false;
@@ -844,21 +815,27 @@ Vector3f Ekf::calcRotVecVariances()
 	float t3 = acos(q0);
 	float t4 = -t2+1.0f;
 	float t5 = t2-1.0f;
-	float t6 = 1.0f/t5;
-	float t7 = q1*t6*2.0f;
-	float t8 = 1.0f/powf(t4,1.5f);
-	float t9 = q0*q1*t3*t8*2.0f;
-	float t10 = t7+t9;
-	float t11 = 1.0f/sqrtf(t4);
-	float t12 = q2*t6*2.0f;
-	float t13 = q0*q2*t3*t8*2.0f;
-	float t14 = t12+t13;
-	float t15 = q3*t6*2.0f;
-	float t16 = q0*q3*t3*t8*2.0f;
-	float t17 = t15+t16;
-	rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
-	rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
-	rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
+	if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
+		float t6 = 1.0f/t5;
+		float t7 = q1*t6*2.0f;
+		float t8 = 1.0f/powf(t4,1.5f);
+		float t9 = q0*q1*t3*t8*2.0f;
+		float t10 = t7+t9;
+		float t11 = 1.0f/sqrtf(t4);
+		float t12 = q2*t6*2.0f;
+		float t13 = q0*q2*t3*t8*2.0f;
+		float t14 = t12+t13;
+		float t15 = q3*t6*2.0f;
+		float t16 = q0*q3*t3*t8*2.0f;
+		float t17 = t15+t16;
+		rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
+		rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
+		rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
+	} else {
+		rot_var_vec(0) = 4.0f * P[1][1];
+		rot_var_vec(1) = 4.0f * P[2][2];
+		rot_var_vec(2) = 4.0f * P[3][3];
+	}
 
 	return rot_var_vec;
 }
diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp
index e6b9bc142f..1f7bc293a8 100644
--- a/EKF/optflow_fusion.cpp
+++ b/EKF/optflow_fusion.cpp
@@ -522,30 +522,28 @@ void Ekf::calcOptFlowBias()
 
 	// if accumulation time differences are not excessive and accumulation time is adequate
 	// compare the optical flow and and navigation rate data and calculate a bias error
-	if (_fuse_flow) {
-		if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f)
-		    && (_flow_sample_delayed.dt > 0.01f)) {
-			// calculate a reference angular rate
-			Vector3f reference_body_rate;
-			reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
+	if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f)
+	    && (_flow_sample_delayed.dt > 0.01f)) {
+		// calculate a reference angular rate
+		Vector3f reference_body_rate;
+		reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
 
-			// calculate the optical flow sensor measured body rate
-			Vector3f of_body_rate;
-			of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
+		// calculate the optical flow sensor measured body rate
+		Vector3f of_body_rate;
+		of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
 
-			// calculate the bias estimate using  a combined LPF and spike filter
-			_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
-					     -0.1f, 0.1f);
-			_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
-					     -0.1f, 0.1f);
-			_flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
-					     -0.1f, 0.1f);
-		}
-
-		// reset the accumulators
-		_imu_del_ang_of.setZero();
-		_delta_time_of = 0.0f;
+		// calculate the bias estimate using  a combined LPF and spike filter
+		_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
+				     -0.1f, 0.1f);
+		_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
+				     -0.1f, 0.1f);
+		_flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
+				     -0.1f, 0.1f);
 	}
+
+	// reset the accumulators
+	_imu_del_ang_of.setZero();
+	_delta_time_of = 0.0f;
 }
 
 // calculate the measurement variance for the optical flow sensor (rad/sec)^2