normalize.m

来自「用EKF」· M 代码 · 共 8 行

function angles = normalize(angles)
% Normalizes angles to interval (-pi,pi]

%angles = pi - mod(pi-angles, 2*pi);
% Same result, more efficiently:

angles = angles + (2*pi)*floor((pi-angles)/(2*pi));