diff --git a/EKF/common.h b/EKF/common.h
index f8840b5267..f7ac319286 100644
--- a/EKF/common.h
+++ b/EKF/common.h
@@ -71,6 +71,13 @@ struct flow_message {
 	uint32_t dt;				// integration time of flow samples
 };
 
+struct ext_vision_message {
+	Vector3f posNED;  // measured NED position relative to the local origin (m)
+	Quaternion quat;  // measured quaternion orientation defining rotation from NED to body frame
+	float posErr;     // 1-Sigma spherical position accuracy (m)
+	float angErr;     // 1-Sigma angular error (rad)
+};
+
 struct outputSample {
 	Quaternion  quat_nominal;	// nominal quaternion describing vehicle attitude
 	Vector3f    vel;	// NED velocity estimate in earth frame in m/s
@@ -126,10 +133,19 @@ struct flowSample {
 	uint64_t time_us; // timestamp in microseconds of the integration period mid-point
 };
 
+struct extVisionSample {
+	Vector3f posNED;  // measured NED position relative to the local origin (m)
+	Quaternion quat;  // measured quaternion orientation defining rotation from NED to body frame
+	float posErr;     // 1-Sigma spherical position accuracy (m)
+	float angErr;     // 1-Sigma angular error (rad)
+	uint64_t time_us; // timestamp of the measurement in microseconds
+};
+
 // Integer definitions for vdist_sensor_type
 #define VDIST_SENSOR_BARO  0	// Use baro height
 #define VDIST_SENSOR_GPS   1	// Use GPS height
 #define VDIST_SENSOR_RANGE 2	// Use range finder height
+#define VDIST_SENSOR_EV    3    // USe external vision
 
 // Bit locations for mag_declination_source
 #define MASK_USE_GEO_DECL   (1<<0)  // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
@@ -140,6 +156,8 @@ struct flowSample {
 #define MASK_USE_GPS    (1<<0)  // set to true to use GPS data
 #define MASK_USE_OF     (1<<1)  // set to true to use optical flow data
 #define MASK_INHIBIT_ACC_BIAS (1<<2)  // set to true to inhibit estimation of accelerometer delta velocity bias
+#define MASK_USE_EVPOS	(1<<3)  // set to true to use external vision NED position data
+#define MASK_USE_EVYAW  (1<<4)	// set to true to use exernal vision quaternion data for yaw
 
 // Integer definitions for mag_fusion_type
 #define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
@@ -150,6 +168,7 @@ struct flowSample {
 #define GPS_MAX_INTERVAL	5e5
 #define BARO_MAX_INTERVAL	2e5
 #define RNG_MAX_INTERVAL	2e5
+#define EV_MAX_INTERVAL		2e5
 
 struct parameters {
 	// measurement source control
@@ -163,6 +182,7 @@ struct parameters {
 	float airspeed_delay_ms;	// airspeed measurement delay relative to the IMU (msec)
 	float flow_delay_ms;		// optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
 	float range_delay_ms;		// range finder measurement delay relative to the IMU (msec)
+	float ev_delay_ms;		// off-board vision measurement delay relative to the IMU (msec)
 
 	// input noise
 	float gyro_noise;		// IMU angular rate noise used for covariance prediction (rad/sec)
@@ -205,6 +225,11 @@ struct parameters {
 	float range_innov_gate;		// range finder fusion innovation consistency gate size (STD)
 	float rng_gnd_clearance;	// minimum valid value for range when on ground (m)
 
+	// vision position fusion
+	float ev_noise;		// observation noise for vision sensor estimates (m)
+	float ev_innov_gate;		// vision estimator fusion innovation consistency gate size (STD)
+	float ev_gnd_clearance;	// minimum valid value for vision based height estimate when on ground (m)
+
 	// optical flow fusion
 	float flow_noise;		// observation noise for optical flow LOS rate measurements (rad/sec)
 	float flow_noise_qual_min;	// observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
@@ -228,6 +253,7 @@ struct parameters {
 	Vector3f gps_pos_body;	// xyz position of the GPS antenna in body frame (m)
 	Vector3f rng_pos_body;	// xyz position of range sensor in body frame (m)
 	Vector3f flow_pos_body;	// xyz position of range sensor focal point in body frame (m)
+	Vector3f ev_pos_body;	// xyz position of VI-sensor focal point in body frame (m)
 
 	// output complementary filter tuning
 	float vel_Tau;	// velocity state correction time constant (1/sec)
@@ -247,6 +273,7 @@ struct parameters {
 		airspeed_delay_ms = 200.0f;
 		flow_delay_ms = 5.0f;
 		range_delay_ms = 5.0f;
+		ev_delay_ms = 100.0f;
 
 		// input noise
 		gyro_noise = 1.5e-2f;
@@ -311,10 +338,12 @@ struct parameters {
 		gps_pos_body = {};
 		rng_pos_body = {};
 		flow_pos_body = {};
+		ev_pos_body = {};
 
 		// output complementary filter tuning time constants
 		vel_Tau = 0.5f;
 		pos_Tau = 0.25f;
+
 	}
 };
 
@@ -383,6 +412,8 @@ union filter_control_status_u {
 		uint16_t baro_hgt    : 1; // 9 - true when baro height is being fused as a primary height reference
 		uint16_t rng_hgt     : 1; // 10 - true when range finder height is being fused as a primary height reference
 		uint16_t gps_hgt     : 1; // 11 - true when range finder height is being fused as a primary height reference
+		uint16_t ev_pos      : 1; // 12 - true when local position data from external vision is being fused
+		uint16_t ev_yaw      : 1; // 13 - true when yaw data from external vision measurements is being fused
 	} flags;
 	uint16_t value;
 };
diff --git a/EKF/control.cpp b/EKF/control.cpp
index 2830d696dc..006e933564 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -56,6 +56,71 @@ void Ekf::controlFusionModes()
 		_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 	}
 
+	// control use of various external souces for positon and velocity aiding
+	controlExternalVisionAiding();
+	controlOpticalFlowAiding();
+	controlGpsAiding();
+	controlHeightAiding();
+	controlMagAiding();
+
+}
+
+void Ekf::controlExternalVisionAiding()
+{
+	// 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;
+			printf("EKF switching to external vision position fusion\n");
+			// 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);
+
+			// calculate initial quaternion states for the ekf
+			_state.quat_nominal = Quaternion(euler_init);
+
+			// 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;
+
+			printf("EKF switching to external vision yaw fusion\n");
+		}
+	}
+
+}
+
+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
@@ -127,6 +192,22 @@ void Ekf::controlFusionModes()
 		_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();
+		}
+	}
+}
+
+void Ekf::controlGpsAiding()
+{
 	// 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) {
@@ -177,7 +258,10 @@ void Ekf::controlFusionModes()
 			}
 		}
 	}
+}
 
+void Ekf::controlHeightSensorTimeouts()
+{
 	/*
 	 * Handle the case where we have not fused height measurements recently and
 	 * uncertainty exceeds the max allowable. Reset using the best available height
@@ -351,18 +435,40 @@ void Ekf::controlFusionModes()
 		}
 
 	}
+}
 
-	// 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 veloity 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();
-		}
+void Ekf::controlHeightAiding()
+{
+	// check for height sensor timeouts and reset and change sensor if necessary
+	controlHeightSensorTimeouts();
+
+	// Control the soure of height measurements for the main filter
+	if (_control_status.flags.ev_pos) {
+	       _control_status.flags.baro_hgt = false;
+	       _control_status.flags.gps_hgt = false;
+	       _control_status.flags.rng_hgt = false;
+	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
+		_control_status.flags.baro_hgt = true;
+		_control_status.flags.gps_hgt = false;
+		_control_status.flags.rng_hgt = false;
+
+	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
+		_control_status.flags.baro_hgt = false;
+		_control_status.flags.gps_hgt = true;
+		_control_status.flags.rng_hgt = false;
+
+	} else 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;
+	}
+}
+
+void Ekf::controlMagAiding()
+{
+	// 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
@@ -415,28 +521,10 @@ void Ekf::controlFusionModes()
 		_control_status.flags.mag_dec = false;
 	}
 
-	// Control the soure of height measurements for the main filter
-	if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
-		_control_status.flags.baro_hgt = true;
-		_control_status.flags.gps_hgt = false;
-		_control_status.flags.rng_hgt = false;
-
-	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
-		_control_status.flags.baro_hgt = false;
-		_control_status.flags.gps_hgt = true;
-		_control_status.flags.rng_hgt = false;
-
-	} else 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;
-	}
-
 	// 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 5578f38ec6..e15cb646a5 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -303,6 +303,26 @@ bool Ekf::update()
 
 		}
 
+		// 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
@@ -315,8 +335,6 @@ bool Ekf::update()
 		if (!_control_status.flags.gps && !_control_status.flags.opt_flow
 		    && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
 			_fuse_pos = true;
-			_gps_sample_delayed.pos(0) = _last_known_posNE(0);
-			_gps_sample_delayed.pos(1) = _last_known_posNE(1);
 			_time_last_fake_gps = _time_last_imu;
 		}
 
@@ -390,7 +408,11 @@ bool Ekf::initialiseFilter(void)
 
 	// set the default height source from the adjustable parameter
 	if (_hgt_counter == 0) {
-		_primary_hgt_source = _params.vdist_sensor_type;
+		if (_params.fusion_mode & MASK_USE_EVPOS) {
+			_primary_hgt_source = VDIST_SENSOR_EV;
+		} else {
+			_primary_hgt_source = _params.vdist_sensor_type;
+		}
 	}
 
 	if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
@@ -409,7 +431,7 @@ bool Ekf::initialiseFilter(void)
 			}
 		}
 
-	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
+	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) {
 		// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
 		// later when it passes checks.
 		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
@@ -550,7 +572,7 @@ void Ekf::predictState()
 
 	// filter and limit input between -50% and +100% of nominal value
 	input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS));
-	_dt_ekf_avg = 0.99f*_dt_ekf_avg + 0.005f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
+	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
 }
 
 bool Ekf::collect_imu(imuSample &imu)
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 85d85d94c6..554cd74977 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -330,6 +330,24 @@ private:
 	// Control the filter fusion modes
 	void controlFusionModes();
 
+	// control fusion of external vision observations
+	void controlExternalVisionAiding();
+
+	// control fusion of optical flow observtions
+	void controlOpticalFlowAiding();
+
+	// control fusion of GPS observations
+	void controlGpsAiding();
+
+	// control fusion of height position observations
+	void controlHeightAiding();
+
+	// control fusion of magnetometer observations
+	void controlMagAiding();
+
+	// control for height sensor timeouts, sensor changes and state resets
+	void controlHeightSensorTimeouts();
+
 	// return the square of two floating point numbers - used in auto coded sections
 	inline float sq(float var)
 	{
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index d1d6d85c63..0e3ac8b008 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -51,15 +51,22 @@
 // gps measurement then use for velocity initialisation
 bool Ekf::resetVelocity()
 {
-	// if we have a valid GPS measurement use it to initialise velocity states
-	gpsSample gps_newest = _gps_buffer.get_newest();
+	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();
 
-	if (_time_last_imu - gps_newest.time_us < 400000) {
-		_state.vel = gps_newest.vel;
+		if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) {
+			_state.vel = gps_newest.vel;
+			return true;
+
+		} 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();
 		return true;
-
 	} else {
-		// XXX use the value of the last known velocity
 		return false;
 	}
 }
@@ -68,18 +75,48 @@ bool Ekf::resetVelocity()
 // gps measurement then use for position initialisation
 bool Ekf::resetPosition()
 {
-	// 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();
+	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();
 
-	float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us);
+		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 < 0.4f) {
-		_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;
+		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 {
-		// XXX use the value of the last known position
 		return false;
 	}
 }
@@ -116,13 +153,14 @@ void Ekf::resetHeight()
 
 			vert_pos_reset = true;
 
+			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
+			baroSample baro_newest = _baro_buffer.get_newest();
+			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+
 		} else {
 			// TODO: reset to last known range based estimate
 		}
 
-		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-		baroSample baro_newest = _baro_buffer.get_newest();
-		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
 
 	} else if (_control_status.flags.baro_hgt) {
 		// initialize vertical position with newest baro measurement
@@ -158,13 +196,24 @@ void Ekf::resetHeight()
 
 			vert_pos_reset = true;
 
+			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
+			baroSample baro_newest = _baro_buffer.get_newest();
+			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+
 		} else {
 			// TODO: reset to last known gps based estimate
 		}
 
-		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-		baroSample baro_newest = _baro_buffer.get_newest();
-		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+	} else if (_control_status.flags.ev_pos) {
+		// initialize vertical position with newest measurement
+		extVisionSample ev_newest = _ext_vision_buffer.get_newest();
+
+		if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) {
+			_state.pos(2) = ev_newest.posNED(2);
+
+		} else {
+			// TODO: reset to last known baro based estimate
+		}
 	}
 
 	// reset the vertical velocity covariance values
diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index 65bfa295c7..7ebd465abe 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -65,6 +65,7 @@ EstimatorInterface::EstimatorInterface():
 	_time_last_baro(0),
 	_time_last_range(0),
 	_time_last_airspeed(0),
+	_time_last_ext_vision(0),
 	_mag_declination_gps(0.0f),
 	_mag_declination_to_save_deg(0.0f)
 {
@@ -284,6 +285,30 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 	}
 }
 
+// set attitude and position data derived from an external vision system
+void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// if data passes checks, push to buffer
+	if (time_usec > _time_last_ext_vision) {
+		extVisionSample ev_sample_new;
+		// calculate the system time-stamp for the mid point of the integration period
+		ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
+		// copy required data
+		ev_sample_new.angErr = evdata->angErr;
+		ev_sample_new.posErr = evdata->posErr;
+		ev_sample_new.quat = evdata->quat;
+		ev_sample_new.posNED = evdata->posNED;
+		// record time for comparison next measurement
+		_time_last_ext_vision = time_usec;
+		// push to buffer
+		_ext_vision_buffer.push(ev_sample_new);
+	}
+}
+
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
 
@@ -294,6 +319,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	      _range_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
+	      _ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _output_buffer.allocate(IMU_BUFFER_LENGTH))) {
 		printf("EKF buffer allocation failed!");
 		unallocate_buffers();
@@ -314,6 +340,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		_airspeed_buffer.push(airspeed_sample_init);
 		flowSample flow_sample_init = {};
 		_flow_buffer.push(flow_sample_init);
+		extVisionSample ext_vision_sample_init = {};
+		_ext_vision_buffer.push(ext_vision_sample_init);
 	}
 
 	// zero the data in the imu data and output observer state buffers
@@ -343,8 +371,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	_time_last_range = 0;
 	_time_last_airspeed = 0;
 	_time_last_optflow = 0;
-
 	memset(&_fault_status.flags, 0, sizeof(_fault_status.flags));
+	_time_last_ext_vision = 0;
 	return true;
 }
 
@@ -357,6 +385,7 @@ void EstimatorInterface::unallocate_buffers()
 	_range_buffer.unallocate();
 	_airspeed_buffer.unallocate();
 	_flow_buffer.unallocate();
+	_ext_vision_buffer.unallocate();
 	_output_buffer.unallocate();
 
 }
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index bd2ecf40a0..e4c09b125f 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -145,6 +145,9 @@ public:
 	// set optical flow data
 	void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
 
+	// set external vision position and attitude data
+	void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
+
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() {return &_params;}
@@ -245,6 +248,7 @@ protected:
 	rangeSample _range_sample_delayed;
 	airspeedSample _airspeed_sample_delayed;
 	flowSample _flow_sample_delayed;
+	extVisionSample _ev_sample_delayed;
 
 	outputSample _output_sample_delayed;	// filter output on the delayed time horizon
 	outputSample _output_new;	// filter output on the non-delayed time horizon
@@ -277,6 +281,7 @@ protected:
 	RingBuffer<rangeSample> _range_buffer;
 	RingBuffer<airspeedSample> _airspeed_buffer;
 	RingBuffer<flowSample> 	_flow_buffer;
+	RingBuffer<extVisionSample> _ext_vision_buffer;
 	RingBuffer<outputSample> _output_buffer;
 
 	uint64_t _time_last_imu;	// timestamp of last imu sample in microseconds
@@ -285,6 +290,7 @@ protected:
 	uint64_t _time_last_baro;	// timestamp of last barometer measurement in microseconds
 	uint64_t _time_last_range;	// timestamp of last range measurement in microseconds
 	uint64_t _time_last_airspeed;	// timestamp of last airspeed measurement in microseconds
+	uint64_t _time_last_ext_vision; // timestamp of last external vision measurement in microseconds
 	uint64_t _time_last_optflow;
 
 	fault_status_u _fault_status;
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index 88c133deae..d93e416159 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -379,12 +379,11 @@ void Ekf::fuseHeading()
 	float q2 = _state.quat_nominal(2);
 	float q3 = _state.quat_nominal(3);
 
-	float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
-	R_YAW = R_YAW * R_YAW;
-
+	float R_YAW = 1.0f;
 	float predicted_hdg;
 	float H_YAW[4];
 	matrix::Vector3f mag_earth_pred;
+	float measured_hdg;
 
 	// determine if a 321 or 312 Euler sequence is best
 	if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
@@ -426,8 +425,21 @@ void Ekf::fuseHeading()
 		euler321(2) = 0.0f;
 		matrix::Dcm<float> R_to_earth(euler321);
 
-		// rotate the magnetometer measurements into earth frame using a zero yaw angle
-		mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+		// calculate the observed yaw angle
+		if (_control_status.flags.mag_hdg) {
+			// rotate the magnetometer measurements into earth frame using a zero yaw angle
+			mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+			// the angle of the projection onto the horizontal gives the yaw angle
+			measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
+		} else if (_control_status.flags.ev_yaw) {
+			// convert the observed quaternion to a rotation matrix
+			matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
+			// calculate the yaw angle for a 312 sequence
+			measured_hdg = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
+		} else {
+			// there is no yaw observation
+			return;
+		}
 
 	} else {
 		// calculate observaton jacobian when we are observing a rotation in a 312 sequence
@@ -491,15 +503,39 @@ void Ekf::fuseHeading()
 		R_to_earth(2, 0) = -s2 * c1;
 		R_to_earth(2, 1) = s1;
 
-		// rotate the magnetometer measurements into earth frame using a zero yaw angle
-		mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+		// calculate the observed yaw angle
+		if (_control_status.flags.mag_hdg) {
+			// rotate the magnetometer measurements into earth frame using a zero yaw angle
+			mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+			// the angle of the projection onto the horizontal gives the yaw angle
+			measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
+		} else if (_control_status.flags.ev_yaw) {
+			// convert the observed quaternion to a rotation matrix
+			matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat);	// transformation matrix from body to world frame
+			// calculate the yaw angle for a 312 sequence
+			measured_hdg = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
+		} else {
+			// there is no yaw observation
+			return;
+		}
+	}
+
+	// Calculate the observation variance
+	if (_control_status.flags.mag_hdg) {
+		// using magnetic heading tuning parameter
+		R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+	} else if (_control_status.flags.ev_yaw) {
+		// using error estimate from external vision data
+		R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
+	} else {
+		// there is no yaw observation
+		return;
 	}
 
 	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
 	// calculate the innovaton variance
 	float PH[4];
 	_heading_innov_var = R_YAW;
-
 	for (unsigned row = 0; row <= 3; row++) {
 		PH[row] = 0.0f;
 
@@ -553,9 +589,6 @@ void Ekf::fuseHeading()
 		}
 	}
 
-	// Use the difference between the horizontal projection of the mag field and declination to give the measured heading
-	float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
-
 	// wrap the heading to the interval between +-pi
 	measured_hdg = matrix::wrap_pi(measured_hdg);
 
diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp
index 433b6eeb1a..c71e014b7d 100644
--- a/EKF/vel_pos_fusion.cpp
+++ b/EKF/vel_pos_fusion.cpp
@@ -83,24 +83,31 @@ void Ekf::fuseVelPosHeight()
 
 	if (_fuse_pos) {
 		fuse_map[3] = fuse_map[4] = true;
-		// horizontal position innovations
-		_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
-		_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
 
-		// observation variance - user parameter defined
-		// if we are in flight and not using GPS, then use a specific parameter
-		if (!_control_status.flags.gps) {
+		// Calculate innovations and observation variance depending on type of observations
+		// being used
+		if (!_control_status.flags.gps && !_control_status.flags.ev_pos) {
+			// No observations - use a static position to constrain drift
 			if (_control_status.flags.in_air) {
 				R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
 
 			} else {
 				R[3] = _params.gps_pos_noise;
 			}
+			_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
+			_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
 
-		} else {
+		} else if (_control_status.flags.gps) {
 			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
 			R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
+			_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
+			_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
+
+		} else if (_control_status.flags.ev_pos) {
+			R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
+			_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
+			_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
 
 		}
 
@@ -145,6 +152,15 @@ void Ekf::fuseVelPosHeight()
 			R[5] = R[5] * R[5];
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
+		} else if (_control_status.flags.ev_pos) {
+			fuse_map[5] = true;
+			// calculate the innovation assuming the external vision observaton is in local NED frame
+			_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
+			// observation variance - defined externally
+			R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
+			R[5] = R[5] * R[5];
+			// innovation gate size
+			gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
 		}
 
 	}