Гироскоп - вырождение угла, Как борются с этим? Опции

[alexPec]

Добрый день всем.

В гироскопе вычисляю угол рыскания через кватернионы по известной формуле:

atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1)

При угле тангажа в 90град. значение всегда примерно равно -90. Соответственно начинаются глюки, система управления чудит.

При этом (когда угол тангажа 90 град.) заметил, что в этом случае угол, вычисляемый как:

asin(2*q[1]*q[3]+2*q[0]*q[2]) это как раз то, что мне нужно.

Т.е. при приближении тангажа к 90 градусам регулируемая величина для системы управления должна плавно перейти от

atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1)

к

asin(2*q[1]*q[3]+2*q[0]*q[2]).

Может кто сталкивался и решал подобную задачу?

Или может классическое решение проблемы есть?

[Taran87]

Сталкивался с ситуацией пересчета угла рысканья (yaw) для мобильных, носимых устройств. Пересчет из кватернион в угол не дает всегда адекватный результат. Решил проблему при переходе в матрицу ДСМ, в которой точно видно при каких углах работает та или иная формула. Если задача еще актуально жду ответа.

[alexPec]

Сталкивался с ситуацией пересчета угла рысканья (yaw) для мобильных, носимых устройств. Пересчет из кватернион в угол не дает всегда адекватный результат. Решил проблему при переходе в матрицу ДСМ, в которой точно видно при каких углах работает та или иная формула. Если задача еще актуально жду ответа.

Да да да, актуально. Интересны подробности, если Вас не затруднит.

UPD Вроде понял, чего хочу. ))
Более формализованная задача:
Есть кватернион текущего поворота Qт, q[n] - его элементы По нему вычисляем углы, нужные мне, по формулам:

x=2*q[1]*q[3]-2*q[0]*q[2];
y=2*q[3]*q[2]+2*q[0]*q[1];
z=q[0]*q[0]+q[3]*q[3]-q[1]*q[1]-q[2]*q[2];

teta = ((atan(x/(sqrt(y*y+z*z))))*180)/3.141519;

phy = atan2(2*q[1]*q[2]-2*q[0]*q[3],2*q[0]*q[0]+2*q[1]*q[1]-1))*180)/3.141519;

Вопрос: как повернуть систему координат (найти кватернион поворота) так, чтобы teta стала равна 0.
Вот так вполне конкретно. Понятно что надо повернуть на угол, обратный teta, но вот относительно какого вектора? Че-то не соображу.
Интуитивно чувствую, что решение такой задачи однозначное.
При таком повороте как раз угол, который вырождается в 90 град, стает тем что нужно.