updata_particles.asv

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

function updata_particles
%---------------------------------------------------------------------
load p
load num
load Robottest
load lastx
load lasty
load lastTh
%----------------------------------------------------------------------
 for k=1:1:num	
      p(k,1)=p(k,1)+(Robottest(1)-lastx)+random('normal',0,0.1);
	  p(k,2)=p(k,2)+(Robottest(2)-lasty)+random('normal',0,0.1);
	  p(k,3)=p(k,3)+(Robottest(3)-lastTh)+10*random('normal',0,0.1);
%----------------------------------------------------------------------
%---------------------------------------------------------------------- 
      if(p(k,1)>=4.6||p(k,1)<=0)
           p(k,1)=4.6*random('normal',0.5,0.1);
       end
       if(p(k,2)>=7.3||p(k,2)<=0)
           p(k,2)=7.3*random('normal',0.5,0.1);
       end  
%----------------------------------------------------------------------
%----------------------------------------------------------------------   
     Init_oneParticle(k);      
 end
    updata_particleValue;  
    test;
    lastx=Robottest(1);
	lasty=Robottest(2);
	lastTh=Robottest(3); 
%----------------------------------------------------------------------
    save('p','p');
    save('Robottest','Robottest');