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

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

Вот что я скажу:

В другой книжке - Applied Mathematics in Integrated Navigation Systems автора Rogers
вычитал:

Disadvantages (of quaternions имеется ввиду) include nonlinear terms in the result and the necessity of
renormalizations within the computational cycles to maintain properties previously
discussed for DCMs.

Там кватерниноны вообще упоминаются мельком, буквально полстраницы (по крайней мере, докуда я долистал),
но стало ясно следующее: на каждом шаге необходимо нормирование кватернионов.

По этому я в коде перед оператором завершения цикла end добавил следующее:
e = sqrt(q0^2 + q1^2 + q2^2 + q3^2);
q0 = q0/e;
q1 = q1/e;
q2 = q2/e;
q3 = q3/e;

и у меня сразу пропало медленное вращение по другим осям, комплексный угол по крену и ошибка комплексного аргумента арккосинуса.
При подстановке данных от реального ДУСа, правда, интегрировало немного с ошибкой - вращал на 30 градусов, а получил 40 - но тут, возможно, все дело в коэффициенте, еще перепроверю.
Поэтому остается только главный вопрос (все еще остается) - это уже к специалистам по ИНС, которые наверняка тут есть - в правильном ли направлении я иду?