Comments 17
Хотелось бы предупредить тех, кто собирается использовать этот фильтр.
На офф. сайте есть более свежая версия — www.x-io.co.uk/open-source-imu-and-ahrs-algorithms — в которой, судя по всему, есть ошибка в формуле шага градиентного спуска (источник).

Как есть
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 — _2q0q2 — ax) + _2q1 * (2.0f * q0q1 + _2q2q3 — ay) — _2bz * q2 * (_2bx * (0.5f — q2q2 — q3q3) + _2bz * (q1q3 — q0q2) — mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 — q0q3) + _2bz * (q0q1 + q2q3) — my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f — q1q1 — q2q2) — mz);

s1 = _2q3 * (2.0f * q1q3 — _2q0q2 — ax) + _2q0 * (2.0f * q0q1 + _2q2q3 — ay) — 4.0f * q1 * (1 — 2.0f * q1q1 — 2.0f * q2q2 — az) + _2bz * q3 * (_2bx * (0.5f — q2q2 — q3q3) + _2bz * (q1q3 — q0q2) — mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 — q0q3) + _2bz * (q0q1 + q2q3) — my) + (_2bx * q3 — _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f — q1q1 — q2q2) — mz);

s2 = -_2q0 * (2.0f * q1q3 — _2q0q2 — ax) + _2q3 * (2.0f * q0q1 + _2q2q3 — ay) — 4.0f * q2 * (1 — 2.0f * q1q1 — 2.0f * q2q2 — az) + (-_4bx * q2 — _2bz * q0) * (_2bx * (0.5f — q2q2 — q3q3) + _2bz * (q1q3 — q0q2) — mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 — q0q3) + _2bz * (q0q1 + q2q3) — my) + (_2bx * q0 — _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f — q1q1 — q2q2) — mz);

s3 = _2q1 * (2.0f * q1q3 — _2q0q2 — ax) + _2q2 * (2.0f * q0q1 + _2q2q3 — ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f — q2q2 — q3q3) + _2bz * (q1q3 — q0q2) — mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 — q0q3) + _2bz * (q0q1 + q2q3) — my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f — q1q1 — q2q2) — mz);



Как должно быть
s0= -_2q2*(2*(q1q3 — q0q2) — ax) + _2q1*(2*(q0q1 + q2q3) — ay) + -_4bz*q2*(_4bx*(0.5 — q2q2 — q3q3) + _4bz*(q1q3 — q0q2) — mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 — q0q3) + _4bz*(q0q1 + q2q3) — my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 — q1q1 — q2q2) — mz);

s1= _2q3*(2*(q1q3 — q0q2) — ax) + _2q0*(2*(q0q1 + q2q3) — ay) + -4*q1*(2*(0.5 — q1q1 — q2q2) — az) + _4bz*q3*(_4bx*(0.5 — q2q2 — q3q3) + _4bz*(q1q3 — q0q2) — mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 — q0q3) + _4bz*(q0q1 + q2q3) — my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 — q1q1 — q2q2) — mz);

s2= -_2q0*(2*(q1q3 — q0q2) — ax) + _2q3*(2*(q0q1 + q2q3) — ay) + (-4*q2)*(2*(0.5 — q1q1 — q2q2) — az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 — q2q2 — q3q3) + _4bz*(q1q3 — q0q2) — mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 — q0q3) + _4bz*(q0q1 + q2q3) — my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 — q1q1 — q2q2) — mz);

s3= _2q1*(2*(q1q3 — q0q2) — ax) + _2q2*(2*(q0q1 + q2q3) — ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 — q2q2 — q3q3) + _4bz*(q1q3 — q0q2) — mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 — q0q3) + _4bz*(q0q1 + q2q3) — my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 — q1q1 — q2q2) — mz);



О! Спасибо, попробую этот фикс!
Мы уже больше месяца на своём квадрокоптере используем этот алгоритм. Когда пытались задействовать магнетометр, то была какая-то лажа с ориентацией. Как я понимаю, Ваш фикс относится именно к той функции, которая использует измерения магнетометра. Нам сейчас приходится использовать ту, которая обходится без него.
Да, без магнетометра другая функция вроде бы без ошибок.

Единственное, что фикс не мой, я его только нагуглил :)
Самодельный с STM32F103 на мозгах. Мы – это я и мой друг. Оно уже летает, но пока что тяжело управляется. Как только оно превратится во что-то достойное, можем написать статью.
Если какие-то деловые предложения есть, пишите в ЛС, свяжемся :)
Эх, к завтрашнему дню мы не успеем сделать необходимые доработки, но будем следить за новостями и следующими встречами :)
Так и не удалось понять, в чем суть алгоиритма и отличие от уже существующих. Сам я давно работаю с немного оптимизированной реализацией «DCM filter» by Robert Mayhony, т.е. не новичек в этом, но так и не понял зачем так усложнять простые в общем-то вещи.
К примеру, насколько он свободен от искажения результата при ускорениях (линейных и центробежных)? Судя из фразы «Компенсация магнитных искажений в этом случае гарантирует, что магнитные возмущения влияют только на курс», компасс тут не помогает.

Тесты очень научные, респект автору. Но они выполнены в тепличных условиях, к примеру не проверялось скорость восстановления фильтра после перегрузки гироскопа (over-saturation).
Напишите пожалуйста статью об используемом фильтре. Или, если хорошо разбираетесь — о фильтре Махони.
MARG — это не фильтр. Это аббревиатура, означающая реализацию ИНС с магнитометром. Приведённый вами документ — это сокращённая версия отчёта, перевод которой находится перед вами — это данная статья.
Код для STM32-Discovery: github.com/dccharacter/AHRS
Там можно выбрать либо этот фильтр, либо Махоуни
Передача данных на ПК через УАРТ или через ЮСБ (джойстик, ком-порт)
Матрицы и кватернионы
Скрипт для визуализации на питоне

Как скелет для вашего проекта пойдет, простите сразу за качество кода — я не настоящий маляр
Сравнение 1
Сравнение 2

Я подозреваю выигрыш этого алгоритма был получен в сравнении с плохо настроенным ФК.
И еще судя по последней ссылке, фильтром этот алгоритм и алгоритм Махоуни называют только из-за использования выраженного в виде передаточных функций ПИД.
Если кто-то интересуется работами Маджвика: у него есть три известных публикации по одноименному фильтру (2010, 2011 и 2014 гг.).
Наиболее известна публикация 2010 г. – она представляет собой отчет по НИОКР в Бристольском Университете (связанной с компьютеризированной хирургией, если правильно помню). В ней описаны принципы построения фильтра, формирующего кватернионы из сигналов трехосевых инерциальных датчиков – акселерометра, магнетометра и гироскопа и приведен пример его практической реализации. Приведены формулы для расчета коэффициентов фильтрации шума и систематической ошибки (дрейфа) сигналов гироскопа.
В 2011 г. Маджвик сотоварищи опубликовал вторую работу (тезисы выступления на конференции IEEE on Rehabilitation Robotics), в которой описаны принципы построения фильтра, формирующего кватернионы из сигналов трехосевых инерциальных датчиков – акселерометра, магнетометра и гироскопа и приведен пример его практической реализации. Кроме того, в ней приведена формула для расчета коэффициента фильтрации шума сигналов гироскопа и экспериментальные данные о влиянии величины этого коэффициента на параметры фильтра.
В 2014 г. он защитил в Бристольском Университете докторскую диссертацию на основе описанных выше разработок и затем потерял интерес к этой теме. Понятно, что диссертация – наиболее капитальная из его публикаций. Кто хочет разобраться – лучше читать именно ее.
1. Sebastian O.H. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Report x-io and University of Bristol (UK) vol. 25, 113–118, 30.04.2010
2. Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan. Estimation of IMU and MARG orientation using a gradient descent algorithm. 2011 IEEE International Conference on Rehabilitation Robotics, Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 – July 1, 2011
3. Sebastian O.H. Madgwick. AHRS algorithms and calibration solutions to facilitate new applications using low-cost MEMS. PhD Theses, Department of Mechanical Engineering, University of Bristol, Bristol, UK, 2014
Only those users with full accounts are able to leave comments. Log in, please.