alligment.m

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

%*********************************************************************************
%*****************************  仿真条件设置  ************************************
%*********************************************************************************
%---------------------------------------------------------------------仿真时间设置
% L -- 仿真时间(s); ts -- 解算间隔30ms;  xs -- 每个解算周期采样次数
%---------------------------------------------------------------------------------
  clear;   clc;  L  =1000;            ts = 30.0e-2;        xs  = 3;
%--------------------------------------------------------------------运动参数设置
% pothiA -- 航向角指北(0—2pi);                      Frqw   -- 姿态角摇摆频率;
% thetaA -- 俯仰摆幅(-pi/2—pi/2);                  garmaA -- 横滚摆幅(-pi—pi);
% St0 -- 初始经纬度;       V0 -- 初始速度(m/s);     A -- 加速度(m/s/s)
%---------------------------------------------------------------------------------
  pothiA = pi/36;           thetaA = pi/36;        garmaA = pi/36;
  PsA  = [pothiA,thetaA,garmaA];                Frqw = [.01,  .01, .01];
  St0  = [pi/4 , pi/3];    
  V0  = 0;             A = 0.03;
%--------------------------------------------------------------------传感器参数设置
% Kw  -- 陀螺仪刻度因子误差    Wbas -- 陀螺零偏 (度/小时)   Wrnd-- 陀螺随机误差
% Ka  -- 加速度计刻度因子误差  Abas -- 加速度计零偏 (ug)    Arnd-- 加速度计随机误差
% 测量误差参数 Pc = [ LamdaE,  PheE,  VeE,  VnE,  VuE]
%----------------------------------------------------------------------------------
  Kw  = [1,1,1]';              Wbas  = [-.10,-.10,-.10]';         Wrnd  = 0.5;
  Ka  = [1,1,1]';              Abas  = [100,100,0]';        Arnd  = 50;
  % Pc = [15^2, 15^2, 0.2^2, 0.2^2, 0.2^2];  
  Pc = [0,0,0,0,0];
  
%-------------------------------------------------------------------------轨迹发生器
  [Wibb,Fb,Ps,Vt,St] = TrcGnrt(A,V0,St0,PsA,Frqw,L,ts);
  
%---------------------------------------------------------------------传感器误差模型
  [WibbM,FbM] = IMUMdl(Wibb,Kw,Wbas,Wrnd,Fb,Ka,Abas,Arnd);
  [StM,VtM]   = GPSMdl(St,Vt,Pc);
  %clear ('Wibb','Fb');
  
%------------------------------------------------------------------------------粗对准
% Vt -- 初始速度;    St0 -- 初始位置;    Ps -- 初始姿态    DeA0 -- 初始失准角
%-----------------------------------------------------------------------------------
  Vt0 = Vt(:,1);     St0 = St(:,1);      DeA0 = [1; 1; -0.5]*pi/180;
  Cbn0 = GetCbn( Ps(:,1) ); 
  Cbn0 = RotateT( Cbn0, DeA0);
  Ps0 = GetPs(Cbn0);
%--------------------------------------------------------------------------动态精对准
  [StJ,VtJ,PsJ] = DynmAllg( WibbM,FbM,St0,Ps0,Vt0,L,ts,VtM,StM );
  figure;
plot(St(1,:),St(2,:),'g','lineWidth',5);hold on;
plot(StJ(1,:),StJ(2,:),'ro');ylabel('运动轨迹(rad)');hold off;
%plot(Vtx,Vty,'ro');ylabel('运动轨迹(rad)');hold off;
%------------------------------------------------------------------------------失准角
% [DeA] = CncltDa(Ps,PsJ) -- 解算失准角;      DeT -- 经纬度误差引起的失准角;
% Xk  -- 对准过程中估计出来的失准角;          将各变量转化成角分;
%-----------------------------------------------------------------------------------
  [DeA] = CncltDa(Ps,PsJ);   %结算失准角                 %[DeT] = GetDeT(St,StJ);
  DeA = (DeA)*60*180/pi;                      
  %-------------------------------------------------------------------------------绘图
  %Plt(Ps,DeA,St,Vt,ts); Plt(Ps,PsJ,Vt,VtJ,ts)
  PsJ=DeA;
  VtJ=VtJ;
  Vt=StJ;
  %-----------------------------------------------------------------------------------
  %*****************************************************
% PltLth -- %绘图时矩阵长度    x -- 参考时间轴   L -- 圆整后时间   Plt(Ps,DeA,St,Vt,ts);
[m,PltLth] = size(PsJ);      x = ones(1,PltLth);    L = ts*(PltLth-1);
%*************************
PothiR =  Ps(1,:).*x;   ThetaR  =  Ps(2,:).*x;   GarmaR  =  Ps(3,:).*x; 
PothiJ = PsJ(1,:).*x;   ThetaJ  = PsJ(2,:).*x;   GarmaJ  = PsJ(3,:).*x;

Vtx  =  Vt(1,:).*x;    Vty  =  Vt(2,:).*x;    Vtz  =  Vt(2,:).*x;
VtJx = VtJ(1,:).*x;    VtJy = VtJ(2,:).*x;    VtJz = VtJ(3,:).*x;
%************************
x     = 0 : ts : L;
%****************************************************************绘姿态图
figure;
subplot(4,3,1); plot(x,PothiR,'r-'); ylabel('姿态角(rad)');
subplot(4,3,2); plot(x,ThetaR,'k-'); 
subplot(4,3,3); plot(x,GarmaR,'b-');
subplot(4,3,4); plot(x,PothiJ,'r-'); ylabel('失准角(arcmin)');
subplot(4,3,5); plot(x,ThetaJ,'k-'); 
subplot(4,3,6); plot(x,GarmaJ,'b-');
%*****************************************************************绘速度图
subplot(4,3,7); plot(x,Vtx,'r-');    ylabel('经纬度(rad)');
subplot(4,3,8); plot(x,Vty,'k-');  
subplot(4,3,9); plot(x,Vtz,'b-');
subplot(4,3,10);plot(x,VtJx,'r-');   ylabel('速度(m/s)');
subplot(4,3,11);plot(x,VtJy,'k-');  
subplot(4,3,12);plot(x,VtJz,'b-');
%**************************************************************************