heading_fusion: fix yaw gyro bias oscillation on ground

When the mag is pulled away before takeoff, the EKF can end up learning
a bad yaw gyro bias. This bad gyro bias makes the estimated heading spin
and even if the bias continues to be learned, it can end up being stuck
in limit cycle due to the innovation changing its sign every time the
heading estimate overshoots the measurement.
To prevent this, the Z gyro bias variance is reset when the mag heading
fusion test ratio fails on ground.

EKF/covariance.cpp

@@ -1084,6 +1084,13 @@ void Ekf::zeroMagCov()
 	P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
 }
 
+void Ekf::resetZDeltaAngBiasCov()
+{
+	const float init_delta_ang_bias_var = sq(_params.switch_on_gyro_bias * _dt_ekf_avg);
+
+	P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var);
+}
+
 void Ekf::resetWindCovariance()
 {
 	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {

EKF/ekf.h

@@ -926,6 +926,8 @@ private:
 	void clearMagCov();
 	void zeroMagCov();
 
+	void resetZDeltaAngBiasCov();
+
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatFromOtherStates();
 

EKF/mag_fusion.cpp

@@ -653,6 +653,10 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 			float gate_limit = sqrtf((sq(gate_sigma) * _heading_innov_var));
 			_heading_innov = math::constrain(innovation, -gate_limit, gate_limit);
 
+			// also reset the yaw gyro variance to converge faster and avoid
+			// being stuck on a previous bad estimate
+			resetZDeltaAngBiasCov();
+
 		} else {
 			return;
 		}