GNSS yaw: use NIS sequence to detect bias in state

A constant large value in the (signed) normalized innovation test ratio is a sign
of bias in the state estimate. This metric can be used to trigger a
covariance boost or reset

Author: bresch

src/modules/ekf2/EKF/estimator_interface.h

@@ -328,6 +328,7 @@ class EstimatorInterface
 
 	// innovation consistency check monitoring ratios
 	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
+	AlphaFilter<float>_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 	Vector3f _mag_test_ratio{};		// magnetometer XYZ innovation consistency check ratios
 	Vector2f _gps_vel_test_ratio{};		// GPS velocity innovation consistency check ratios
 	Vector2f _gps_pos_test_ratio{};		// GPS position innovation consistency check ratios

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -151,23 +151,23 @@ void Ekf::fuseGpsYaw()
 
 	if (_yaw_test_ratio > 1.0f) {
 		_innov_check_fail_status.flags.reject_yaw = true;
-
-		// if we are in air we don't want to fuse the measurement
-		// we allow to use it when on the ground because the large innovation could be caused
-		// by interference or a large initial gyro bias
-		if (_control_status.flags.in_air) {
-			return;
-
-		} else {
-			// constrain the innovation to the maximum set by the gate
-			const float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var));
-			_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
-		}
+		return;
 
 	} else {
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
+	_yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio);
+
+	if (!_control_status.flags.in_air
+	    && fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f) {
+
+		// A constant large signed test ratio is a sign of wrong gyro bias
+		// Reset the yaw gyro variance to converge faster and avoid
+		// being stuck on a previous bad estimate
+		resetZDeltaAngBiasCov();
+	}
+
 	// calculate observation jacobian
 	// Observation jacobian and Kalman gain vectors
 	SparseVector24f<0,1,2,3> Hfusion;
@@ -213,6 +213,7 @@ bool Ekf::resetYawToGps()
 	resetQuatStateYaw(measured_yaw, yaw_variance, true);
 
 	_time_last_gps_yaw_fuse = _time_last_imu;
+	_yaw_signed_test_ratio_lpf.reset(0.f);
 
 	return true;
 }

src/modules/ekf2/test/test_EKF_gps_yaw.cpp

@@ -141,7 +141,7 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 	// AND WHEN: the the measurement changes
 	gps_heading += math::radians(2.f);
 	_sensor_simulator._gps.setYaw(gps_heading);
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(20);
 
 	// THEN: the estimate slowly converges to the new measurement
 	// Note that the process is slow, because the gyro did not detect any motion
@@ -225,11 +225,12 @@ TEST_F(EkfGpsHeadingTest, yaw_jump_on_ground)
 	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
 	gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f));
 	_sensor_simulator._gps.setYaw(gps_heading);
-	_sensor_simulator.runSeconds(2);
+	_sensor_simulator.runSeconds(8);
 
 	// THEN: the fusion should reset
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+	EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f));
 }
 
 TEST_F(EkfGpsHeadingTest, yaw_jump_in_air)