Home > src > demos > bot_demo > bot_h.m

bot_h

PURPOSE ^

Azimuth measurement function for EKF/UKF.

SYNOPSIS ^

function Y = bot_h(x,s)

DESCRIPTION ^

 Azimuth measurement function for EKF/UKF.

  h = atan((y-sy) / (x-sx))

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Fri 12-Aug-2011 15:15:16 by m2html © 2005