forsee_robotposition.m

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

function forsee_RobotPosition
%forsee the position of Robot based on all particles,this is ok.
%2007.09.29
%------------------------------------------------------------------------
  global num
  global Robot 
  global 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;
%------------------------------------------------------------------------