updata_particles.m

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

function updata_particles
%---------------------------------------------------------------------
global p
global num
global Robottest
global lastx
global lasty
global lastTh
%--------------------------------------------------------------------
 for k=1:1:num	
      p(k,1)=p(k,1)+(Robottest(1)-lastx)+randn(1)/10;
	  p(k,2)=p(k,2)+(Robottest(2)-lasty)+randn(1)/10;
	  p(k,3)=p(k,3)+(Robottest(3)-lastTh)+10*randn(1)/10;     
      if(p(k,1)>7||p(k,1)<0)
%        p(k,1)=random('normal',Robottest(1),0.1);
%              p(k,4)=0;
            p(k,1)=7*rand(1);
       end
       if(p(k,2)>4||p(k,2)<0)
%           p(k,2)=random('normal',Robottest(2),0.1);
%              p(k,4)=0;
            p(k,2)=4*rand(1);
       end  
     Init_oneParticle(k);      
  end
%--------------------------------------------------------------------    
  updata_particleValue;
  
	lastx=Robottest(1);
	lasty=Robottest(2);
	lastTh=Robottest(3); 
%---------------------------------------------------------------------