init_particles.m

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

function Init_particles
%--------------------------------------------------------------
%inital particles.this is ok
global num
global  p
global  Robottest
%--------------------------------------------------------------
for k=1:1:num
% 2007.09.29
%--------------------------------------------------------------
    p(k,1)=7*rand(1);
    p(k,2)=4.2*rand(1);
    p(k,3)=360*rand(1);
%     p(k,1)=random('normal',Robottest(1),3);
%     p(k,2)=random('normal',Robottest(2),3);
%     p(k,3)=random('normal',Robottest(3),3);
     if(p(k,1)>7||p(k,1)<0)
%        p(k,1)=random('normal',Robottest(1),0.1);
%              p(k,4)=0;
            p(k,1)=7*rand(1);
       end
       if(p(k,2)>4||p(k,2)<0)
%           p(k,2)=random('normal',Robottest(2),0.1);
%              p(k,4)=0;
            p(k,2)=4*rand(1);
       end  
    p(k,4)=1./num;    
    Init_oneParticle(k);
end 
%---------------------------------------------------------------