updata_particles.m

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

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)+random('normal',0,1);%+(Robottest(1)-lastx);%+random('normal',0,0.1);
	  p(k,2)=p(k,2)+random('normal',0,1);%+(Robottest(2)-lasty);%+random('normal',0,0.1);
	  p(k,3)=p(k,3)+10*random('normal',0,5);%+(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;     
%     lastx=Robottest(1);
% 	lasty=Robottest(2);
% 	lastTh=Robottest(3); 
   % test 
%----------------------------------------------------------------------
    save('p','p');
    save('Robottest','Robottest');