forsee_robotposition.m

粒子滤波算法在机器人定位中的应用

function forsee_RobotPosition
%forsee the position of Robot based on all particles,this is ok.
%2007.09.29
%------------------------------------------------------------------------
   load num
   load Robot 
   load p
%-------------------------------------------------------------------------
   Robot(1)=0;
   Robot(2)=0;
   Robot(3)=0;
  for i=1:1:num  
	  Robot(1)=Robot(1)+p(i,4)*p(i,1);
      Robot(2)=Robot(2)+p(i,4)*p(i,2);
	  Robot(3)=Robot(3)+p(i,4)*p(i,3);
  end
%------------------------------------------------------------------------
Init_RobotSonar;
%------------------------------------------------------------------------
save('Robot','Robot');
save('p','p');