diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index d595fbdd9..db31edb4d 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -635,9 +635,9 @@ void downsample_data()
 	raw.gyrotemp[0] = valid_data_buffer[6];
 	raw.gyrotemp[1] = valid_data_buffer[7];
 
-	raw.gyros_filtered[0] = gyro_data.filtered.x * 180 / M_PI;
-	raw.gyros_filtered[1] = gyro_data.filtered.y * 180 / M_PI;
-	raw.gyros_filtered[2] = gyro_data.filtered.z * 180 / M_PI;
+	raw.gyros_filtered[0] = (gyro_data.filtered.x - Nav.gyro_bias[0]) * 180 / M_PI;
+	raw.gyros_filtered[1] = (gyro_data.filtered.y - Nav.gyro_bias[1]) * 180 / M_PI;
+	raw.gyros_filtered[2] = (gyro_data.filtered.z - Nav.gyro_bias[2]) * 180 / M_PI;
 
 	raw.accels[0] = valid_data_buffer[2];
 	raw.accels[1] = valid_data_buffer[0];
diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h
index 801a27848..b05d8b868 100644
--- a/flight/AHRS/inc/insgps.h
+++ b/flight/AHRS/inc/insgps.h
@@ -79,6 +79,7 @@ struct NavStruct {
 	float Pos[3];		// Position in meters and relative to a local NED frame
 	float Vel[3];		// Velocity in meters and in NED
 	float q[4];		// unit quaternion rotation relative to NED
+	float gyro_bias[3];
 } Nav;
 
 /**
diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c
index b6f5471ef..e428c82b7 100644
--- a/flight/AHRS/insgps.c
+++ b/flight/AHRS/insgps.c
@@ -92,7 +92,7 @@ void INSGPSInit()		//pretty much just a place holder for now
 
 	Q[0] = Q[1] = Q[2] = 50e-8;	// gyro noise variance (rad/s)^2
 	Q[3] = Q[4] = Q[5] = 0.01;	// accelerometer noise variance (m/s^2)^2
-	Q[6] = Q[7] = Q[8] = 2e-6;	// gyro bias random walk variance (rad/s^2)^2
+	Q[6] = Q[7] = Q[8] = 2e-14;	// gyro bias random walk variance (rad/s^2)^2
 
 	R[0] = R[1] = 0.004;	// High freq GPS horizontal position noise variance (m^2)
 	R[2] = 0.036;		// High freq GPS vertical position noise variance (m^2)
@@ -234,6 +234,9 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
 	Nav.q[1] = X[7];
 	Nav.q[2] = X[8];
 	Nav.q[3] = X[9];
+	Nav.gyro_bias[0] = X[10];
+	Nav.gyro_bias[1] = X[11];
+	Nav.gyro_bias[2] = X[12];	
 }
 
 void INSCovariancePrediction(float dT)