plotsys.m

高速车辆组合导航研究

function  plotsys
% plot the result

[plotP,truer,truev,r,v,T,tf] = EKF();
detr = abs(truer-r);
detv = abs(truev-v);
Pr = 2*(sqrt(plotP(1,:))+sqrt(plotP(2,:))+sqrt(plotP(3,:)))/3;
Pv = 2*(sqrt(plotP(4,:))+sqrt(plotP(5,:))+sqrt(plotP(6,:)))/3;

close all;
t=0:T:tf;
figure;
plot(t,detr,'r-',t,Pr,'b--');%
xlabel('时间/s');
ylabel('位置/m');
legend('估计位置与实际位置之差','2倍位置估计误差方差');

figure;
plot(t,detv,'r-',t,Pv,'b--');%
xlabel('时间/s');
ylabel('速度/m');
legend('估计速度与实际速度之差','2倍速度估计误差方差');