ekf2: cleanup zero innovation heading fusion

- refactor updateQuaternion() to compute the yaw jacobian directly (respecting  the rotation sequence determination)
 - fuseHeading()/fuseYaw321()/fuseYaw312() helpers are eliminated and now mag heading fusion and EV yaw fusion compute the innovation in place
 - clear up logic for performing zero innovation heading fusion when quaternion variance exceeds threshold (no more _is_yaw_fusion_inhibited flag manipulation)
 - when at rest continue fusing last static heading with very low variance even if other heading sources are active

src/modules/ekf2/EKF/control.cpp

@@ -190,6 +190,8 @@ void Ekf::controlFusionModes()
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
 	controlAuxVelFusion();
 
+	controlZeroInnovationHeadingUpdate();
+
 	controlZeroVelocityUpdate();
 
 	// Fake position measurement for constraining drift when no other velocity or position measurements
@@ -364,14 +366,11 @@ void Ekf::controlExternalVisionFusion()
 				resetYawToEv();
 			}
 
-			if (shouldUse321RotationSequence(_R_to_earth)) {
-				float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
-				fuseYaw321(measured_hdg, _ev_sample_delayed.angVar);
+			float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
 
-			} else {
-				float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
-				fuseYaw312(measured_hdg, _ev_sample_delayed.angVar);
-			}
+			float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
+			float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
+			fuseYaw(innovation, obs_var);
 		}
 
 		// record observation and estimate for use next time