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

[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]).

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

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

[spl]

Прошу прощения за некромантию.
Уважаемый AlexPec, удалось ли вам повернуть систему координат на угол обратный teta?
А то мне как-раз требуется проделать такой-же фокус, только дополнительно еще повернуть и на угол обратный phy. Т.е. чтобы осталось только вращение по одному углу psi.