orientat.asv

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

function Orientat
%-------------------------------------------------------------------------
%Orientat our robot this is ok.
%2007.09.29
%--------------------------------------------------------------------------
    global Robottest    
    global  num
    global p    
%---------------------------------------------------------------------------

%local varial
step=0;
%local varial

    Init_particles;  
    Init_RobottestSonar;
    
    updata_particleValue;   
    forsee_RobotPosition;     
    plot_Robot_Particle;    
 while Orientat_IsEnd==0        
      lastx=Robottest(1);   
      lasty=Robottest(2);     
      lastTh=Robottest(3);
      %-------------------------------------------------------------------
      %********************************************************************
      if step<4
          Robottest(1)=Robottest(1)+0.25;
          Robottest(2)=Robottest(2);
          Robottest(3)=Robottest(3); 
          step=step+1;
      end
      if step==4
          Robottest(1)=Robottest(1);
          Robottest(2)=Robottest(2)+0.25;
          Robottest(3)=Robottest(3)+90; 
          step=step+1;
      end
       if step>4&&step<7
          Robottest(1)=Robottest(1);
          Robottest(2)=Robottest(2)+0.25;
          Robottest(3)=Robottest(3); 
          step=step+1;
       end
       if step==7
          Robottest(1)=Robottest(1)+0.25;
          Robottest(2)=Robottest(2);
          Robottest(3)=Robottest(3)+90; 
          step=step+1;
       end
       if step>7&&step<11
          Robottest(1)=Robottest(1)+0.5;
          Robottest(2)=Robottest(2);
          Robottest(3)=Robottest(3); 
          step=step+1;
       end
       if step==11
          Robottest(1)=Robottest(1);
          Robottest(2)=Robottest(2)+0.5;
          Robottest(3)=Robottest(3)+90; 
          step=step+1;
       end
        if step>11&&step<15
          Robottest(1)=Robottest(1);
          Robottest(2)=Robottest(2)+0.5;
          Robottest(3)=Robottest(3); 
          step=step+1;
        end
        if step==15
            break;
        end      
     %*********************************************************************     
     % 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');
 %-------------------------------------------------------------------------