Фильтр Мажвика

В одном из уроков мы познакомились с понятием системы стабилизации полета, которая даёт возможность летающему роботу хоть как то удерживать положение относительно горизонта. Мы разобрали альфа-бета фильтр — математический аппарат, который позволяет с помощью акселерометра и гироскопа получать достаточно точные значения тангажа и крена.

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

Есть два распространенных алгоритма, которые работают лучше альфа-бета — это фильтр Калмана и фильтр Мажвика. Оба являются достаточно сложными, чтобы разбираться с ними в рамках данного урока. Однако, мы можем использовать готовую программную реализацию фильтра в своих целях.

Фильтр Себастиана Мажвика

Данный алгоритм смешивания данных акселерометра, гироскопа и магнитометра был предложен Себастианом Мажвиком (Sebastian Madgwick) в 2009 году, в рамках научного исследования на соискание Ph.D. Статью с описанием математической модели фильтра, как и реализацию алгоритма на разных языках программирования можно найти на сайте проекта (ссылка в конце урока).

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

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