Дано:
B — 3-х мерный вектор магнитной индукции (получаем с магнетометра)
a — вектор ускорения (в состоянии покоя = вектору ускорения свободного падения)
Найти:
кватернион поворота относительно Земли ( NED — X=North, Y=East, Z=Down)
(ну или матрицу — это не важно)
На данный момент я знаю, что есть 5-10 алгоритмов для решения этой задачи- QUEST(QUaternion ESTimator), FQA (Factored Quaternion Algorithm), ESOQ (EStimator of the Optimal Quaternion), ESOQ-2, FOAM (Fast Optimal Attitude Matrix), еще есть решение через SVD (сингулярное разложение)
Проблема в том, что я не могу найти ни одной реализации ни одного из этих алгоритмов. Причем, судя по всему, все они были реализованы только в матлабе, но даже код матлаба я не могу найти. Уверен, что определение ориентации в пространстве относительно Земли — это типовая задача для любых ездящих/летающих/плавающих штук, которые делаете/делали вы, уважаемые подписчики хаба DIY. Странно, что так сложно найти информацию
А в чем проблема-то? Элементарно же все делается: есть вектор ускорения (т.е. мы знаем, где «низ»); есть вектор МП. Вычисляем поправки на широту-долготу местонахождения датчика, получаем вектор направления меридиана. Все: мы можем вычислить ориентацию датчика в сферической СК относительно поверхности Земли.
Я тоже не смог получить кватернион. Любые углы, сферические или цилиндрические координаты — всё, что угодно… То есть, всё, что я могу представить в объёме. Но — каждый раз gimbal lock на полюсах. Говорят, кватернион — одно из решений проблемы, но там столько теории, что я так и не смог его реализовать.
А, дошло.
У вас проблема в скачке азимута при переходе через зенит/надир.
Вот, например: wat.gamedev.ru/articles/quaternions/
просто преобразуете вектор широта-долгота-угол поворота в кватернион.
Смысл использования кватернионов именно в том чтобы избавиться от углов эйлера и гимбал лока. Это значит, что я не оперирую понятиями азимута, зенита и тангажа. Я просто поворачиваю локальную систему координат вокруг некой оси в реверенсной системе координат. Задача именно в том чтобы найти эту ось и угол (чем, собственно, и является кватернион).
Простой
