gusucode.com > 用粒子滤波算法进行跟踪的matlab代码 > gmm_utilities/gmm_update_linearised.m
function gr = gmm_update_linearised(g1, g2, hfunc) D = size(g1.x, 1); M = size(g1.x, 2); N = size(g2.x, 2); R = M*N; gr.x = zeros(D,R); gr.P = zeros(D,D,R); gr.w = zeros(1,R); k=1; for i=1:M xi = g1.x(:,i); Pi = g1.P(:,:,i); wi = g1.w(i); for j=1:N [v,H] = feval(hfunc,g1,g2); [gr.x(:,k), gr.P(:,:,k), w] = KF_update_w(xi,Pi, v,g2.P(:,:,j), H); gr.w(k) = w*wi*g2.w(j); k = k+1; end end