Получение гладкого кватерниона из матрицы направляющых косинусов?

Добрый день, уважаемое сообщество!


Столкнулся с проблемой:


1. Работаю в Matlab/Simulink. Имею блок инерциальных датчиков — гироскоп, акселерометр, компас.

2. Из угловых скоростей в связанной системе координат интегрированием уравнения Пуассона получаю кватернион поворота локальной системы координат относительно глобальной.

3. По данным акселерометра и магнетометра путем векторного умножения получаю проекции ортов глобальной системы координат на локальную. Составляю матрицу направляющих косинусов, матирцу превращаю в кватернион.

4. В результате имею 2 кватерниона, один из них гладкий (от гироскопа), но с плывущим нулем. Кватернион от акселя и компаса зашумленный, но точно знает где была глобальная система.

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

6. Кватернионы, если их рассматривать как инструмент поворота вектора вокруг оси равнозначны. Тоесть если я единичный вектор вращаю этими двумя кватернионами, то он вращается одинаково в обеих случаях. С разницей лишь в характере вращения (дрожание, уплывание ноля).


Но если их рассматривать как вектор из 4 элементов — то кватернион, который получен преобразованием из матрицы, не во всей области определения равняется кватерниону от гироскопа. Он имеет разрывы, вызванные алгоритмом преобразования матрицы в кватернион. И скалярная часть никогда не уходит в отрицательные значения, потому как получается извлечением квадратного корня:


Так как одному и тому же повороту соответствуют два кватерниона, то в определенные моменты (когда у кватерниона с гироскопа скаляр уходит в минуса) он перескакивает от одного значения к другому. Для вращения это значения не имеет, но если мы хотим сравнить два кватерниона для создания управляющего воздействия — то возникает проблема.

Quaternions.png

Верхний график — кватернион с гироскопа.

Нижний — с акселерометра и компаса.

Они идентичны в плане вращения, но нижний не имеет скаляра ниже нуля (желтая линия), и поэтому скачут остальные компоненты.


Как мне получить из проекций осей одной системы координат на другую гладкий кватернион рассогласования этих двух систем? Можно ли как-то избежать использвания преобразования матрицы в кватернион?


В фундаментальных трудах Бранца и Шмыгалевского ответ не нашел. Может на Хабре кто подскажет.


Заранее благодарю за совет!


PS: Засел за написание собственной костыля функции. Возможно это единственное решение.

Старался как можно подробней расписать, но могу уточнить проблему если потребуется.
  • Вопрос задан
  • 5313 просмотров
Пригласить эксперта
Ответы на вопрос 2
@kometa_triatlon
Пап, а с кем это ты разговаривал? :)
Ответ написан
dr_brain
@dr_brain
вдруг поможет
avs.chat.ru/sphere3d.htm

мне кажется что проблема в шаге 3
Если я ничего не путаю, то преобразование A[Матрица Вращения] -> [Кватернион] -> B[Матрица Вращения]: A !=B

Ещё, если где нибудь используется вычисление обратной матрицы, то необходимо проверить правильность этой функции.
Например в DirectX D3DX вычисление обратной матрицы работает корректно не для всех значений. Я брал реализацию из nVidia SDK
Ответ написан
Ваш ответ на вопрос

Войдите, чтобы написать ответ

Войти через центр авторизации
Похожие вопросы
Medicineteaser.ru Краснодар
от 100 000 до 150 000 руб.
meyvn Санкт-Петербург
от 80 000 руб.
Archeads Inc. Москва
от 130 000 до 160 000 руб.
15 февр. 2019, в 20:17
1500 руб./за проект
15 февр. 2019, в 20:03
1000 руб./за проект