az_dh_dx

AZ_DH_DX Measurement derivative function for EKF.

  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)

0001 %AZ_DH_DX Measurement derivative function for EKF.
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ärkkä
0010 %
0011 % $Id: az_dh_dx.m,v 1.1 2003/12/10 16:40:09 ssarkka Exp $
0012 %
0013 % This software is distributed under the GNU General Public
0014 % Licence (version 2 or later); please refer to the file
0015 % Licence.txt, included with the software, for details.
0016 
0017 function dY = az_dh_dx(x,s)
0018   dY = zeros(size(s,2),size(x,1));
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