EKF: Rework quaternion yaw reset.

Use a new method that preserves the roll and pitch information and adds the uncertainty for yaw only.
Ensure that correlation information to non-quaternion states is removed when a reset occurs to prevent fusion of subsequent observations (e.g. GPS) causing incorrect yaw.

EKF/control.cpp

@@ -215,6 +215,10 @@ void Ekf::controlExternalVisionFusion()
 
 				// calculate initial quaternion states for the ekf
 				_state.quat_nominal = Quatf(euler_init);
+				uncorrelateQuatStates();
+
+				// adjust the quaternion covariances estimated yaw error
+				increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
 
 				// calculate the amount that the quaternion has changed by
 				_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;