Home > src > demos > bot_demo > bot_d2h_dx2.m

# bot_d2h_dx2

## PURPOSE Hessian of the measurement function in BOT-demo.

## SYNOPSIS function dY = bot_d2h_dx2(x,s)

## DESCRIPTION ` Hessian of the measurement function in BOT-demo.`

## CROSS-REFERENCE INFORMATION This function calls:
This function is called by:
• bot_demo_all % Bearings Only Tracking (BOT) demonstration with various filters
• ekfs_bot_demo Bearings Only Tracking (BOT) demonstration with EKF

## SOURCE CODE ```0001 % Hessian of the measurement function in BOT-demo.
0002
0003 % Copyright (C) 2007 Jouni Hartikainen
0004 %
0005 % This software is distributed under the GNU General Public
0006 % Licence (version 2 or later); please refer to the file
0007 % Licence.txt, included with the software, for details.
0008
0009 function dY = bot_d2h_dx2(x,s)
0010   % Space for Hessians. Note that we need a Hessian for
0011   % each dimension in the measurement space, that is we need
0012   % a Hessian for each sensor in this case.
0013   dY = zeros(size(s,2),size(x,1),size(x,1));
0014
0015   % Loop through sensors.
0016   for i=1:size(s,2)
0017     % Derivative twice wrt. x
0018     dx2 = -2*(x(1)-s(1,i)) / ((x(1)-s(1,i))^2+(x(2)-s(2,i))^2)^2;
0019     % Derivative twice wrt. y
0020     dy2 = -2*(x(2)-s(2,i)) / ((x(1)-s(1,i))^2+(x(2)-s(2,i))^2)^2;
0021     % Derivative wrt. x and y
0022     dxdy = ((x(2)-s(2,i))^2-(x(1)-s(1,i))^2) / ((x(1)-s(1,i))^2+(x(2)-s(2,i))^2)^2;
0023     dh = [dx2  dxdy 0 0;...
0024       dxdy dy2  0 0;...
0025       0    0    0 0;...
0026           0    0    0 0];
0027     dY(i,:,:) = dh;
0028   end```

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