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

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

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

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

[FPGAz]

А это разве не эффект "схлопывания рамок"? ))

[alexPec]

А это разве не эффект "схлопывания рамок"? ))

В кватернионах такого нет. Это в углах эйлера. Просто действительно при повороте на 90 град углы местами меняются местами, вот мне и надо плавно, по ходу поворота на 90 град. менять регулируемую величину с одного угла на другой (учитывая и тот и другой, но какой-то в большей степени, какой-то в меньшей).
Да, вникнуть так сразу трудно, расчитываю на тех кто решал такое. Им должно быть понятно.


PS А нет у кого геометрии как получаются эти углы из кватерниона?
Почему именно например именно такие аргументы 2*q[1]*q[2]-2*q[0]*q[3] и 2*q[0]*q[0]+2*q[1]*q[1]-1? Почему еще минус 1? Подозреваю что
в другой комбинации элементы кватерниона дадут нужный угол.

Например где-то откопал такое вычисление угла тангажа:

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];
reg_ang=((atan(x/(sqrt(y*y+z*z))))*180)/3.141519;

Работает отлично. Но вот сейчас даже не могу найти откуда это взял. Хотел перечитать откуда взялось там чего.