visualize.m

扩展卡尔曼滤波算法是滤波领域较为重要的方法之一。本滤波算法是典型的卡尔曼滤波应用问题。

function visualize(init_flag,pose,xywdistance,debug_string)

global hold_rate iter lastp1 visu_time text_h1 text_h2 diameter_robot width
distance = width/2;

if ( init_flag ==0)
   xypose = [pose(1) pose(2) diameter_robot diameter_robot];
   close all;
   iter =1;
   figure(1);
   clf;
   axis([-10 10 -10 10]);
   hold on;
   
   %start of robot
   %yx-position
   xypose = xypose-[0.5 0.5 0 0];
   lastp1 = rectangle('Position',xypose,'Curvature',[1,1],'FaceColor','g');
     
   %inner walls
   rectangle('Position',[-6,-6,12,12],'FaceColor','r'); 
   
   %outer walls
   line([-10 10],[-10 -10],'Color','r','Linewidth',5);
   line([-10 10],[10 10],'Color','r','Linewidth',5);
   line([-10 -10],[-10 10],'Color','r','Linewidth',5);
   line([10 10],[-10 10],'Color','r','Linewidth',5);  

   %measurement information
   start_text = sprintf('Measurement \nInformation');
   text_h1= text(-5,3,start_text,'FontSize',16);
   start_text2 = sprintf('Control \nInformation');
   text_h2 = text(-5,-2,start_text2 ,'FontSize',16) 
   pause(visu_time);
elseif(init_flag ==1)
   xypose = [pose(1) pose(2) diameter_robot diameter_robot];
   if(mod(iter-1,hold_rate)~= 0)
   delete(lastp1);
   else
   set(lastp1,'FaceColor','b');   
   end
   xypose = xypose-[0.5 0.5 0 0];
   lastp1 = rectangle('Position',xypose,'Curvature',[1,1],'FaceColor','g');
   distance_text = sprintf('Pos x: %0.5g \nPos in y: %0.5g\nOrientation: %0.5g',xywdistance(1),xywdistance(2),xywdistance(3));
   set(text_h1,'String',distance_text);
   if(nargin ==4)
   set(text_h2,'String',debug_string);   
   end
   iter = iter+1;
   pause(visu_time);
elseif(init_flag ==2)
   xypose = [pose(1) pose(2) diameter_robot diameter_robot];
   plot(xypose(1),xypose(2),'k'); 
elseif(init_flag ==3)
   xypose = [pose(1) pose(2) diameter_robot diameter_robot];
   plot(xypose(1),xypose(2),'m'); 
end