az_dh_dx_2a

AZ_DH_DX Measurement derivative function for EKF.with 2d attributes

  \V{x} = [x y vx vy a1 a2]^T

  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)
  da_da1 = 1
  da_da2 = 1

0001 %AZ_DH_DX Measurement derivative function for EKF.with 2d attributes
0002 %
0003 %  \V{x} = [x y vx vy a1 a2]^T
0004 %
0005 %  dh_dx  = -(y-sy) / (x-sx)^2 * 1 / (1 + (y-sy)^2 / (x-sx)^2)
0006 %         = -(y-sy) / ((x-sx)^2 + (y-sy)^2)
0007 %  dh_dy  = 1 / (x-sx) * 1 / (1 + (y-sy)^2 / (x-sx)^2)
0008 %         = (x-sx) / ((x-sx)^2 + (y-sy)^2)
0009 %  da_da1 = 1
0010 %  da_da2 = 1
0011 
0012 % Copyright (C) 2003 Simo Särkkä
0013 %
0014 % $Id: az_dh_dx_2a.m,v 1.1 2003/12/10 18:30:12 ssarkka Exp $
0015 %
0016 % This software is distributed under the GNU General Public
0017 % Licence (version 2 or later); please refer to the file
0018 % Licence.txt, included with the software, for details.
0019 
0020 function dY = az_dh_dx_2a(x,s)
0021   dY = [-(x(2)-s(2)) / ((x(1)-s(1))^2 + (x(2)-s(2))^2) ...
0022        (x(1)-s(1)) / ((x(1)-s(1))^2 + (x(2)-s(2))^2) 0 0 0 0;...
0023         0 0 0 0 1 0;
0024         0 0 0 0 0 1];
0025