angle_ok.asv

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

function tt=angle_ok(x1,y1,x2,y2,Q)
   pi=3.1415926;
   if(Q<0)
       Q=Q+360*10;
   end
   angle=mod(Q,360);
     if((tan((angle./180)*3.14)>=0)&&(x2>=x1)&&(y2>=y1)&&(angle>0)&&(angle<=90))
         tt=1    
      else
          if(tan((angle./180)*3.14)<=0&&x2=<x1&&y2>=y1&&angle>90&&angle<=180)
              tt=1        
           else    
               if(tan((angle./180)*3.14)>=0&&x2=<x1&&y2<=y1&&angle>180&&angle<=270)
                 tt=1            
              else
                  if(tan((angle./180)*3.14)<=0&&x2>=x1&&y2<=y1&&angle>270&&angle<=360)
                     tt=1;
                 else
                     tt=0
                   end
               end
           end
       end