Как можно отследить краткосрочную траекторию 6-осевого инерциального измерительного блока?

Я искал и искал на форумах, писал одно сообщение за другим в DeepSeek и ChatGPT, но наткнулся на препятствие.

Сейчас я работаю над проектом, в котором хочу писать числа в воздухе, используя MPU6050, привязанный к кончику моего пальца вместе с микроконтроллером ESP32, поэтому я не хочу отслеживать положение в долгосрочной перспективе. Однако я не уверен, возможно ли это сделать. Датчик имеет гироскоп и акселерометр. Я попытался реализовать фильтр Mahony из библиотеки Adafruit AHRS, чтобы дважды интегрировать значения ускорения, исключить вектор силы тяжести и смотреть только на значения X и Z. На Python у меня есть программа визуализации, которая берет значения из плоскости XZ и строит график. Программа работает нормально, если писать очень быстро, не слишком крупно и сверху вниз (например, как вы бы написали 5, 3 или 2). Но для чисел вроде 8 или 6 приходится возвращаться к началу, и здесь программа дает сбой. Число становится искаженным и нечитаемым. Кроме того, когда я кладу датчик на стол, значения X и Z, как сообщается, становятся все более отрицательными с линейной скоростью, и я думал, что фильтр это отфильтрует... Есть ли что-то, что мне следует учесть? Есть ли какие-то упущения? Буду очень благодарен за совет!

Ниже представлена программа, которую я сейчас использую в Arduino IDE:

#include <Adafruit_MPU60.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_AHRS_Mahony.h>

Adafruit_MPU6050 mpu;
Adafruit_Mahony filter;

// Переменные положения и скорости
float vel_x = 0, vel_z = 0;
float pos_x = 0, pos_z = 0;
unsigned long last_time = 0;
bool recording = false;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  filter.begin(30); // Инициализируем фильтр
}

void loop() {
  // Обработка команд запуска/остановки из Python
  if (Serial.available()) {
    char c = Serial.read();
    if (c == 'S') {
      recording = true;
      pos_x = 0;
      pos_z = 0;
      vel_x = 0;
      vel_z = 0;
      last_time = millis();
    } else if (c == 'E') {
      recording = false;
    }
  }

  if (recording) {
    sensors_event_t accel, gyro, temp;
    mpu.getEvent(&accel, &gyro, &temp);

    float gx = gyro.gyro.x;
    float gy = gyro.gyro.y;
    float gz = gyro.gyro.z;
    filter.updateIMU(gx, gy, gz, accel.acceleration.x, accel.acceleration.y, accel.acceleration.z);

    // Получить кватернион и повернуть линейное ускорение в мировую систему отсчета
    float q0, q1, q2, q3;
    filter.getQuaternion(&q0, &q1, &q2, &q3);
    float grav_x = 2.0f * (q1*q3 - q0*q2);
    float grav_y = 2.0f * (q0*q1 + q2*q3);
    float grav_z = 2.0f * (q0*q0 - 0.5f + q3*q3);

    // Вычислить линейное ускорение (рама датчика)
    float lax = accel.acceleration.x - grav_x * 9.81;
    float lay = accel.acceleration.y - grav_y * 9.81;
    float laz = accel.acceleration.z - grav_z * 9.81;

    // Повернуть в мировую систему координат (упрощенно для плоскости XZ)
    float ax_w = (1 - 2*q2*q2 - 2*q3*q3) * lax + (2*q1*q2 - 2*q0*q3) * lay + (2*q1*q3 + 2*q0*q2) * laz;
    float az_w = (2*q1*q3 - 2*q0*q2) * lax + (2*q2*q3 + 2*q0*q1) * lay + (1 - 2*q1*q1 - 2*q2*q2) * laz;

    // Интегрируем для получения позиции
    float dt = (millis() - last_time) / 1000.0;
    last_time = millis();

    vel_x += ax_w * dt;
    vel_z += az_w * dt;
    pos_x += vel_x * dt;
    pos_z += vel_z * dt;

    // Отправляем данные в Python
    Serial.print(pos_x);
    Serial.print(",");
    Serial.println(pos_z);

    delay(30);
  }
}