getqn.m

自己编写的鲁棒滤波算法的应用

function [Q] = GetQn( Cbn )
%*********************************************************
% ************ 从方向余弦阵中解出四元数 ******************
%*********************************************************
T11 = Cbn(1,1);    T12 = Cbn(1,2);    T13 = Cbn(1,3);
T21 = Cbn(2,1);    T22 = Cbn(2,2);    T23 = Cbn(2,3);
T31 = Cbn(3,1);    T32 = Cbn(3,2);    T33 = Cbn(3,3);
%***************************************************四元数
q1 = sign(T32-T23)*0.5*sqrt(1+T11-T22-T33);
q2 = sign(T13-T31)*0.5*sqrt(1-T11+T22-T33);
q3 = sign(T21-T12)*0.5*sqrt(1-T11-T22+T33);
q0 = sqrt(1-q1^2-q2^2-q3^2);
Q = [q0  q1  q2  q3]';
%*********************************************************