Comments 17
Хотелось бы предупредить тех, кто собирается использовать этот фильтр.
На офф. сайте есть более свежая версия — www.x-io.co.uk/open-source-imu-and-ahrs-algorithms — в которой, судя по всему, есть ошибка в формуле шага градиентного спуска (источник).
На офф. сайте есть более свежая версия — 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.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);
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 на мозгах. Мы – это я и мой друг. Оно уже летает, но пока что тяжело управляется. Как только оно превратится во что-то достойное, можем написать статью.
Если какие-то деловые предложения есть, пишите в ЛС, свяжемся :)
Если какие-то деловые предложения есть, пишите в ЛС, свяжемся :)
Предложение одно — давайте летать вместе! :-)
vk.com/copterpilot?w=wall-73467743_4844%2Fall
Мне было бы очень любопытно глянуть, т.к. сам написал писал программу для коптера, она даже летала.
vk.com/copterpilot?w=wall-73467743_4844%2Fall
Мне было бы очень любопытно глянуть, т.к. сам написал писал программу для коптера, она даже летала.
Так и не удалось понять, в чем суть алгоиритма и отличие от уже существующих. Сам я давно работаю с немного оптимизированной реализацией «DCM filter» by Robert Mayhony, т.е. не новичек в этом, но так и не понял зачем так усложнять простые в общем-то вещи.
К примеру, насколько он свободен от искажения результата при ускорениях (линейных и центробежных)? Судя из фразы «Компенсация магнитных искажений в этом случае гарантирует, что магнитные возмущения влияют только на курс», компасс тут не помогает.
Тесты очень научные, респект автору. Но они выполнены в тепличных условиях, к примеру не проверялось скорость восстановления фильтра после перегрузки гироскопа (over-saturation).
Тесты очень научные, респект автору. Но они выполнены в тепличных условиях, к примеру не проверялось скорость восстановления фильтра после перегрузки гироскопа (over-saturation).
Напишите пожалуйста статью об используемом фильтре. Или, если хорошо разбираетесь — о фильтре Махони.
На сколько я понимаю есть еще фильтр MARG, который иногда работает даже лучше Madwick: Отчет от Себастьяна Маджвика. Что скажите о нем?
Код для STM32-Discovery: github.com/dccharacter/AHRS
Там можно выбрать либо этот фильтр, либо Махоуни
Передача данных на ПК через УАРТ или через ЮСБ (джойстик, ком-порт)
Матрицы и кватернионы
Скрипт для визуализации на питоне
Как скелет для вашего проекта пойдет, простите сразу за качество кода — я не настоящий маляр
Там можно выбрать либо этот фильтр, либо Махоуни
Передача данных на ПК через УАРТ или через ЮСБ (джойстик, ком-порт)
Матрицы и кватернионы
Скрипт для визуализации на питоне
Как скелет для вашего проекта пойдет, простите сразу за качество кода — я не настоящий маляр
Сравнение 1
Сравнение 2
Я подозреваю выигрыш этого алгоритма был получен в сравнении с плохо настроенным ФК.
И еще судя по последней ссылке, фильтром этот алгоритм и алгоритм Махоуни называют только из-за использования выраженного в виде передаточных функций ПИД.
Сравнение 2
Я подозреваю выигрыш этого алгоритма был получен в сравнении с плохо настроенным ФК.
И еще судя по последней ссылке, фильтром этот алгоритм и алгоритм Махоуни называют только из-за использования выраженного в виде передаточных функций ПИД.
Смотрю код (ред. 19/02/2012 SOH Madgwick Magnetometer measurement is normalised)
Почему-то Маджвик удалил коэффициент зета из алгоритма.
Ссылка на сам код на сайте Маджвика
Почему-то Маджвик удалил коэффициент зета из алгоритма.
Ссылка на сам код на сайте Маджвика
Если кто-то интересуется работами Маджвика: у него есть три известных публикации по одноименному фильтру (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
Наиболее известна публикация 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
Sign up to leave a comment.
Фильтр Маджвика