Вопрос по математике кватернионов Опции

[Mityan]

Речь идет об определении положения объекта по его угловым скоростям.

В литературе написано: получаем начальный кватернион q0 q1 q2 q3, потом есть формулы, которые вычисляют производную кватерниона на основе угловых скоростей. Интегрируя эту производную, получаем кватернион, который можно также по соответствующим формулам пересчитать в углы эйлера.

Сделал так (в МАТЛАБе):
% Задал начальное положение - курс, тангаж, крен.
yaw = deg2rad(54);
pitch = deg2rad(-20);
roll = 0;
% вычислил кватернион (начальный)
dcm = angle2dcm(yaw, pitch, roll);
quat = dcm2quat(dcm);
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
% Здесь можно было сразу angle2quat - не суть важно.
% Затем задал, как у меня будет вращаться тело -
% по одной из осей со скоростью 1 градус в секунду 36 секунд.
p = [zeros(30,1); deg2rad(1)*ones(36,1); zeros(34,1)]; % крен
q = zeros(100,1);
r = zeros(100,1);
U = zeros(100,3); % Углы
% Затем в цикле вычисляю углы Эйлера - фи, тэта, пси
% После этого производную кватерниона на основе q0 q1 q2 q3 p q r
% Обновляю значение q = q + dq - и на следующий круг.
for i = 1:1:len
theta = asind(-2*(q1*q3 - q0*q2));
s = sign(2*(q2*q3 + q0*q1));
u = q0^2 - q1^2 - q2^2 + q3^2;
l = sqrt(1 - 4*(q1*q3 - q0*q2)^2);
phi = acosd(u/l)*s;
s = sign(2*(q1*q2 + q0*q3));
u = q0^2 + q1^2 - q2^2 - q3^2;
psi = acosd(u/l)*s;
U(i,1) = theta;
U(i,2) = phi;
U(i,3) = psi;
dq0 = -(q1*p(i) + q2*q(i) + q3*r(i))/2;
dq1 = (q0*p(i) + q2*r(i) - q3*q(i))/2;
dq2 = (q0*q(i) + q3*p(i) - q1*r(i))/2;
dq3 = (q0*r(i) + q1*q(i) - q2*p(i))/2;
q0 = q0 + dq0;
q1 = q1 + dq1;
q2 = q2 + dq2;
q3 = q3 + dq3;
end

Математика взята из книги Performance, Stability, Dynamics and Control of Airplanes автора Pamadi
Получаю вот что (задавая разные скорости вращения):
1) Когда угол поворота переваливает за 180 градусов, он дальше становится комплексным и растет в отрицательную сторону,
например, 185 градусов видно как 180 - 5i
2) Углы другие тоже изменяются. Например, если у меня крен 4град*36с = 148град, то курс вместо 54 стал 51.8, а тангаж вместо -20 стал -20.9
3) Когда подставил вектор отсчетов реальных с датчика угловых скоростей (за вычетом zero-rate level), получил сообщение от МАТЛАба, что аргумент арккосинуса должен быть действительным. Это величина l = sqrt(1 - 4*(q1*q3 - q0*q2)^2); становится меньше 0.

Вопросы:
1) правильна ли сама логика, или я в чем-то запутался и что-то не учел?
2) почему изменяются остальные углы
4) почему получается комплексный угол
3) в книге (стр.347) ничего не говорится о каких-то пределах кватернионов, эта формула приведена ничтоже сумняшеся,
и непонятно, у тех, кто тоже занимается кватернионами, q1*q3 - q0*q2 никогда-никогда не бывает > 0.5?
(это чтобы корень стал мнимым).

Спасибо.

[Aner]

По вашему я заблуждаюсь, тогда вперед на грабли, вы не первый уверяю вас. Задумайтесь над точностью требуемую вам, обратите внимание на допущения и ограничения при уменьшении порядка дифуров. Пробуйте, просьба проинфрмировать на чем застряните.

[AndreyVN]

Честно скажу, с кватернионами не работал, но доводилось писать программу по молекулярной механике.
Для визуализации 3D молекулы приходилось переходить из системы координат центра масс молекулы в глобальную систему координат и отрисовывать молекулу в соответствии с текущими углами поворота.

Матрицу с углами расписал, и к каждой операции приходилось добавлять, кажется пару if(), чтобы избежать неопределенности с углам, ок которой Вы говорили. Как я понимаю, у Вас задача та-же самая. Было это давно, я пользовался я книгой Дж. Фоли, А. вэн Дэм, Основы интерактивной машинной графики. М.Мир, 1985. К стати, там есть опечатки именно в формулах на повороты.

Стоит ли ради шести операций if() углубляться в кватернионные дебри?

Вот, нашел процедуру, в которой вычислялись повороты координат атомов, по крайней мере, советую сравнить вычислительные мощности для обычной геометрии и кватернионной.

/** Повернуть координаты атомов и оси инерции в молекуле MATH*****/
void TMain::RotMolPos( int N, float dA, int MovRot)
{
int i;
double CosdA,SindA;
float x,y,z,Ii1a,Ii1b,Ii1g,Ii2a,Ii2b,Ii2g,Ii3a,Ii3b,Ii3g;
CosdA = cos(dA);
SindA = sin(dA);
for(i=0; i< Calc->M[N].maxatom; i++){ /*повернуть все атомы в молекуле*/
x = Calc->M[N].X[i]-Calc->M[N].X0;
y = Calc->M[N].Y[i]-Calc->M[N].Y0;
z = Calc->M[N].Z[i]-Calc->M[N].Z0;
if(MovRot==1){ /* вокруг оси Z */
Calc->M[N].X[i]=(x*CosdA - y*SindA) + Calc->M[N].X0;
Calc->M[N].Y[i]=(x*SindA + y*CosdA) + Calc->M[N].Y0; }
if(MovRot==2){ /* вокруг оси Y */
Calc->M[N].X[i]=(x*CosdA + z*SindA) + Calc->M[N].X0;
Calc->M[N].Z[i]=(z*CosdA - x*SindA) + Calc->M[N].Z0; }
if(MovRot==3){ /* вокруг оси X */
Calc->M[N].Y[i]=(y*CosdA - z*SindA) + Calc->M[N].Y0;
Calc->M[N].Z[i]=(y*SindA + z*CosdA) + Calc->M[N].Z0; }
}
/*повернуть напрвляющие косинусы, задающие главные оси инерции*/
Ii1a = Calc->M[N].I1a;
Ii1b = Calc->M[N].I1b;
Ii1g = Calc->M[N].I1g;
Ii2a = Calc->M[N].I2a;
Ii2b = Calc->M[N].I2b;
Ii2g = Calc->M[N].I2g;
Ii3a = Calc->M[N].I3a;
Ii3b = Calc->M[N].I3b;
Ii3g = Calc->M[N].I3g;
if(MovRot==1){ /* вокруг оси Z */
Calc->M[N].I1a = Ii1a*CosdA - Ii1b*SindA;
Calc->M[N].I2a = Ii2a*CosdA - Ii2b*SindA;
Calc->M[N].I3a = Ii3a*CosdA - Ii3b*SindA;
Calc->M[N].I1b = Ii1a*SindA + Ii1b*CosdA;
Calc->M[N].I2b = Ii2a*SindA + Ii2b*CosdA;
Calc->M[N].I3b = Ii3a*SindA + Ii3b*CosdA; }
if(MovRot==2){ /* вокруг оси Y */
Calc->M[N].I1a = Ii1a*CosdA + Ii1g*SindA;
Calc->M[N].I2a = Ii2a*CosdA + Ii2g*SindA;
Calc->M[N].I3a = Ii3a*CosdA + Ii3g*SindA;
Calc->M[N].I1g = Ii1g*CosdA - Ii1a*SindA;
Calc->M[N].I2g = Ii2g*CosdA - Ii2a*SindA;
Calc->M[N].I3g = Ii3g*CosdA - Ii3a*SindA; }
if(MovRot==3){ /* вокруг оси X */
Calc->M[N].I1b = Ii1b*CosdA - Ii1g*SindA;
Calc->M[N].I2b = Ii2b*CosdA - Ii2g*SindA;
Calc->M[N].I3b = Ii3b*CosdA - Ii3g*SindA;
Calc->M[N].I1g = Ii1b*SindA + Ii1g*CosdA;
Calc->M[N].I2g = Ii2b*SindA + Ii2g*CosdA;
Calc->M[N].I3g = Ii3b*SindA + Ii3g*CosdA; }
}