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