plot_robot_particle.m

来自「粒子滤波算法在机器人定位中的应用」· M 代码 · 共 41 行

function plot_Robot_Particle
%-------------------------------------------------------------------------
 clf
 load Robottest
 load num
 load p
%--------------------------------------------------------------------------
plot(0,0:0.01:4.7,'r');
hold on
plot(0:0.01:7.4,0,'r');
hold on
plot(7.4,0:0.01:4.7,'g');
hold on
plot(0:0.01:7.4,4.7,'r');
hold on
plot(0:0.01:0.7,3.4,'r');
hold on
plot(0.7,3.4:0.01:4.7,'r');
hold on
plot(3.3,4.1:0.01:4.7,'r');
hold on
plot(3.3:0.01:6.5,4.1,'r');
hold on
plot(6.5,3.2:0.01:4.1,'r');
hold on
plot(6.5:0.01:7.4,3.2,'r');
hold on
plot(6.7:0.01:7.4,1.5,'r');
hold on
plot(6.7,0:0.01:1.5,'r');
hold on
%--------------------------------------------------------------------------
plot(Robottest(2), Robottest(1),'o', 'MarkerEdgeColor','g','MarkerFaceColor',[0 1 0], 'MarkerSize',20);
 hold on
 for k=1:1:num
  plot(p(k,2),p(k,1),'o','MarkerEdgeColor','r','MarkerFaceColor',[0 0 1], 'MarkerSize',2);
  hold on
 end  
 pause(1);
%-------------------------------------------------------------------------
%axis([-1,8,-1,5.5])