Показания гироскопа MPU6050 слишком дрейфуют только при быстрых изменениях

Я пытаюсь написать простой скетч для MPU6050.

Проблема заключается в том, что датчик в значительной степени стабилен, когда он неподвижен или постоянно меняет угол при низких и средних угловых скоростях, но если я попытаюсь изменить угол слишком быстро, показания будут сильно дрейфовать на десятки градусов в в любом направлении, как показано на изображениях ниже с серийного плоттера.

При изменении угла наклона от медленного до среднего гироскоп почти идентично отслеживает шаг акселерометра (со временем немного дрейфует, но это нормально и приемлемо для дополнительного фильтра).

Однако при быстрых изменениях высота тона мгновенно смещается на десятки градусов от акселерометра. Огромный дрейф делает дополнительный фильтр практически бесполезным для исправления вывода

Это мой первый опыт работы с датчиком, поэтому у меня нет опыта работы с его параметрами. Но проблема, похоже, в линиях интегрирования новых угловых скоростей по крену и тангажу.

• Я поленился и использовал коэффициенты шкалы чувствительности по умолчанию; это как-то связано с таким поведением?

• Dt, измеренный для Arduino, составляет 20 мс между показаниями; это слишком медленно для быстрых изменений или что-то в этом роде?

• В техническом описании также упоминается программируемый выходной диапазон. Нужно ли это программировать для большей пропускной способности и точных результатов?

ИЗМЕНИТЬ:

Я попытался немного прочитать о масштабном коэффициенте чувствительности. Казалось, что увеличение угловой скорости свыше 250 градусов в секунду не измеряется при выборе конфигурации по умолчанию.

После некоторых тестов я понял, что 1000 град/с — это хорошая конфигурация для меня; дрифт конечно намного лучше, но все же не то, на что я надеялся. Он по-прежнему смещается на +- 10 градусов за две-три секунды перемещения.

Вывод изменений от медленного к среднему:

Вывод быстрых изменений:

#include <Wire.h>
#define CAL_AVERAGE 2000

const int MPU = 0x68; //I2C-адрес MPU6050
const float alpha = 0.95;

double ax, ay, az;
double gx, gy, gz;

// float accAngleX, accAngleY, accAngleZ;
//float gyroAgnleX, gyroAngleY, gyroAngleZ;

double accErrorX, accErrorY, accErrorZ;
double gyroErrorx, gyroErrorY, gyroErrorZ;

double accRoll, accPitch;
double gyroRoll, gyroPitch, gyroYaw;

double roll, pitch, yaw;

double dt, currentTime, previousTime;


void setup(){
  previousTime = 0;

  Serial.begin(115200);
  Wire.begin();     //инициируем общение

  //сброс процессора
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);

  //откалибровать здесь
  calculateError();
}

void loop(){
  readAcc();
  readGyro();

  // получаем крен и шаг акселерометра
  accRoll = atan2(ay, az) * 180 / PI;
  accPitch = atan2(ax, az) * 180 / PI;
    
  currentTime = millis();
  //устанавливаем крен и шаг гироскопа для крена и шага акселерометра только при первом чтении
  if (previousTime != 0) { 
    // получить крен, тангаж и рыскание гироскопа
    dt = currentTime - previousTime;
    dt /= 1000;
    gyroRoll += gx * dt;
    gyroPitch += -gy * dt;
    gyroYaw += gz * dt;
  } else {
    gyroRoll = accRoll;
    gyroPitch = accPitch;
    gyroYaw = 0;
  }

  previousTime =  currentTime;

  roll = alpha*gyroRoll + (1 - alpha)*accRoll;
  pitch = alpha*gyroPitch + (1 - alpha)*accPitch;
  yaw = gyroYaw;

  //наконец распечатать собранные значения
  printData();
}

void readAcc(){
  //сначала читаем данные акселерометра
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);

  // необработанные данные акселерометра
  ax = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
  ay = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
  az = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;

  ax -= accErrorX;
  ay -= accErrorY;
  az -= accErrorZ;
}

void readGyro(){
  //второе чтение гироскопа
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);

  //необработанные данные гироскопа
  gx = ( (Wire.read() << 8) | Wire.read() ) / 131.0;
  gy = ( (Wire.read() << 8) | Wire.read() ) / 131.0;
  gz = ( (Wire.read() << 8) | Wire.read() ) / 131.0;

  gx -= gyroErrorx;
  gy -= gyroErrorY;
  gz -= gyroErrorZ;
}

void calculateError(){
  float axErr = 0, ayErr = 0, azErr = 0;
  float gxErr = 0, gyErr = 0, gzErr = 0;

  for (int i = 0; i < CAL_AVERAGE; i++) {
    readAcc();
    az = az - 1;

    axErr += ax;
    ayErr += ay;
    azErr += az;

    readGyro();

    gxErr += gx;
    gyErr += gy;
    gzErr += gz;
  }

  accErrorX = axErr/CAL_AVERAGE;
  accErrorY = ayErr/CAL_AVERAGE;
  accErrorZ = azErr/CAL_AVERAGE;

  gyroErrorx = gxErr/CAL_AVERAGE;
  gyroErrorY = gyErr/CAL_AVERAGE;
  gyroErrorZ = gzErr/CAL_AVERAGE;
}

void printData() {
  Serial.print(ax);
  Serial.print(",     ");
  Serial.print(ay);
  Serial.print(",     ");
  Serial.print(az);

  Serial.print(",               ");

  Serial.print(gx);
  Serial.print(",     ");
  Serial.print(gy);
  Serial.print(",     ");
  Serial.print(gz);

  Serial.print(",               ");

  Serial.print(accPitch);
  Serial.print(",     ");
  Serial.print(accRoll);

  Serial.print(",               ");

  Serial.print(gyroPitch);
  Serial.print(",     ");
  Serial.print(gyroRoll);
  Serial.print(",     ");
  Serial.print(gyroYaw);

  Serial.print(",               ");

  Serial.print(pitch);
  Serial.print(",     ");
  Serial.print(roll);
  Serial.print(",     ");
  Serial.print(yaw);

  Serial.print(",     ");
  Serial.println(dt);
}