EKF: Reduce EKF-GSF vulnerability to large yaw gyro bias errors (#831)

* EKF: Reduce EKF-GSF vulnerability to large yaw gyro bias errors

* Update EKF/ekf_helper.cpp

* EKF: Fix comment typo

Co-authored-by: Roman Bapst <bapstroman@gmail.com>

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>

EKF/EKFGSF_yaw.cpp

@@ -14,8 +14,9 @@ EKFGSF_yaw::EKFGSF_yaw()
 }
 
 void EKFGSF_yaw::update(const imuSample& imu_sample,
-                	bool run_EKF,           // set to true when flying or movement is suitable for yaw estimation
+			bool run_EKF,			// set to true when flying or movement is suitable for yaw estimation
-                	float airspeed)   	// true airspeed used for centripetal accel compensation - set to 0 when not required.
+			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
+			const Vector3f &imu_gyro_bias)  // estimated rate gyro bias (rad/sec)
 {
 	// copy to class variables
 	_delta_ang = imu_sample.delta_ang;
@@ -60,6 +61,11 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 		if (!_ekf_gsf_vel_fuse_started) {
 			initialiseEKFGSF();
 			ahrsAlignYaw();
+			// Initialise to gyro bias estimate from main filter because there could be a large
+			// uncorrected rate gyro bias error about the gravity vector
+			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
+				_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
+			}
 			_ekf_gsf_vel_fuse_started = true;
 		} else {
 			bool bad_update = false;

EKF/EKFGSF_yaw.h

@@ -31,11 +31,12 @@ public:
 
     	// Update Filter States - this should be called whenever new IMU data is available
 	void update(const imuSample &imu_sample,
-                	bool run_EKF,           // set to true when flying or movement is suitable for yaw estimation
+			bool run_EKF,  			// set to true when flying or movement is suitable for yaw estimation
-                	float airspeed);   	// true airspeed used for centripetal accel compensation - set to 0 when not required.
+			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
+			const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
 
-	void setVelocity(const Vector2f &velocity,	// NE velocity measurement (m/s)
+	void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
-                     	float accuracy);	// 1-sigma accuracy of velocity measurement (m/s)
+			float accuracy);	   // 1-sigma accuracy of velocity measurement (m/s)
 
 	// get solution data for logging
 	bool getLogData(float *yaw_composite,

EKF/ekf_helper.cpp

@@ -1858,7 +1858,8 @@ void Ekf::runYawEKFGSF()
 		TAS = _airspeed_sample_delayed.true_airspeed;
 	}
 
-	yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS);
+	const Vector3f imu_gyro_bias = getGyroBias();
+	yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
 
 	// basic sanity check on GPS velocity data
 	if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && ISFINITE(_gps_sample_delayed.vel(0)) && ISFINITE(_gps_sample_delayed.vel(1))) {