orientat_isend.asv

来自「粒子滤波算法在机器人定位中的应用」· ASV 代码 · 共 29 行

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