www.gusucode.com > MATLAB编程毕业设计 EKF SLAM仿真全部源代码 > pi_to_pi.m

    function angle = pi_to_pi(angle)
%将角度整合到黑的范围[-pi~pi]

angle = mod(angle, 2*pi);

i=find(angle>pi);
angle(i)=angle(i)-2*pi;

i=find(angle<-pi);
angle(i)=angle(i)+2*pi;