thrsmpl.m

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

function [Q, Cbn] = ThrSmpl(Q, Deta1, Deta2, Deta3)
%***********************************************************************
% ******************** 用三子样法更新四元数 ****************************
%   Deta(i) - 角增量   Q - 四元数   Cbn -方向余弦阵
%***********************************************************************
RotaV = Deta1 + Deta2 + Deta3 + 0.45*cross(Deta1,Deta3);
RotaV = RotaV + 0.675*cross( Deta2,(Deta3-Deta1) );
%***********************************************************等效旋转角度
Vphe  = sqrt(RotaV'*RotaV);
%*************************************************************四元数更新
if Vphe ~= 0      
        CVphe = cos(Vphe/2);  SVphe = sin(Vphe/2)/Vphe;
        DetaQ = [CVphe; SVphe*RotaV];
        WQ = [Q(1)  -Q(2)  -Q(3) -Q(4)
              Q(2)   Q(1)  -Q(4)  Q(3)
              Q(3)   Q(4)   Q(1) -Q(2)
              Q(4)  -Q(3)   Q(2)  Q(1)];
        Q  = WQ * DetaQ;
end
Q = Q/sqrt(Q(1)^2+Q(2)^2+Q(3)^2+Q(4)^2);         % 四元素最佳归一化
q0  = Q(1);  q1 = Q(2);  q2 = Q(3);  q3 = Q(4);
%*********************************************************方向余弦阵更新
Cbn = [q0^2+q1^2-q2^2-q3^2,  2*(q1*q2-q0*q3),    2*(q1*q3+q0*q2);
        2*(q1*q2+q0*q3),   q0^2-q1^2+q2^2-q3^2,  2*(q2*q3-q0*q1);
        2*(q1*q3-q0*q2),     2*(q2*q3+q0*q1),   q0^2-q1^2-q2^2+q3^2];
%***********************************************************************