plot_robot_particle.m

基于声纳信息的粒子滤波算法

function plot_Robot_Particle
%-------------------------------------------------------------------------
 %clf
 global Robot 
 global Robottest
 global num
 global p
 global plotnum
 global stepnum
 global errornum;
%-------------------------------------------------------------------------
axis([0,7.4,0,4.7])
plot(0,0:0.01:4.7,'k');
hold on
plot(0:0.01:7.4,0,'k');
hold on
plot(7.4,0:0.01:4.7,'k');
hold on
plot(0:0.01:7.4,4.7,'k');
hold on
plot(0:0.01:0.7,3.4,'k');
hold on
plot(0.7,3.4:0.01:4.7,'k');
hold on
plot(3.3,4.1:0.01:4.7,'k');
hold on
plot(3.3:0.01:6.5,4.1,'k');
hold on
plot(6.5,3.2:0.01:4.1,'k');
hold on
plot(6.5:0.01:7.4,3.2,'k');
hold on
plot(6.7:0.01:7.4,1.5,'k');
hold on
plot(6.7,0:0.01:1.5,'k');
hold on
%--------------------------------------------------------------------------
if(plotnum<10)
    figure(1);
    subplot(3,3,plotnum);
    plotnum=plotnum+1;
else 
    if(plotnum>9&&plotnum<19)
    figure(2);
    tmp=plotnum-9;
    subplot(3,3,tmp);
    plotnum=plotnum+1;
    else
    figure(3);
    tmp=plotnum-18;
    subplot(3,3,tmp);
    plotnum=plotnum+1;
    end
end 
plot(Robottest(1), Robottest(2),'o', 'MarkerEdgeColor','g','MarkerFaceColor',[0 1 0], 'MarkerSize',10);
 hold on 
 for k=1:1:num
  plot(p(k,1),p(k,2),'o','MarkerEdgeColor','r','MarkerFaceColor',[0 0 1], 'MarkerSize',2);
  hold on
 end  
  plot(Robot(1), Robot(2),'*k');
 hold on 
% if(plotnum==2)
% legend('Robot','Particle','Robot*');
 %end
 xlabel('x(m)');
 ylabel('y(m)');
if(plotnum==27)
      figure(4)
     for step=1:1:stepnum
     plot(step,errornum(step),'*k');
     hold on  
     end
     xlabel('机器人运动步数');
     ylabel('误差(m)');
 end 
 
 
% plot(p(1,1),p(1,2),'o','MarkerEdgeColor','r','MarkerFaceColor',[0 0 1], 'MarkerSize',2);
% hold on
% plot(p(2,1),p(2,2),'o','MarkerEdgeColor','g','MarkerFaceColor',[0 1 0], 'MarkerSize',2);
%   hold on
% plot(p(3,1),p(3,2),'o','MarkerEdgeColor','b','MarkerFaceColor',[1 0 0], 'MarkerSize',2);
%   hold on
% pause();
%-------------------------------------------------------------------------
%axis([-1,8,-1,5.5])