MPU6050/DMP Прерывистый плохой вывод данных?

У меня есть модуль DFRobots MPU6050, подключенный к SCL/SDA/Pin 2 на плате Mega 2560, используя библиотеки I2Cdev/MPU6050_6Axis_MotionApps Джеффа Роуберга. У меня есть 2,2 K съемников на модуле MPU6050, привязанный к +5V, как показано на следующем фото.

Я использовал модифицированную версию кода из примера Джеффа, чтобы получить значения заголовков из MPU6050, как показано ниже

bool GetIMUHeadingDeg() //08/28/18 chg to bool return 

{

//Цель: Инкапсулировать все необходимое для получения значения рыскания
//Входы:
// global_yawval = переменная float, определенная в глобальной области
// fifocount = переменная uint8_t, определенная в глобальной области
// packetsize = переменная uint8_t, определенная в глобальной области
// mpuIntStatus = переменная uint8_t, определенная в глобальной области
// whilecount = длинная переменная, определенная в глобальной области
// resetcount = переменная int, определенная в глобальной области
//Выходы:
// global_yawval заполняется последним значением рыскания от датчика
// true = успех, false = неудача
//Примечания:
// 08/13/18 теперь возвращает global_yawval в вызывающую рутину
// 08/28/18 chg return to bool so can return success/failure


// если программирование не удалось, не пытайтесь ничего сделать
//if (!dmpReady) return;
//if (!dmpReady) false;

bool result = false; //added 01/16/19 to supress warnings


// ждите прерывания MPU или дополнительных пакетов, доступных
while (!mpuInterrupt && fifoCount < DMPpacketSize)
{
    whilecount++;
    delay(10);
    delay(10);

    if (mpuInterrupt)
    {
        fifoCount = mpu.getFIFOCount();
        mySerial.printf("%ld\t%d\n", whilecount, fifoCount);
        whilecount = 0;
        break;
    }
}

// сбросить флаг прерывания и получить байт INT_STATUS
mpuIntStatus = mpu.getIntStatus();
mpuInterrupt = false;

// get current FIFO count
fifoCount = mpu.getFIFOCount();

// проверьте наличие переполнения (этого никогда не должно произойти, если только наш код не слишком неэффективен)
if ((mpuIntStatus & 0x10) || fifoCount == 1024)//0x10 is mask for MPU6050_INTERRUPT_FIFO_OFLOW_BIT
{
    // reset so we can continue cleanly
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));
    result = false; //added 01/16/19 to supress warnings

    // в противном случае проверьте наличие готового прерывания DMP data ready (это должно происходить часто)
}
else if (mpuIntStatus & 0x02)//0x02 is mask for MPU6050_INTERRUPT_DMP_INT_BIT
{
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < DMPpacketSize)
    {
        fifoCount = mpu.getFIFOCount();
        //mySerial.printf("fifo count = %d\n", fifoCount);
    }

    //07/08/18 added to watch for non-modulo FIFO counts
    if (fifoCount == 0 || fifoCount % DMPpacketSize != 0)
    {
        mpu.resetFIFO();
        FIFO_resetcount++;
    }
    else
    {
        //07/07/18 modified to read all outstanding packets
        // read a packet from FIFO
        while (fifoCount >= DMPpacketSize)
        {
            //mySerial.printf("fifo count = %d\n", fifoCount);
            mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);
            fifoCount -= DMPpacketSize;
        }

        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        global_yawval = ypr[0] * 180 / M_PI;
        result = true; //added 01/16/19 to supress warnings
    }

    ////DEBUG!!
    //      mySerial.printf("yawdeg = %4.2f\n", global_yawval);
    //      mySerial.printf("time = %ld\tyawdeg = %6.2f\n", millis(), global_yawval);
    ////DEBUG!!
}

return result; //added 01/16/19 to surpress warnings

}

Я заметил случайные неверные точки данных в вычисленных значениях курса, извлеченных из MPU6050, как показано на прилагаемом графике. Есть идеи, что я могу с этим поделать? Является ли обнаружение/удаление плохих значений ожидаемой проблемой при работе с продуктами IMU, такими как MPU6050?

В качестве эксперимента я исключил все точки, для которых скорость вращения была нереально высока, в результате чего на графике ниже появилась линия "Adj Hdg" (серая линия). Это (в основном) работает, за исключением одного случая, когда было два плохих значения подряд.

ТИА,

Фрэнк