cncltda.m

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

function  [DeA] = CncltDa(Ps,PsJ)
%************************************************************************
% *************失准角计算      [DeA] = CncltDa(Ps,PsJ)*******************
% DeA - 失准角向量    Ps - 真实姿态角向量     PsJ - 解算失准角向量
%************************************************************************
Pothi = Ps(1,:);       Theta = Ps(2,:);        Garma = Ps(3,:);
T11 =  cos(Garma).*cos(Pothi) - sin(Garma).*sin(Theta).*sin(Pothi);
T12 = -cos(Theta).*sin(Pothi);
T13 =  sin(Garma).*cos(Pothi) + cos(Garma).*sin(Theta).*sin(Pothi);
T21 =  cos(Garma).*sin(Pothi) + sin(Garma).*sin(Theta).*cos(Pothi);
T22 =  cos(Theta).*cos(Pothi);
T23 =  sin(Garma).*sin(Pothi) - cos(Garma).*sin(Theta).*cos(Pothi);
T31 = -sin(Garma).*cos(Theta);
T32 =  sin(Theta);
T33 =  cos(Garma).*cos(Theta);
% Cbt计算        t -- 真实地理坐标系
%***********************************************************************
PothiJ = PsJ(1,:);     ThetaJ = PsJ(2,:);       GarmaJ = PsJ(3,:);
T11J =  cos(GarmaJ).*cos(PothiJ) - sin(GarmaJ).*sin(ThetaJ).*sin(PothiJ);
T12J = -cos(ThetaJ).*sin(PothiJ);
T13J =  sin(GarmaJ).*cos(PothiJ) + cos(GarmaJ).*sin(ThetaJ).*sin(PothiJ);
T21J =  cos(GarmaJ).*sin(PothiJ) + sin(GarmaJ).*sin(ThetaJ).*cos(PothiJ);
T22J =  cos(ThetaJ).*cos(PothiJ);
T23J =  sin(GarmaJ).*sin(PothiJ) - cos(GarmaJ).*sin(ThetaJ).*cos(PothiJ);
T31J = -sin(GarmaJ).*cos(ThetaJ);
T32J =  sin(ThetaJ);
T33J =  cos(GarmaJ).*cos(ThetaJ);
% Cbp计算        p -- 数学平台坐标系
%************************************************************************
%DeA11 = T11J.*T11 + T12J.*T12 + T13J.*T13;
DeA12 = T11J.*T21 + T12J.*T22 + T13J.*T23;
DeA13 = T11J.*T31 + T12J.*T32 + T13J.*T33;
DeA21 = T21J.*T11 + T22J.*T12 + T23J.*T13;
%DeA22 = T21J.*T21 + T22J.*T22 + T23J.*T23;
DeA23 = T21J.*T31 + T22J.*T32 + T23J.*T33;
DeA31 = T31J.*T11 + T32J.*T12 + T33J.*T13;
DeA32 = T31J.*T21 + T32J.*T22 + T33J.*T23;
%DeA33 = T31J.*T31 + T32J.*T32 + T33J.*T33;
% Cbp = Ctp*Cbt;          Ctp = Cbp*Cbt'
%************************************************************************
DeA = [DeA23;  -DeA13;   DeA12];        % Apha  Beta  Gamar 失准角
%DeA = [DeA32;  -DeA31;   DeA21];