AHRS алгоритм Себастиана Мажвика (Sebastian Madgwick)

Дата публикации: May 27, 2014 7:25:22 PM

Выжав все соки из виброразвязки, я таки решился заменить примитивный комплементарный фильтр (КФ) инклинометра на что-то более подходящее. Ведь значения углов по-прежнему сильно скакали при максимальной тяге двигателей. На что можно заменить КФ? Да на тот же фильтр Калмана. Хорошо всем известный фильтр имеет множество реализаций на разных языках, и воплощен в виде библиотек для разных платформ. Так, например, библиотеку с фильтром Калмана легко найти для Arduino и для моей любимой mbed.

Но это не мой путь:) Еще давненько я заприметил другой фильтр, который, судя по описанию, давал результаты еще более лучшие чем фильтр Калмана. Разработал этот математический аппарат хороший человек по имени Sebastian Madgwick, в рамках работы над своей докторской (Ph.D.) диссертацией.

У меня не входило в планы углубляться в принципы работы фильтра, благо все это можно найти в его статье. Я лишь взял исходники, адаптировал их под mbed и грязно использовал в своей подсистеме стабилизации. Благо, на официальном (?) сайте проекта есть исходники для разных платформ, включая: c#, c++, и даже для MathLab.

Использование

По сути, автор предлагает использовать одну из двух функций: MadgwickAHRSupdate или MadgwickAHRSupdateIMU, которые возвращают кватернион положения инклинометра в пространстве. Функции имеют вид:  

MadgwickAHRSupdate( tdelta, gyr[X], gyr[Y], gyr[Z], acc[X], acc[Y], acc[Z], mag[X], mag[Y], mag[Z])

MadgwickAHRSupdateIMU( tdelta, gyr[X], gyr[Y], gyr[Z], acc[X], acc[Y], acc[Z])

Здесь 

• tdelta - добавленный мной аргумент - пройденное между итерациями время;

• gyr[X] - показания гиротахометра (пусть гироскопа) в рад/сек;

• acc[X] - показания акселерометра в исходных единицах;

• mag[X] - показания магнитометра в исходных единицах;

В общем то функции отличаются только тем, что первая умеет учитывать показания магнитометра, а вторая нет.

Результаты

Как говорится, лучше один раз увидеть:

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

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