angle.m

四元数的隆戈库塔法

function Ztaij=Angle()
global Q
Pi=3.1415926;
q0=Q(1);
q1=Q(2);
q2=Q(3);
q3=Q(4);
T11=q1^2+q0^2-q3^2-q2^2;%初始姿态矩阵元素确定
T12=2*(q1*q2-q0*q3);
T13=2*(q1*q3+q0*q2);
T21=2*(q1*q2+q0*q3);
T22=q2^2-q3^2+q0^2-q1^2;
T23=2*(q2*q3-q0*q1);
T31=2*(q1*q3-q0*q2);
T32=2*(q2*q3+q0*q1);
T33=q3^2-q2^2-q1^2+q0^2;
a=asin(T32);
b_1=atan(-T31/T33);
if abs(T22)<0.0001
    if T12<0
        b=-Pi/2;
    else b=Pi/2;
    end
elseif T22>0
    b=b_1;
    elseif T12>0
        b=b_1+Pi;
else  b=b_1-Pi;
end
c_1=atan(T12/T22);
if T33>=0
    c=c_1;
elseif c_1>=0
    c=c_1-Pi;
else  c=c_1+Pi;
end
Ztaij=[a c b]'