EKF: Reset angle error covariance after yaw and mag field reset

The yaw angle could have changed by a significant amount making the correlations invalid.
Setting angle variances to zero prevents the initial kick in angles due to 3D fusion starting

EKF/ekf_helper.cpp

@@ -117,6 +117,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	_state.quat_nominal = Quaternion(euler_init);
 	_output_new.quat_nominal = _state.quat_nominal;
 
+	// reset the angle error variances because the yaw angle could have changed by a significant amount
+	// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
+	// will grow them again.
+	zeroRows(P, 0, 2);
+	zeroCols(P, 0, 2);
+
 	// calculate initial earth magnetic field states
 	matrix::Dcm<float> R_to_earth(euler_init);
 	_state.mag_I = R_to_earth * mag_init;