plt.m

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

%**************************************************************************
%***************************  绘图   **************************************
%**************************************************************************
%*****************************************************
function Plt(Ps,PsJ,Vt,VtJ,ts)
%*****************************************************
% 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-');
%**************************************************************************
figure;
plot(Vtx,Vty);ylabel('运动轨迹(rad)');