test.asv

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

function test
 load  p
 load num
 load Robottest
 load Robot
tmp(1,1:12)=0;
for i=1:1:num-1
    tmp(1,1:12)=p(i,1:12);
    k=i;
    tt=p(i,4);
      for j=i+1:1:num        
          if(p(j,4)>=tt)
            tmp(1,1:12)=p(j,1:12);
            tt=p(j,4);
            k=j;
         end
     end
     p(k,1:12)=p(i,1:12);
     p(i,1:12)=tmp(1,1:12);   
end
 fprintf(1,'Real____Robot____Real\n');
 Robottest(1:12)
 fprintf(1,'best____Particle____best\n');
 p(1,1:12)
 fprintf(1,'forsee____Robot____forsee\n')