AP_InertialSensor: fix best_diff on gyro initialization

Without this patch, if accel_diff.length() > 0.2f and j == 0, then
best_diff[k] would be zero forever since diff_norm[k] >= 0 for any j.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -803,7 +803,7 @@ AP_InertialSensor::_init_gyro()
     for (uint8_t k=0; k<num_gyros; k++) {
         _gyro_offset[k].set(Vector3f());
         new_gyro_offset[k].zero();
-        best_diff[k] = 0;
+        best_diff[k] = -1.f;
         last_average[k].zero();
         converged[k] = false;
     }
@@ -859,7 +859,7 @@ AP_InertialSensor::_init_gyro()
         }
 
         for (uint8_t k=0; k<num_gyros; k++) {
-            if (j == 0) {
+            if (best_diff[k] < 0) {
                 best_diff[k] = diff_norm[k];
                 best_avg[k] = gyro_avg[k];
             } else if (gyro_diff[k].length() < ToRad(0.1f)) {