diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 5e0bbcdd6..8da48e2ce 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -556,6 +556,8 @@ void downsample_data()
  * sense for the z accel so make sure 6 point calibration is also run and those values set
  * after these read.
  */
+#define NBIAS 100
+#define NVAR  500
 void calibrate_sensors()
 {
 	int i,j;
@@ -563,16 +565,35 @@ void calibrate_sensors()
 	float gyro_bias[3] = {0, 0, 0};
 	float mag_bias[3] = {0, 0, 0};
 
-	// run few loops to get mean
-	gyro_data.calibration.bias[0] = 0;
-	gyro_data.calibration.bias[1] = 0;
-	gyro_data.calibration.bias[2] = 0;
-	accel_data.calibration.bias[0] = 0;
-	accel_data.calibration.bias[1] = 0;
-	accel_data.calibration.bias[2] = 0;
-	mag_data.calibration.bias[0] = 0;
-	mag_data.calibration.bias[1] = 0;
-	mag_data.calibration.bias[2] = 0;
+	for (i = 0, j = 0; i < NBIAS; i++) {
+		while (ahrs_state != AHRS_DATA_READY) ;
+		ahrs_state = AHRS_PROCESSING;
+		downsample_data();
+		gyro_bias[0] += gyro_data.filtered.x / NBIAS;
+		gyro_bias[1] += gyro_data.filtered.y / NBIAS;
+		gyro_bias[2] += gyro_data.filtered.z / NBIAS;
+		accel_bias[0] += accel_data.filtered.x / NBIAS;
+		accel_bias[1] += accel_data.filtered.y / NBIAS;
+		accel_bias[2] += accel_data.filtered.z / NBIAS;
+		ahrs_state = AHRS_IDLE;
+		
+#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
+		if(PIOS_HMC5843_NewDataAvailable()) {
+			j ++;
+			PIOS_HMC5843_ReadMag(mag_data.raw.axis);
+			mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
+			mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
+			mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
+			mag_bias[0] += mag_data.scaled.axis[0];
+			mag_bias[1] += mag_data.scaled.axis[1];
+			mag_bias[2] += mag_data.scaled.axis[2];
+		}
+#endif
+		process_spi_request();
+	}
+	mag_bias[0] /= j;
+	mag_bias[1] /= j;
+	mag_bias[2] /= j;
 	
 	gyro_data.calibration.variance[0] = 0;
 	gyro_data.calibration.variance[1] = 0;
@@ -584,24 +605,17 @@ void calibrate_sensors()
 	accel_data.calibration.variance[1] = 0;
 	accel_data.calibration.variance[2] = 0;
 	
-	for (i = 0, j = 0; i < 250; i++) {
+	for (i = 0, j = 0; j < NVAR; j++) {
 		while (ahrs_state != AHRS_DATA_READY) ;
 		ahrs_state = AHRS_PROCESSING;
 		downsample_data();
-		gyro_bias[0] += gyro_data.filtered.x;
-		gyro_bias[1] += gyro_data.filtered.y;
-		gyro_bias[2] += gyro_data.filtered.z;
-		accel_bias[0] += accel_data.filtered.x;
-		accel_bias[1] += accel_data.filtered.y;
-		accel_bias[2] += accel_data.filtered.z;
-		gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x,2);
-		gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y,2);
-		gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z,2);
-		accel_data.calibration.variance[0] += pow(accel_data.filtered.x,2);
-		accel_data.calibration.variance[1] += pow(accel_data.filtered.y,2);
-		accel_data.calibration.variance[2] += pow(accel_data.filtered.z,2);
+		gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x-gyro_bias[0],2) / NVAR;
+		gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y-gyro_bias[1],2) / NVAR;
+		gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z-gyro_bias[2],2) / NVAR;
+		accel_data.calibration.variance[0] += pow(accel_data.filtered.x-accel_bias[0],2) / NVAR;
+		accel_data.calibration.variance[1] += pow(accel_data.filtered.y-accel_bias[1],2) / NVAR;
+		accel_data.calibration.variance[2] += pow(accel_data.filtered.z-accel_bias[2],2) / NVAR;
 		ahrs_state = AHRS_IDLE;
-		
 #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
 		if(PIOS_HMC5843_NewDataAvailable()) {
 			j ++;
@@ -609,38 +623,17 @@ void calibrate_sensors()
 			mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
 			mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
 			mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
-			
-			mag_bias[0] += mag_data.scaled.axis[0];
-			mag_bias[1] += mag_data.scaled.axis[1];
-			mag_bias[2] += mag_data.scaled.axis[2];
-			mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0],2);
-			mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1],2);
-			mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2],2);
-		}			
+			mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0]-mag_bias[0],2);
+			mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1]-mag_bias[1],2);
+			mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2]-mag_bias[2],2);
+		}
 #endif
-
 		process_spi_request();
 	}
-	gyro_data.calibration.bias[0]  = gyro_bias[0] / i;
-	gyro_data.calibration.bias[1]  = gyro_bias[1] / i;
-	gyro_data.calibration.bias[2]  = gyro_bias[2] / i;
-	accel_data.calibration.bias[0] = accel_bias[0] / i;
-	accel_data.calibration.bias[1] = accel_bias[1] / i;
-	accel_data.calibration.bias[2] = accel_bias[2] / i;
-	mag_data.calibration.bias[0]   = mag_bias[0] / j;
-	mag_data.calibration.bias[1]   = mag_bias[1] / j;
-	mag_data.calibration.bias[2]   = mag_bias[2] / j;
-
-	// more iterations for variance
-	gyro_data.calibration.variance[0] = gyro_data.calibration.variance[0] / i - pow(gyro_data.calibration.bias[0],2);
-	gyro_data.calibration.variance[1] = gyro_data.calibration.variance[1] / i - pow(gyro_data.calibration.bias[1],2);
-	gyro_data.calibration.variance[2] = gyro_data.calibration.variance[2] / i - pow(gyro_data.calibration.bias[2],2);
-	accel_data.calibration.variance[0] = accel_data.calibration.variance[0] / i - pow(accel_data.calibration.bias[0],2);
-	accel_data.calibration.variance[1] = accel_data.calibration.variance[1] / i - pow(accel_data.calibration.bias[1],2);
-	accel_data.calibration.variance[2] = accel_data.calibration.variance[2] / i - pow(accel_data.calibration.bias[2],2);
-	mag_data.calibration.variance[0] = mag_data.calibration.variance[0] / j - pow(mag_data.calibration.bias[0],2);
-	mag_data.calibration.variance[1] = mag_data.calibration.variance[1] / j - pow(mag_data.calibration.bias[1],2);
-	mag_data.calibration.variance[2] = mag_data.calibration.variance[2] / j - pow(mag_data.calibration.bias[2],2);
+	
+	mag_data.calibration.variance[0] /= j;
+	mag_data.calibration.variance[1] /= j;
+	mag_data.calibration.variance[2] /= j;
 }
 
 /**
diff --git a/ground/src/plugins/config/configahrswidget.cpp b/ground/src/plugins/config/configahrswidget.cpp
index 109853833..b724a194b 100644
--- a/ground/src/plugins/config/configahrswidget.cpp
+++ b/ground/src/plugins/config/configahrswidget.cpp
@@ -547,10 +547,22 @@ void ConfigAHRSWidget::computeScaleBias()
     field->setDouble(sign(S[1]) * b[1], 1);
     field->setDouble(sign(S[2]) * b[2], 2);
 
+    // Go ahead and fix gyro scale here
     field = obj->getField(QString("gyro_bias"));
-    field->setDouble(-listMean(gyro_accum_x),0);
-    field->setDouble(-listMean(gyro_accum_y),1);
-    field->setDouble(-listMean(gyro_accum_z),2);
+    double a = field->getDouble(0);
+    field->setDouble(-sign(a) * a, 0);
+    a = field->getDouble(1);
+    field->setDouble(sign(a) * a, 1);
+    a = field->getDouble(2);
+    field->setDouble(-sign(a) * a, 2);
+
+    field = obj->getField(QString("gyro_bias"));
+    a = listMean(gyro_accum_x);
+    field->setDouble(sign(a)*a,0);
+    a = listMean(gyro_accum_y);
+    field->setDouble(-sign(a)*a,1);
+    a = listMean(gyro_accum_z);
+    field->setDouble(sign(a)*a,2);
 
     obj->updated();