orientat_isend.asv

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

function pat=Orientat_IsEnd
 global  Robot
 global Robottest
global error1 
global errornum
global stepnum
%------------------------------------------------------------------ 
	tmp=0;
     for i=1:1:8	
	      tmp=tmp+((Robottest(4+i)-Robot(i+4)).^2);          
      end   
      tmp=(sqrt(tmp));
      errornum(stepnum)=tmp;
    if(tmp>error1)
       pat=0;
    else 
      pat=1;
	   for i=1:1:3    
       Robot(i)
       end
      figure(4)
     for step=1:1:stepnum
     plot(step,errornum(step),'*k');
     hold on  
     end
      xlabel('机器人运动步数');
     ylabel('误差(m)');
    end
   stepnum=stepnum+1;
   
 %------------------------------------------------------------------