Добрый день, уважаемое сообщество!
Столкнулся с проблемой:
1. Работаю в Matlab/Simulink. Имею блок инерциальных датчиков — гироскоп, акселерометр, компас.
2. Из угловых скоростей в связанной системе координат интегрированием уравнения Пуассона получаю кватернион поворота локальной системы координат относительно глобальной.
3. По данным акселерометра и магнетометра путем векторного умножения получаю проекции ортов глобальной системы координат на локальную. Составляю матрицу направляющих косинусов, матирцу превращаю в кватернион.
4. В результате имею 2 кватерниона, один из них гладкий (от гироскопа), но с плывущим нулем. Кватернион от акселя и компаса зашумленный, но точно знает где была глобальная система.
5. Далее считаю кватернион рассогласования между этими двумя кватернионами и получаю из него угловые скорости для коррекции данных гироскопа, чтобы его данные стремились к данным с аксеелрометра и компаса.
6. Кватернионы, если их рассматривать как инструмент поворота вектора вокруг оси равнозначны. Тоесть если я единичный вектор вращаю этими двумя кватернионами, то он вращается одинаково в обеих случаях. С разницей лишь в характере вращения (дрожание, уплывание ноля).
Но если их рассматривать как вектор из 4 элементов — то кватернион, который получен преобразованием из матрицы, не во всей области определения равняется кватерниону от гироскопа. Он имеет разрывы, вызванные алгоритмом преобразования матрицы в кватернион. И скалярная часть никогда не уходит в отрицательные значения, потому как получается извлечением квадратного корня:
Так как одному и тому же повороту соответствуют два кватерниона, то в определенные моменты (когда у кватерниона с гироскопа скаляр уходит в минуса) он перескакивает от одного значения к другому. Для вращения это значения не имеет, но если мы хотим сравнить два кватерниона для создания управляющего воздействия — то возникает проблема.
Верхний график — кватернион с гироскопа.
Нижний — с акселерометра и компаса.
Они идентичны в плане вращения, но нижний не имеет скаляра ниже нуля (желтая линия), и поэтому скачут остальные компоненты.
Как мне получить из проекций осей одной системы координат на другую гладкий кватернион рассогласования этих двух систем? Можно ли как-то избежать использвания преобразования матрицы в кватернион?
В фундаментальных трудах Бранца и Шмыгалевского ответ не нашел. Может на Хабре кто подскажет.
Заранее благодарю за совет!
PS: Засел за написание собственной
костыля функции. Возможно это единственное решение.
Старался как можно подробней расписать, но могу уточнить проблему если потребуется.
Средний
Средний
Простой
