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));
}
}