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:
• bot_demo_all % Bearings Only Tracking (BOT) demonstration with various filters
• ckfs_bot_demo % Bearings Only Tracking (BOT) demonstration with CKF
• ekfs_bot_demo Bearings Only Tracking (BOT) demonstration with EKF
• ghkfs_bot_demo % Bearings Only Tracking (BOT) demonstration with GHKF
• ukfs_bot_demo Bearings Only Tracking (BOT) demonstration with UKF.

## 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�%
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