void Estimate()
 {
     if (!float.IsNaN(Pitch))
     {
         Pitch = KalmanPitch.KalmanUpdate(Pitch, ComputeDataFromImu(gyroData[1], GYRO_RANGE));
     }
     if (!float.IsNaN(Roll))
     {
         Roll = KalmanRoll.KalmanUpdate(Roll, ComputeDataFromImu(gyroData[0], GYRO_RANGE));
     }
 }