Home > src > demos > bot_demo > bot_dh_dx.m

bot_dh_dx

PURPOSE ^

Jacobian of the measurement model function in BOT demo.

SYNOPSIS ^

function dY = bot_dh_dx(x,s)

DESCRIPTION ^

 Jacobian of the measurement model function in BOT demo.

  dh_dx = -(y-sy) / (x-sx)^2 * 1 / (1 + (y-sy)^2 / (x-sx)^2)
        = -(y-sy) / ((x-sx)^2 + (y-sy)^2)
  dh_dy = 1 / (x-sx) * 1 / (1 + (y-sy)^2 / (x-sx)^2)
        = (x-sx) / ((x-sx)^2 + (y-sy)^2)

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 % Jacobian of the measurement model function in BOT demo.
0002 %
0003 %  dh_dx = -(y-sy) / (x-sx)^2 * 1 / (1 + (y-sy)^2 / (x-sx)^2)
0004 %        = -(y-sy) / ((x-sx)^2 + (y-sy)^2)
0005 %  dh_dy = 1 / (x-sx) * 1 / (1 + (y-sy)^2 / (x-sx)^2)
0006 %        = (x-sx) / ((x-sx)^2 + (y-sy)^2)
0007 %
0008 
0009 % Copyright (C) 2003 Simo S�kk�%
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 dY = bot_dh_dx(x,s)
0015   % Reserve space for the Jacobian.
0016   dY = zeros(size(s,2),size(x,1));
0017   
0018   % Loop through sensors
0019   for i=1:size(s,2)
0020     dh = [-(x(2)-s(2,i)) / ((x(1)-s(1,i))^2 + (x(2)-s(2,i))^2);...
0021        (x(1)-s(1,i)) / ((x(1)-s(1,i))^2 + (x(2)-s(2,i))^2);...
0022       zeros(size(x,1)-2,1)]';
0023     dY(i,:) = dh; 
0024   end
0025

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