az_dh_dx_2a.m

来自「基于RBMCDA (Rao-Blackwellized Monte Carlo 」· M 代码 · 共 12 行

%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

% Copyright (C) 2003 Simo S鋜kk