trcgnrt.m

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

function [Wibb,Fb,Ps,Vt,St] = TrcGnrt(A,V0,St0,PsA,Frqw,L,ts)
%***************************************************************************
%********************   飞行器轨迹发生器  **********************************
%***********************************************************************常值
% G -- 重力加速度;   wie -- 地球转动角速度;   Re -- 地球半径;
G  = [0  0  -9.8]';    wie = 0.000072921;     Re  = 6378137;
%***********************************************************************时间
% xs -- 每个采样周期解算3次;      PltLth -- 绘图时矩阵长度;
% ArryLth -- 解算矩阵长度;        L -- 圆整后时间;
xs = 6;                PltLth  = fix(L/ts+1);
L  = (PltLth-1)*ts;    ArryLth = xs*(PltLth-1) + 1;
Vmax = 10;      % 最大速度20m/s
lmax=0;
if  A~=0
tmax = (abs((Vmax-V0)/A));     lmax = fix(tmax/ts);
end
%****************************************************************输出姿态轨迹
pothiA =PsA(1);    thetaA = PsA(2);    garmaA = PsA(3);   pothix = pi/18;
Frqw1 = Frqw(1);     Frqw2 = Frqw(2);   Frqw3 = Frqw(3);
if Frqw1==0
    Pothi  = pothiA*ones(1,PltLth);              %航向
    PothiM = pothiA*ones(1,ArryLth);
    PothiD = pothiA*zeros(1,ArryLth);
else
    %------------------------------------------------------------------------
    Pothi  = 2*pi*(0 : Frqw1*ts : Frqw1*L) + pothiA*ones(1,PltLth); %圆周运动
    PothiM = 2*pi*(0:Frqw1*ts/xs: Frqw1*L) + pothiA*ones(1,ArryLth);
    PothiD = 2*pi*Frqw1*ones(1,ArryLth);
    Pothi  = Pothi - fix(Pothi/(2*pi)) * 2*pi;  %使Pothi<360度
    PothiM = PothiM - fix(PothiM/(2*pi)) * 2*pi;
    %------------------------------------------------------------------------
    %Pothi  = pothix * sin( 0 : 2*pi*Frqw1*ts : 2*pi*Frqw1*L );  % 正弦运动
    %PothiM = pothix * sin( 0 : 2*pi*Frqw1*ts/xs : 2*pi*Frqw1*L );
    %PothiD = 2*pi*Frqw1*pothix * cos( 0 : 2*pi*Frqw1*ts/xs: 2*pi*Frqw1*L );
    %Pothi  =Pothi+pothiA*ones(1,PltLth); PothiM=PothiM+pothiA*ones(1,ArryLth);
end
if Frqw2==0
    %------------------------------------------------------------------------
    Theta  = thetaA*ones(1,PltLth);              %俯仰
    ThetaM = thetaA*ones(1,ArryLth);
    ThetaD = thetaA*zeros(1,ArryLth);
else
    %-----------------------------------------------------------------------
    Theta  = thetaA * sin( 0 : 2*pi*Frqw2*ts : 2*pi*Frqw2*L );   % 正弦运动
    ThetaM = thetaA * sin( 0 : 2*pi*Frqw2*ts/xs : 2*pi*Frqw2*L );
    ThetaD = 2*pi*Frqw2*thetaA * cos( 0 : 2*pi*Frqw2*ts/xs: 2*pi*Frqw2*L );
end
if Frqw3==0
    %-----------------------------------------------------------------------
    Garma  = garmaA*ones(1,PltLth);              %俯仰
    GarmaM = garmaA*ones(1,ArryLth);
    GarmaD = garmaA*zeros(1,ArryLth);
else
    %-----------------------------------------------------------------------
    Garma  = garmaA * sin( 0 : 2*pi*Frqw3*ts : 2*pi*Frqw3*L );  % 正弦运动
    GarmaM = garmaA * sin( 0 : 2*pi*Frqw3*ts/xs : 2*pi*Frqw3*L );
    GarmaD = 2*pi*Frqw3*garmaA * cos( 0 : 2*pi*Frqw3*ts/xs : 2*pi*Frqw3*L );
end
Ps = [Pothi; Theta; Garma];
%********************************************************飞行器的绝对速度计算
if A==0
    VbM = V0*ones(1,ArryLth);     Vb  = V0*ones(1,PltLth);
else
    VbM = (V0: A*ts/xs : V0+A*L);
    Vb  = (V0: A*ts : V0+A*L);
end
if PltLth>lmax&A~=0
    VbM(xs*lmax+1:ArryLth) = Vmax*ones(1,ArryLth-xs*lmax);
    Vb (lmax+1:PltLth) = Vmax*ones(1,PltLth-lmax);
end
%**********************飞行器速度在地理坐标系的分量
VtM = [-VbM.*sin(PothiM); VbM.*cos(PothiM); zeros(1,ArryLth)];
Vt  = [-Vb.*sin(Pothi); Vb.*cos(Pothi); zeros(1,PltLth)];
VxtM= VtM( 1, :);  VytM = VtM( 2, :);  Vxt = Vt( 1, :);  Vyt = Vt( 2, :);
%******************************************************通过差分速度得到加速度
AtM = [(diff(VtM'))'*xs/ts,  zeros(3,1)];
%At = [-A*sin(Pothi).*cos(Theta); A*cos(Pothi).*cos(Theta); A*sin(Theta)];
%******************************************************************经纬度轨迹
PheM   = St0(2) + cumtrapz(VytM)*ts/xs/Re;
LamdaM = St0(1) + cumtrapz(VxtM)*ts/xs/Re.*sec(PheM);
Phe   = St0(2) + cumtrapz(Vyt)*ts/Re;
Lamda = St0(1) + cumtrapz(Vxt)*ts/Re.*sec(Phe);
St    = [Lamda;Phe];
%*****************************************************************************
%****************************  陀螺仪测量计算   ******************************
%****************************************************Cbn—由b系到n系的转换矩阵 
T11M =  cos(GarmaM).*cos(PothiM) - sin(GarmaM).*sin(ThetaM).*sin(PothiM);
T12M = -cos(ThetaM).*sin(PothiM);
T13M =  sin(GarmaM).*cos(PothiM) + cos(GarmaM).*sin(ThetaM).*sin(PothiM);
T21M =  cos(GarmaM).*sin(PothiM) + sin(GarmaM).*sin(ThetaM).*cos(PothiM);
T22M =  cos(ThetaM).*cos(PothiM);
T23M =  sin(GarmaM).*sin(PothiM) - cos(GarmaM).*sin(ThetaM).*cos(PothiM);
T31M = -sin(GarmaM).*cos(ThetaM);
T32M =  sin(ThetaM);
T33M =  cos(GarmaM).*cos(ThetaM);
Cbn0 = [T11M(1), T12M(1), T13M(1);
        T21M(1), T22M(1), T23M(1);
        T31M(1), T32M(1), T33M(1)];
Ctn = eye(3,3);
%*********************************地球转动引起地球坐标系相对惯性坐标系的角速度
Wiet = [zeros(1,ArryLth);  wie*cos(PheM);  wie*sin(PheM)];
%*************************************飞行器运动引起的导航系相对地球系的角速度
Went = [-VytM/Re; VxtM/Re; VxtM.*tan(PheM)/Re];
%***************************************导航系相对惯性系的角速度以地理系为参考
Wint = Wiet+Went;
%*************************************飞行器相对于地理系的角速度以机体系为参考
% PsCh = [PothiD   ThetaD   GarmaD]'  见82页
% Wnbb = [-sin(Garma)*cos(Theta)    cos(Garma)   0;
%             sin(Theta)                0        1;*PsCh
%          cos(Garma)*cos(Theta)    sin(Garma)   0]
Wnbb = [-sin(GarmaM).*cos(ThetaM).*PothiD + cos(GarmaM).*ThetaD;
                                   sin(ThetaM).*PothiD + GarmaD;
         cos(GarmaM).*cos(ThetaM).*PothiD + sin(GarmaM).*ThetaD];
%***************************************地理系相对惯性系的角速度以机体系为参考
% Wibb = Winb + Wnbb = Ctb*Wint + Wnbb = Ctn*Cnb*Wint + Wnbb
Winb = [T11M.*Wint(1,:) + T21M.*Wint(2,:) + T31M.*Wint(3,:);
        T12M.*Wint(1,:) + T22M.*Wint(2,:) + T32M.*Wint(3,:);
        T13M.*Wint(1,:) + T23M.*Wint(2,:) + T33M.*Wint(3,:)];
Wibb = Winb + Wnbb;    
%*****************************************************************基本惯导方程
Ft = AtM + cross( (2*Wiet+Went), VtM ) - G*ones(1,ArryLth);
Fb = [T11M.*Ft(1,:)+T21M.*Ft(2,:)+T31M.*Ft(3,:);
      T12M.*Ft(1,:)+T22M.*Ft(2,:)+T32M.*Ft(3,:);
      T13M.*Ft(1,:)+T23M.*Ft(2,:)+T33M.*Ft(3,:)];
%****************************************************************************