В предыдущей части статьи мы изложили теорию фильтра альфа-бета. Также мы провели моделирование в Matlab для демонстрации влияния настроек на поведение фильтра. Пришло время реализовать фильтр на микроконтроллере.
В проекте мы использовали плату STM32F4 Discovery и модуль MiniIMU v2.
Реализация
Реализованное приложение показывает через интерфейс USART текущие показания гироскопа по всем трем осям и оценки угловой скорости, сделанные с использованием альфа-бета фильтра. Приложение использует систему FreeRTOS с четырьмя потоками:
- поток чтения данных с датчика по I2C
- поток, вычисляющий текущую оценку фильтра
- данные для показа через интерфейс USART
- диагностический поток, отвечающий за инициализацию других потоков и мигание светодиода для проверки правильности работы приложения.
Измерения с датчика выполняются каждые 100 мс. Частоты фильтра и регистратора, отображающие данные через USART, были настроены на эту частоту.
Выполнение
Реализация альфа-бета-фильтра очень проста и состоит из 4 строк кода для каждой оси:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
x_pri.x = x_post.x + dt * v_post.x; x_pri.y = x_post.y + dt * v_post.y; x_pri.z = x_post.z + dt * v_post.z; v_pri.x = v_post.x; v_pri.y = v_post.y; v_pri.z = v_post.z; x_post.x = x_pri.x + alfa.x * (sensor_get_x() - x_pri.x); x_post.y = x_pri.y + alfa.y * (sensor_get_y() - x_pri.y); x_post.z = x_pri.z + alfa.z * (sensor_get_z() - x_pri.z); v_post.x = v_pri.x + beta.x * (sensor_get_x() - x_pri.x) / dt; v_post.y = v_pri.y + beta.y * (sensor_get_y() - x_pri.y) / dt; v_post.z = v_pri.z + beta.z * (sensor_get_z() - x_pri.z) / dt; |
Мы собрали данные из вышеуказанного приложения с помощью программы RS Data Logger, а затем преобразовали их в данные, читаемые в Matlab. Мы использовали простую программу на C. На следующем этапе построили графики измеренных и оценочных значений для всех трех осей:
Анализ сигналов
Сигнал от гироскопа слегка зашумлен, а изменения сигнала довольно резкие. В этом случае хорошо использовать большое значение параметра альфа, что означает, что отфильтрованный сигнал будет похож на исходный. Фильтрация более полезна для зашумленных сигналов.
Итак, давайте посмотрим, как работает фильтр для акселерометра. Ниже приведены измерения для трех осей для параметра альфа = 0,2 и бета = 0,05:
Вывод
На этот раз мы видим некоторый выброс при быстром изменении значения, но фильтр хорошо справляется с шумом. Как вы уже могли понять, фильтр альфа-бета — более простая альтернатива фильтру Калмана.
Его реализация не сложна и занимает 4 строчки кода. Мы настраиваемся с помощью двух параметров, наиболее важным из которых является параметр альфа.
С Уважением, МониторБанк