diffangleunwrap.m

来自「it is a matlab file foe develop SLAM loc」· M 代码 · 共 33 行

%DIFFANGLEUNWRAP Take difference of two angles and unwrap it.
%   A = DIFFANGLEUNWRAP(A1,A2) determines the minimal difference
%   A = A1-A2 between two angles A1 and A2. If either A1 or A2
%   is Inf, Inf is returned.
%
%   See also SETANGLETORANGE.

% v.1.0, Dec. 2003, Kai Arras, CAS-KTH

function delta = diffangleunwrap(a1,a2);

if (a1 < Inf) & (a2 < Inf),
  
  % Normalize angles a1 and a2
  a1 = setangletorange(a1,0);  
  a2 = setangletorange(a2,0);
  
  % Take difference and unwrap
  delta = a1 - a2;
  if a1 > a2,
    while delta >  pi,
      delta = delta - 2*pi;
    end;
  elseif a2 > a1,
    while delta < -pi,
      delta = delta + 2*pi;
    end;
  end;
  
else
  delta = Inf;
end;