updata_particlevalue.m

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

function updata_particleValue
%------------------------------------------------------------
%update particle's value based on the status of particles in time.this is
%ok,2007.09.29
%------------------------------------------------------------
global num
global p
global Robottest
%------------------------------------------------------------
  for i=1:1:num
  	  tmp=0;      
      for j=1:1:8 
      tmp=tmp+((p(i,4+j)-Robottest(j+4)).^2);
      end  
      %tmp=tmp*tmp;
	  p(i,4)=1./(tmp+1);      
  end   
%------------------------------------------------------------
   tmp=0;   
%------------------------------------------------------------  
   for i=1:1:num  
	   tmp=tmp+p(i,4);      
   end 
%------------------------------------------------------------  
   for i=1:1:num
     p(i,4)=p(i,4)/tmp;     
   end  
   
   
   %tmp=tmp
  tmp=sum(p(1:num,4))
%-------------------------------------------------------------