25 lines
830 B
Matlab
25 lines
830 B
Matlab
|
function Qb2n = m2q(Cb2n)
|
||
|
|
% 姿态阵转四元数
|
||
|
|
%
|
||
|
|
% Input: Cb2n
|
||
|
|
% Output: Qb2n
|
||
|
|
%
|
||
|
|
C11 = Cb2n(1,1); C12 = Cb2n(1,2); C13 = Cb2n(1,3);
|
||
|
|
C21 = Cb2n(2,1); C22 = Cb2n(2,2); C23 = Cb2n(2,3);
|
||
|
|
C31 = Cb2n(3,1); C32 = Cb2n(3,2); C33 = Cb2n(3,3);
|
||
|
|
if C11>=C22+C33
|
||
|
|
q1 = 0.5*sqrt(1+C11-C22-C33);
|
||
|
|
q0 = (C32-C23)/(4*q1); q2 = (C12+C21)/(4*q1); q3 = (C13+C31)/(4*q1);
|
||
|
|
elseif C22>=C11+C33
|
||
|
|
q2 = 0.5*sqrt(1-C11+C22-C33);
|
||
|
|
q0 = (C13-C31)/(4*q2); q1 = (C12+C21)/(4*q2); q3 = (C23+C32)/(4*q2);
|
||
|
|
elseif C33>=C11+C22
|
||
|
|
q3 = 0.5*sqrt(1-C11-C22+C33);
|
||
|
|
q0 = (C21-C12)/(4*q3); q1 = (C13+C31)/(4*q3); q2 = (C23+C32)/(4*q3);
|
||
|
|
else
|
||
|
|
q0 = 0.5*sqrt(1+C11+C22+C33);
|
||
|
|
q1 = (C32-C23)/(4*q0); q2 = (C13-C31)/(4*q0); q3 = (C21-C12)/(4*q0);
|
||
|
|
end
|
||
|
|
Qb2n = [q0; q1; q2; q3];
|
||
|
|
|