Дополнительный фильтр IMU в C ++? - PullRequest
Новая
       25

Дополнительный фильтр IMU в C ++?

0 голосов
/ 08 марта 2019

Я пытаюсь внедрить дополнительный фильтр, чтобы уменьшить влияние дрейфа и шума, влияющих на SparkFun LSM9DS1 IMU. Цель состоит в том, чтобы получить достоверное представление (углы Эйлера) вращения устройства в реальном времени. Прочитав об этом типе фильтров на нескольких веб-сайтах, я попытался написать некоторый код для достижения этой цели, но, должно быть, мне чего-то не хватает, потому что я не могу найти работающего решения.

Я начну с определения своего временного шага DT, который будет использоваться для интегрирования угловой скорости гироскопа, чтобы найти угловое положение по каждой из 3 осей: 

#define SAMPLE_RATE 952
#define DT 1/SAMPLE_RATE // 1.050420168 ms

Затем я вычисляю углы, интегрируя угловую скорость гироскопа по времени. Я не совсем уверен, если это правильный способ сделать это и был бы очень признателен за помощь: 

void calcGyroAngles()
{
    gyroAngleX += rateX*DT;
    gyroAngleY += rateY*DT;
    gyroAngleZ += rateZ*DT;
}

Угловые данные получены от акселерометра с использованием функции atan2 (функция, обрабатывающая это, уже присутствовала в библиотеке LSM9DS1 Arduino), и, следовательно, фильтр определяется этой функцией: 

void runFilter()
{
    // Work out roll, pitch and yaw from accelerometer data
    calcAttitude(imu.ax, imu.ay, imu.az, -imu.my, -imu.mx, imu.mz);
    calcGyroAngles();

    // 98% of the gyro signal is fused with 2% of the accelerometer signal
    x_filtered = 0.98*(gyroAngleX) + 0.02*roll;
    y_filtered = 0.98*(gyroAngleY) + 0.02*pitch;
    z_filtered = 0.98*(gyroAngleZ) + 0.02*yaw;

    printf("%f, %f, %f \n", x_filtered, y_filtered, z_filtered);
}

По какой-то причине все, что я получаю из этого, - это бессмысленные значения, которые продолжают расти со временем и не соответствуют углам, которые я ищу. Может кто-нибудь помочь мне понять, как заставить дополнительный фильтр работать должным образом в C ++?

...