pi_to_pi.m

来自「SLAM实时定位与地图创建,采用扩展卡尔曼滤波器,实现机器人定位与地图绘制」· M 代码 · 共 14 行

function angle = pi_to_pi(angle)

%function angle = pi_to_pi(angle)
% Input: array of angles.
% Tim Bailey 2000

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;