az_h_2a

AZ_H Azimuth measurement function for EKF.with 2d attributes

  \V{x} = [x y vx vy a1 a2]^T

  h = atan((y-sy) / (x-sx))
  a = [a1 a2]^T
  Y = [h;a]

0001 %AZ_H Azimuth measurement function for EKF.with 2d attributes
0002 %
0003 %  \V{x} = [x y vx vy a1 a2]^T
0004 %
0005 %  h = atan((y-sy) / (x-sx))
0006 %  a = [a1 a2]^T
0007 %  Y = [h;a]
0008 %
0009 
0010 % Copyright (C) 2003 Simo Särkkä
0011 %
0012 % $Id: az_h_2a.m,v 1.1 2003/12/10 18:30:12 ssarkka Exp $
0013 %
0014 % This software is distributed under the GNU General Public
0015 % Licence (version 2 or later); please refer to the file
0016 % Licence.txt, included with the software, for details.
0017 
0018 function Y = az_h_2a(x,s)
0019   Y = zeros(size(s,2),size(x,2));
0020 
0021   h = atan2(x(2)-s(2),x(1)-s(1));
0022   np = find(h>0.5*pi);
0023   nn = find(h<-0.5*pi);
0024   if length(nn)>length(np)
0025     h(np) = h(np)-2*pi;
0026   else
0027     h(nn) = h(nn)+2*pi;
0028   end
0029   a = [x(end-1);x(end)];
0030   Y = [h;a];
0031