anglewrap.m

Mobile Robots 非线性跟踪

function a = AngleWrap(a)

if(a>pi)
    a=a-2*pi;
elseif(a<-pi)
    a = a+2*pi;
end;