updata_particlevalue.m

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

function updata_particleValue
%------------------------------------------------------------
%update particle's value based on the status of particles in time.this is
%ok,2007.09.29
%------------------------------------------------------------
load num
load p
load 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=(sqrt(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 
%-------------------------------------------------------------
  save('p','p');