init_particles.asv

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

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