roboforum.ru

Собственно, сабж. Есть данные вектор магнитной индукции (от магнетометра) и вектор линейного ускорения (пусть, для простоты, это только гравитация)
Задача - получить кватернион или матрицу ориентации относительно референсной системы координат NED (XYZ = North-East-Down)
Что-то уже перерыл кучу публикаций, нашел много ссылок на некие алгоритмы (QUEST, FQA, ESOQ и другие) но не могу найти реализацию ни одного из них. Буду безмерно благодарен за помощь в данном вопросе.

Чистая математика с векторами.

1. находишь угол между 2мя векторами гравитации (0,0,1) и твоим с акса (x,y,z)
http://www.pm298.ru/preobr4.php
2. векторное произведение между этими векторами дает тебе перпендикулярный вектор.
3. составляешь матрицу поворота своего вектора относительно перпендикулярного вектора (2) на угол (1)
http://ru.wikipedia.org/wiki/%D0%9C%D0% ... 1%82%D0%B0
4. остается только проекция x и y осей (двухмерная система), поворачиваешь этот вектор к вектору магнитометра. грубо говоря вторая матрица поворота отночительно оси Z

в помощь:
http://habrahabr.ru/post/131931/
http://ru.wikipedia.org/wiki/%D0%9C%D0% ... 1%82%D0%B0

смотри реализации madgwick, mahony

Добавлено спустя 1 минуту 53 секунды:
https://github.com/dccharacter/AHRS

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

Пол года назад прочитал это и ничего не понял. Долго рылся, везде, понял что готовое не найти- кроме готовых реализаций с фильтром вместе. Решил решать задачу сам, подумал и решил - через кватернионы, чуть иначе, но тоже в два поворота. Сейчас натыкаюсь на этот комментарий, читаю его снова и понимаю, что это был правильный ответ. Просто тогда я к нему не был готов

Спасибо!

Считать через матрицы - отстой. Через кватернионы правильнее.
Я вот об это мосг себе сломал в свое время: http://diydrones.com/profiles/blogs/dcm ... irst-draft
А кватернионы - перемножил их и усе...

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

Ну, я не думаю, что, прямо, отстой. Очень сильно зависит от области применения. Кватернионы лучше матриц только тем что они короче и для их умножения нужно меньше операций. Зато они ограничены единственной трансформацией- поворотом. А матрицы могут объединять в себе сразу много линейных трансформаций- и поворот, и перенос, и наклоны, проекции. И комбинировать можно теми-же самыми умножениями. Кроме того, например OpenGL умеет очень быстро перемножать матрицы. Быстрее чем можно руками кватернионы перемножать. Еще нужно помнить, что для применения поворота в случае с матрицей- мы просто на нее умножаем вектор. А в случае с кватернионами- умножаем кватернион на вектор и на обратный кватернион, который тоже нужно вычислить, а для этого нужно деление и квадратный корень. Так что, количество операций, хоть и меньше, но не всегда быстрее- матрицы обходятся быстрыми операциями сложения и умножения.

Ну на ардуине я юзаю кватернионы для ориентации. Но матрицы- для калибровки сенсоров.
Сделал раширенный фильтр Калмана на ардуину ... и она не потянула. Слишком медленно матрицы умножает- в реальном времени нереал.

stm32f3-discovery. 32бит, 72 мгц, аппаратный fpu, гиро, аксель, компас.
Вот нафиг дуню мучать?

Ну хотя бы потому что она НАМНОГО меньше

размер при желании это поправимо - кроме дискавери полно всяких других плат, да и самому можно сделать

Ребята подскажите. Может кто использовал данный алгоритм.
Хочу сделать AHRS(курсовертикаль) для робота, по сути в моём случае мне нужен минимум компас, угол рысканья.
Как считывать данные с сенсоров разобрался.
Собираюсь использовать алгоритм Маджвика(Madgwick algorithm).
В каком виде нужно передать данные с акселя, гироскопа и магнитометра на вход алгоритма?
Какие настройки у алгоритма?
И как получить угол рысканья из выходных данных?

Реализация алгоритма Маджвика на С http://www.x-io.co.uk/res/sw/madgwick_algorithm_c.zip
Чья то реализация на STM32F3 https://github.com/dccharacter/AHRS
Использую сенсоры L3GD20 http://www.st.com/web/en/resource/technical/document/datasheet/DM00036465.pdf
и
LSM303D http://www.st.com/web/en/resource/technical/document/datasheet/DM00057547.pdf
Я делаю на STM32F4, подключены через i2c.

для всех данных нужно вычитать bias (смещение 0),
гиро и аксель калибруются легко, магнетометр сложенее, подробнее тут https://github.com/ptrbrtz/razor-9dof-a ... alibration

Для алгоритма нужно чтобы ось Z была вниз, т.е. микросхема/плата должна быть перевёрнута вверх ногами.



Коэффициенты ПД регулятора Kp, Ki и частота вычисления положения по новым данным.



В радианах (yaw - угол рысканья):


Но не ждите много от этого датчика и алгоритма, курс будет уплывать.
В моём блоге на первых 2 страницах несколько роликов как я добивался устойчивого курса forum87/topic15337.html

Даже если вы всё откалибруете верно, гиро довольно быстро уплываете, а магнетометр врёт в зависимости от окружающей среды, вот пример forum87/topic15337-15.html#p326155

Начиная с 3тьей страницы диалог наш с Дмитрием по поводу нужен компас или нет и если без компаса то какой гиро можно считать "приличным".

_________________
R3 forum87/topic15337.html
R2 roboforum.ru/forum102/topic13980.html
R4 roboforum.ru/post336339.html#p336339
Делать надо хорошо - плохо получится само!

Спасибо.

Я в коде одной реализации наткнулся на


Зачем данные с гироскопа умножаются на Pi и делятся на 180?
В каком диапазоне должны быть числа входных данных MadgwickAHRSupdate?
Частота обновления как раз важна для вычисления сдвига гироскопа? градусы * время ?
В той же реализации данные с датчиков делятся на какие-либо коэффициенты, я так понял соответствующие чувствительности сенсоров, как их подобрать?

Делить на Pi и умножить на 180, это есть перевод значений угла из радиан в градусы.

http://ru.yasno.tv/article/math/45-ugly ... -i-radiany


гиро в радианах, остальное неважно

почитайте что каждый датчик выдаёт на выходе.

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

Я бы рекомендовал начать с готовой программы для ардуино, которая позволяет калибровать и получать результат https://github.com/ptrbrtz/razor-9dof-ahrs

а уж потом с пониманием дела писать своё на STM32

следуйте инструкции по настройке https://github.com/ptrbrtz/razor-9dof-a ... i/Tutorial

_________________
R3 forum87/topic15337.html
R2 roboforum.ru/forum102/topic13980.html
R4 roboforum.ru/post336339.html#p336339
Делать надо хорошо - плохо получится само!

https://github.com/dccharacter/AHRS хошь маджвик хошь махони хошь через сериал хошь через юсби
Там же скрипт на питоне для визуализации

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