test.m

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

function test
global  p
global Robottest
global num
 p(1:num,4);
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,'**************************************************\n');
fprintf(1,'p____best_______best\n');
p(1,1:12)
%p(2,1:12)
fprintf(1,'Real________Robot________Real\n');
Robottest(1:12)
fprintf(1,'*****************************************************\n');
pause(0.1);