Как правильно настроить фильтр Махони? - PullRequest
Новая
       18

Как правильно настроить фильтр Махони?

0 голосов
/ 17 февраля 2020

Я получаю данные акселерометра и гироскопа от устройства MPU-6050.
Я обрабатываю эти данные с помощью фильтра Махони, который я сам написал, и сравниваю результаты с фильтром Махони AHRS в Matlab. Совпадение результатов обработки.

Проблема в том, что эти результаты не очень ясны.
Например, когда я наклоняю устройство MPU-6050 вдоль одной из осей на 90 градусов, то после фильтрации не вижу эти 90 градусов.
Возможно, необходимо каким-то образом настроить фильтр Махони (есть параметр Ки, Частота выборки и некоторые другие).
Подскажите, как настроить фильтр Махони, если устройство MPU-6050 получает данные с частотой 1000 Гц.
Результаты фильтрации - см. графики.
enter image description here enter image description here

Вот часть Matlab код, в котором используется фильтр MahonyAHRS: 

AHRS = MahonyAHRS('SamplePeriod', 1/256, 'Kp', 0.5);
quaternion = zeros(length(time), 4);
for t = 1:length(time)
    %AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:)); % gyroscope units must be radians
    AHRS.UpdateIMU(GYRO(t,:) * (pi/180), ACC(t,:)); % gyroscope units must be radians
    quaternion(t, :) = AHRS.Quaternion;
end

euler = quatern2euler(quaternConj(quaternion)) * (180/pi);  % use conjugate for sensor frame relative to Earth and convert to degrees.

И оригинальная реализация Мэгвиком фильтра Махони - здесь

...