gusucode.com > 用粒子滤波算法进行跟踪的matlab代码 > gmm_utilities/gmm_derivative_parameters.m
function J = gmm_derivative_parameters(g, s, type) %function J = gmm_derivative_parameters(g, s, type) % % INPUTS: % g - gmm % s - set of N samples from the domain of g % type - type of parameter for which to compute Jacobian. If type > 0, % then the Jacobian for type is computed analytically; if type < 0, % then the Jacobian for -type is computed using a numerical approximation. % % OUTPUT: % J - Jacobian of g with respect to selected gmm-component parameters. % Suppose there are M parameters, then J is (N x M). % % REFERENCE: % Williams, J.L., Gaussian Mixture Reduction for Tracking Multiple % Maneuvering Targets in Clutter, Masters thesis, Air Force Institute % of Technology, Wright-Patterson Air Force Base, 2003. % % NOTES: % Compute the Jacobian of g at each point s. One of three types of Jacobian may % be selected: % 1. dg/da, where w = a^2 is the weights in g. % 2. dg/dx, where x is the set of mean vectors in g. % 3. dg/dR, where P = L'*L is the set of covariance matrices in g, and R % is the upper-triangular parts of L. % % The parametrisation of g as a function of (a, x, R) permits optimisation % or adjustment of g while ensuring non-negative weights and positive- % definite covariances. % % Let N be the number of samples, K the number of Gaussian components, and % D the gmm domain dimension (ie, the sample dimension). % For type 1 (dg/da), J is (N x K). % For type 2 (dg/dx), J is (N x KD). % For type 3 (dg/dR), J is (N x K(D(D+1)/2)). % % Tim Bailey 2006. if type < 0 J = gmm_derivative_parameters_numerical(g, s, -type); else J = gmm_derivative_parameters_analytical(g, s, type); end % % function J = gmm_derivative_parameters_numerical(g, s, type) % Compute parameter Jacobians using numerical_Jacobian() approximation. switch type case 1 % dg/da J = numerical_Jacobian(sqrt(g.w), @modela, [], [], g, s); case 2 % dg/dx J = numerical_Jacobian(g.x(:), @modelb, [], [], g, s); case 3 % dg/dR for i=1:length(g.w) L = chol(g.P(:,:,i)); R(:,i) = extract_triu(L); end J = numerical_Jacobian(R(:), @modelc, [], [], g, s); otherwise error('Invalid selection: type must be between 1 and 3.') end % % function f = modela(a, g, s) g.w = a.^2; f = gmm_evaluate(g, s)'; % function f = modelb(x, g, s) g.x(:) = x; f = gmm_evaluate(g, s)'; % function f = modelc(R, g, s) [D,N] = size(g.x); R = reshape(R, [D*(D+1)/2 N]); L = zeros(D,D); for i=1:N L = fill_triu(L, R(:,i)); g.P(:,:,i) = L'*L; end f = gmm_evaluate(g, s)'; % function A = fill_triu(A, x) % trick based on Acklam N = size(A,1); i = find(triu(ones(N), 0)); A(i) = x; % function x = extract_triu(A) N = size(A,1); i = find(triu(ones(N), 0)); x = A(i); % % function J = gmm_derivative_parameters_analytical(g, s, type) % Compute parameter Jacobians analytically using the derivation of % Williams, pages 3-27 to 3-31. switch type case 1 % weights: dg/da for i=1:length(g.w) v = s - repvec(g.x(:,i), size(s,2)); J(:,i) = 2 * sqrt(g.w(i)) * gauss_evaluate(v, g.P(:,:,i))'; end case 2 % means: dg/dx D = size(g.x,1); ii=1; for i=1:length(g.w) v = s - repvec(g.x(:,i), size(s,2)); w = gauss_evaluate(v, g.P(:,:,i)); J(:,ii:ii+D-1) = g.w(i) * v' * inv(g.P(:,:,i)) .* repvec(w(:), D); ii = ii+D; end case 3 % covariances: dg/dR ii = 1; D = size(g.x,1); N = D*(D+1)/2; for i=1:length(g.w) v = s - repvec(g.x(:,i), size(s,2)); w = gauss_evaluate(v, g.P(:,:,i)); P = g.P(:,:,i); Pinv = inv(P); L = chol(P); for j=1:size(s,2) vv = v(:,j) * v(:,j)'; dgdL = g.w(i) * w(j) * Pinv*(vv-P)*Pinv * L'; % Note, Williams eq 3.45 has L not L', but it didn't work (Q. what does the other off-diagonal mean?) J(j, ii:ii+N-1) = extract_triu(dgdL')'; end ii = ii+N; end otherwise error('Invalid selection: type must be between 1 and 3.') end