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

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


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


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

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

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

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

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

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


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


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



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

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

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


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


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


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


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

Старался как можно подробней расписать, но могу уточнить проблему если потребуется.