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

[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?
(это чтобы корень стал мнимым).

Спасибо.

[Mityan]

Данные с датчиков снимаются 50 раз в секунду. Полагаю, вычислительных мощностей TMS320 хватит. Ничего огромного в приведенных формулах не вижу.
Кстати, если я в своем матлабовском коде, там, где углы сохраняются в матрицу U, добавляю U2 = quat2angle([q0 q1 q2q 3]); - то есть родную функцию, она почему-то вообще какую-то ерунду выдает.

[Aner]

Данные с датчиков снимаются 50 раз в секунду. Полагаю, вычислительных мощностей TMS320 хватит. Ничего огромного в приведенных формулах не вижу.
Кстати, если я в своем матлабовском коде, там, где углы сохраняются в матрицу U, добавляю U2 = quat2angle([q0 q1 q2q 3]); - то есть родную функцию, она почему-то вообще какую-то ерунду выдает.

TMS320 не хватит.