plotsys.asv

高速车辆组合导航研究

function  plotsys
% plot the result

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

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

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