EKF: Add fusion of external yaw data

EKF/common.h

@@ -163,6 +163,7 @@ 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

@@ -70,9 +70,30 @@ void Ekf::controlFusionModes()
 	}
 
 	// external vision yaw aiding selection logic
-	if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align
-			&& (_time_last_imu - _time_last_ext_vision) < 5e5) {
-		//TODO
+	if ((_params.mag_fusion_type == USE_EV_YAW) && !_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
+			_control_status.flags.ev_yaw = true;
+		}
 	}
 
 	// optical flow fusion mode selection logic

EKF/ekf.cpp

@@ -310,6 +310,10 @@ bool Ekf::update()
 				_fuse_pos = true;
 				_fuse_height = true;
 			}
+			// 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

EKF/mag_fusion.cpp

@@ -385,6 +385,7 @@ void Ekf::fuseHeading()
 	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 +427,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_sample_delayed.quat);	// transformation matrix from body to world frame
+			// calculate the yaw angle for a 312 sequence
+			measured_hdg = atan2f(R_to_earth(1, 0) , R_to_earth(0, 0));
+		} else {
+			// there is no yaw observation
+			return;
+		}
 
 	} else {
 		// calculate observaton jacobian when we are observing a rotation in a 312 sequence
@@ -491,8 +505,21 @@ 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_sample_delayed.quat);	// transformation matrix from body to world frame
+			// calculate the yaw angle for a 312 sequence
+			measured_hdg = atan2f(-R_to_earth(0, 1) , R_to_earth(1, 1));
+		} 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
@@ -553,9 +580,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);