diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h
index 3719aec7ce..7add1dedef 100644
--- a/src/modules/ekf2/EKF/EKFGSF_yaw.h
+++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h
@@ -67,11 +67,11 @@ public:
 
 	void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }
 
-	void setGyroBias(const Vector3f &imu_gyro_bias)
+	void setGyroBias(const Vector3f &imu_gyro_bias, const bool force = false)
 	{
 		// Initialise to gyro bias estimate from main filter because there could be a large
 		// uncorrected rate gyro bias error about the gravity vector
-		if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started) {
+		if (!_ahrs_ekf_gsf_tilt_aligned || !_ekf_gsf_vel_fuse_started || force) {
 			// init gyro bias for each model
 			for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
 				_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp
index d86020815a..cf2f9eb7b4 100644
--- a/src/modules/ekf2/EKF/gps_control.cpp
+++ b/src/modules/ekf2/EKF/gps_control.cpp
@@ -47,7 +47,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	}
 
 	if (!gyro_bias_inhibited()) {
-		_yawEstimator.setGyroBias(getGyroBias());
+		_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);
 	}
 
 	// run EKF-GSF yaw estimator once per imu_delayed update