qa.m

星命转换 星命转换 星命转换

%通过姿态矩阵A计算姿态四元素Q
function Q=Qa(A)
a=A(3,1)-A(1,3);
b=A(2,3)-A(3,2);
c=A(1,2)-A(2,1);
if a==0
    a=1;
end
if b==0
    b=1;
end
if c==0
    c=1;
end
if (1+A(1,1)+A(2,2)+A(3,3)) >= 0.004
    Q(4)=sqrt(1+A(1,1)+A(2,2)+A(3,3))/2;
    Q(1)=(A(2,3)-A(3,2))/4/Q(4);
    Q(2)=(A(3,1)-A(1,3))/4/Q(4);
    Q(3)=(A(1,2)-A(2,1))/4/Q(4);
elseif ((1+A(1,1)+A(2,2)+A(3,3)) < 0.004) & ((1-A(1,1)+A(2,2)-A(3,3)) >=  0.004)
    Q(2)=sqrt(1-A(1,1)+A(2,2)-A(3,3))/2*sign(a);
    Q(1)=(A(1,2)+A(2,1))/4/Q(2);
    Q(3)=(A(2,3)+A(3,2))/4/Q(2);
    Q(4)=(A(3,1)-A(1,3))/4/Q(2);
elseif ((1+A(1,1)+A(2,2)+A(3,3)) < 0.004) & ((1-A(1,1)+A(2,2)-A(3,3)) <  0.004) & ((1+A(1,1)-A(2,2)-A(3,3)) >=  0.004)
    Q(1)=sqrt(1+A(1,1)-A(2,2)-A(3,3))/2*sign(b);
    Q(2)=(A(1,2)+A(2,1))/4/Q(1);
    Q(3)=(A(1,3)+A(3,1))/4/Q(1);
    Q(4)=(A(2,3)-A(3,2))/4/Q(1);
else
    Q(3)=sqrt(1-A(1,1)-A(2,2)+A(3,3))/2*sign(c);
    Q(1)=(A(1,3)+A(3,1))/4/Q(3);
    Q(2)=(A(2,3)+A(3,2))/4/Q(3);
    Q(4)=(A(1,2)-A(2,1))/4/Q(3);
end