www.pudn.com > ekfcode.zip > plots.m

replot 
 
figure(2) 
clf 
hold on; 
v=[0 WORLD_SIZE 0 WORLD_SIZE]; 
axis(v); 
%plot(beacons(:,1),beacons(:,2),'go') 
[obs_p, state_p]=p_obs(obs,xtrue); 
plot(xtrue(1,:),xtrue(2,:),'b',obs_p(1,:),obs_p(2,:),'rx') 
legend('True Vehicle Path','Beacon Observations') 
title('True Vehicle Path and Beacon Observations') 
xlabel('X distance (m)') 
ylabel('Y distance (m)') 
hold off 
 
 
figure(3) 
clf 
hold on 
plot(xtrue(4,:),abs(xtrue(1,:)-xest(1,:)),'r',xtrue(4,:),sqrt(squeeze(Pest(1,1,:))),'b') 
legend('True Error','Estimated Error') 
xlabel('Time (s)') 
ylabel('Error (m)') 
title('X Direction Error') 
hold off 
 
figure(4) 
clf 
hold on 
plot(xtrue(4,:),abs(xtrue(2,:)-xest(2,:)),'r',xtrue(4,:),sqrt(squeeze(Pest(2,2,:))),'b') 
legend('True Error','Estimated Error') 
xlabel('Time (s)') 
ylabel('Error (m)') 
title('Y Direction Error') 
hold off 
 
figure(5) 
clf 
hold on 
plot(xtrue(4,:),abs(xtrue(3,:)-xest(3,:)),'r',xtrue(4,:),sqrt(squeeze(Pest(3,3,:))),'b') 
legend('True Error','Estimated Error') 
xlabel('Time (s)') 
ylabel('Error (rads)') 
title('Orientation Error') 
hold off 
 
figure(6) 
clf 
hold on 
plot(xtrue(4,:),xest(4,:),'r',xtrue(4,:),WHEEL_RADIUS+sqrt(squeeze(Pest(4,4,:))),'b',xtrue(4,:),WHEEL_RADIUS-sqrt(squeeze(Pest(4,4,:))),'b') 
legend('Estimated Wheel Radius','Estimated Wheel Radius Error') 
title('Estimated Wheel Radius and Wheel Radius Error') 
xlabel('Time (s)') 
ylabel('Wheel Radius (m)'); 
hold off 
 
%[inn,sinn]=proc_innov(innov,innvar,obs); 
[inn,sinn]=track_innov(innov,innvar,obs,xest,utrue); 
 
figure(7) 
clf 
hold on 
plot(inn(3,:),inn(1,:),'bx') 
plot(inn(3,:),inn(1,:),'b') 
plot(inn(3,:),sinn(1,:),'g') 
plot(inn(3,:),-sinn(1,:),'g') 
title('Long-Track Innovation and Innovation Standard Deviation') 
xlabel('Time (s)') 
ylabel('Innovation (m)'); 
hold off 
 
figure(8) 
clf 
hold on 
plot(inn(3,:),inn(2,:),'bx') 
plot(inn(3,:),inn(2,:),'b') 
plot(inn(3,:),sinn(2,:),'g') 
plot(inn(3,:),-sinn(2,:),'g') 
title('Cross-Track Innovation and Innovation Standard Deviation') 
xlabel('Time (s)') 
ylabel('Innovation (m)'); 
hold off 
 
[rows,cols]=size(xest); 
 
rho=zeros(6,cols); 
 
for i=1:cols 
   %x-y 
   rho(1,i)=Pest(1,2,i)/sqrt(Pest(1,1,i)*Pest(2,2,i)); 
   %x-phi 
   rho(2,i)=Pest(1,3,i)/sqrt(Pest(1,1,i)*Pest(3,3,i)); 
   %y-phi 
   rho(3,i)=Pest(2,3,i)/sqrt(Pest(2,2,i)*Pest(3,3,i)); 
   %x-R 
   rho(4,i)=Pest(1,4,i)/sqrt(Pest(1,1,i)*Pest(4,4,i)); 
   %y-R 
   rho(5,i)=Pest(2,4,i)/sqrt(Pest(2,2,i)*Pest(4,4,i)); 
   %Phi-R 
   rho(6,i)=Pest(3,4,i)/sqrt(Pest(3,3,i)*Pest(4,4,i)); 
 
end 
 
 
figure(9) 
clf 
hold on 
plot(xtrue(4,:),rho(1,:)) 
xlabel('Time (s)') 
ylabel('Correlation Coefficient') 
title('X-Y Correlation Coefficient') 
hold off 
 
figure(10) 
clf 
hold on 
plot(xtrue(4,:),rho(2,:),'b',xtrue(4,:),rho(3,:),'r') 
legend('X to Orientation Correlation','Y to Orientation Correlation') 
xlabel('Time (s)') 
ylabel('Correlation Coefficient') 
title('Position to Orientation Correlation Coefficient') 
hold off 
 
figure(11) 
clf 
hold on 
plot(xtrue(4,:),rho(4,:),'b',xtrue(4,:),rho(5,:),'r') 
legend('X to Wheel Radius Correlation','Y to Wheel Radius Correlation') 
xlabel('Time (s)') 
ylabel('Correlation Coefficient') 
title('Position to Wheel Radius Correlation Coefficient') 
hold off 
 
figure(12) 
clf 
hold on 
plot(xtrue(4,:),rho(6,:),'b') 
xlabel('Time (s)') 
ylabel('Correlation Coefficient') 
title('Orientation to Wheel Radius Correlation Coefficient') 
hold off