orientat.m

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

function Orientat
%-------------------------------------------------------------------------
%Orientat our robot this is ok.
%2007.09.29
%--------------------------------------------------------------------------
    global Robottest    
    global  num
    global p    
%---------------------------------------------------------------------------
    Init_particles;  
    Init_RobottestSonar;
    
    updata_particleValue;   
    forsee_RobotPosition;     
    plot_Robot_Particle;    
 while Orientat_IsEnd==0        
      lastx=Robottest(1);   
      lasty=Robottest(2);     
      lastTh=Robottest(3);
      %-------------------------------------------------------------------
      Robottest(1)=Robottest(1)+randn(1)/4;
      Robottest(2)=Robottest(2)+randn(1)/4;
      Robottest(3)=Robottest(3)+20*randn(1);  
     %--------------------------------------------------------------------
     %--------------------------------------------------------------------
      if Robottest(1)>5          
          Robottest(1)=Robottest(1)+random('normal',-0.5,0.1);
      end
      if Robottest(1)<3         
          Robottest(1)=Robottest(1)+random('normal',0.5,0.1);
      end
      if Robottest(2)> 3         
          Robottest(2)=Robottest(2)+random('normal',-0.5,0.1);          
      end      
       if Robottest(2)<1.5        
          Robottest(2)=Robottest(2)+random('normal',0.5,0.1);          
      end
     %--------------------------------------------------------------------
     %--------------------------------------------------------------------
      Init_RobottestSonar;          
      updata_particles;  
      test;
%       forsee_RobotPosition; 
%       plot_Robot_Particle;
      
      ReSample_paricles;
      forsee_RobotPosition; 
    %  plot_Robot_Particle;
%       updata_particles;  
      ReSample_paricles;
      forsee_RobotPosition; 
      plot_Robot_Particle;
end
	fprintf(1,'Orientat_end\n');
 %-------------------------------------------------------------------------