EKF: Fix bugs preventing use of external vision yaw data

EKF/common.h

@@ -163,7 +163,6 @@ struct extVisionSample {
 #define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
 #define MAG_FUSE_TYPE_HEADING   1   // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
 #define MAG_FUSE_TYPE_3D        2   // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
-#define USE_EV_YAW		3   // Fuse yaw angle from external vision system
 
 // Maximum sensor intervals in usec
 #define GPS_MAX_INTERVAL	5e5

EKF/control.cpp

@@ -86,7 +86,7 @@ void Ekf::controlExternalVisionAiding()
 	}
 
 	// external vision yaw aiding selection logic
-	if ((_params.mag_fusion_type == USE_EV_YAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
+	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
@@ -107,8 +107,12 @@ void Ekf::controlExternalVisionAiding()
 			// flag the yaw as aligned
 			_control_status.flags.yaw_align = true;
 
-			// turn on fusion of external vision yaw measurements
+			// 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");
 		}
 	}
@@ -462,6 +466,11 @@ void Ekf::controlHeightAiding()
 
 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
 	// or the more accurate 3-axis fusion
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {

EKF/mag_fusion.cpp

@@ -379,9 +379,7 @@ 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);
+	float R_YAW = 1.0f;
-	R_YAW = R_YAW * R_YAW;
-
 	float predicted_hdg;
 	float H_YAW[4];
 	matrix::Vector3f mag_earth_pred;
@@ -522,11 +520,22 @@ void Ekf::fuseHeading()
 		}
 	}
 
+	// 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;