orientat.asv

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

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