AZ_H Azimuth measurement function for EKF. h = atan((y-sy) / (x-sx))
0001 %AZ_H Azimuth measurement function for EKF. 0002 % 0003 % h = atan((y-sy) / (x-sx)) 0004 % 0005 0006 % Copyright (C) 2003 Simo Särkkä 0007 % 0008 % $Id: az_h.m,v 1.1.1.1 2003/09/15 10:54:33 ssarkka Exp $ 0009 % 0010 % This software is distributed under the GNU General Public 0011 % Licence (version 2 or later); please refer to the file 0012 % Licence.txt, included with the software, for details. 0013 0014 function Y = az_h(x,s) 0015 Y = zeros(size(s,2),size(x,2)); 0016 0017 for i=1:size(s,2) 0018 h = atan2(x(2,:)-s(2,i),x(1,:)-s(1,i)); 0019 np = find(h>0.5*pi); 0020 nn = find(h<-0.5*pi); 0021 if length(nn)>length(np) 0022 h(np) = h(np)-2*pi; 0023 else 0024 h(nn) = h(nn)+2*pi; 0025 end 0026 Y(i,:) = h; 0027 end 0028