q0fun.m

四元数的隆戈库塔法

function Q0=Q0fun()
global M
global Q0
a=M(1,2);%（朱）俯仰角faia xita   --
b=M(1,3);%（朱）偏航角fai2 pusai  --
c=M(1,4);%（朱）自转角gama        --
T11=cos(c)*cos(b)+sin(c)*sin(b)*sin(a);%由初始角进行初始姿态矩阵元素确定
T12=-cos(c)*sin(b)+sin(c)*cos(b)*sin(a);
T13=-sin(c)*cos(a);
T21=sin(b)*cos(a);
T22=cos(b)*cos(a);
T23=sin(a);
T31=sin(c)*cos(b)-cos(c)*sin(b)*sin(a);
T32=-sin(c)*sin(b)-cos(c)*cos(b)*sin(a);
T33=cos(c)*cos(a);
q00=0.5*sqrt(1+T11+T22+T33);%由初始姿态阵确定初始四元数
q10=0.5*sqrt(1+T11-T22-T33);
q20=0.5*sqrt(1-T11+T22-T33);
q30=0.5*sqrt(1-T11-T22+T33);
m=max([q00 q10 q20 q30]);
if m==q00
    q0=abs(0.5*sqrt(1+T11+T22+T33));
    q1=(T23-T32)/(4*q0);
    q2=(T31-T13)/(4*q0);
    q3=(T12-T21)/(4*q0);
else if m==q10
    q1=sign(T32-T23)*0.5*sqrt(1+T11-T22-T33);
    q2=(T12+T21)/(4*q1);
    q3=(T13+T31)/(4*q1);
    q0=abs((T23+T32)/(4*q1));
    else if m==q20
            q2=sign(T13-T31)*0.5*sqrt(1-T11+T22-T33);
            q3=(T23+T32)/(4*q2);
            q0=(T31-T13)/(4*q2);
            q1=abs((T12+T21)/(4*q2));
        else m==q30
            q3=sign(T21-T12)*0.5*sqrt(1-T11-T22+T33);
            q0=abs((T12-T21)/(4*q3));
            q1=(T13+T31)/(4*q3);
            q2=(T23+T32)/(4*q3);
        end
    end
end
Q0=[q0 q1 q2 q3]'