orientat_isend.asv

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

function pat=Orientat_IsEnd

 load Robot
 load Robottest
 load error1 
%------------------------------------------------------------------ 
	tmp=0;
     for i=1:1:8	
	      tmp=tmp+((Robottest(4+i)-Robot(i+4)).^2);          
      end   
      tmp=(sqrt(tmp));
    if(tmp>error1)
      pat=0;
    else  
	for i=1:1:3    
     Robot(i)
    end
 %------------------------------------------------------------------
     plot(Robottest(2),0.01:0.01:Robottest(1));
     hold on
     plot(0.01:0.01:Robottest(2),Robottest(1));
     axis([0,7.4,0,4.7]);
     pat=1;
    end