gusucode.com > 用粒子滤波算法进行跟踪的matlab代码 > gmm_utilities/KF_update_w.m
function [x,P,w]= KF_update_w(x,P,v,R,H, logflag) %function [x,P,w]= KF_update_w(x,P,v,R,H, logflag) % % Calculate the Kalman Filter update given the prior state [x,P], the innovation, v, the % observe uncertainty R, and the (linearised) observation model H. The weight, w, is the % update normalising constant. % % Tim Bailey 2005. if nargin == 5, logflag = 0; end PHt = P*H'; S = H*PHt + R; Sc = chol(S); % note: S = Sc'*Sc Sci = inv(Sc); % note: inv(S) = Sci*Sci' Wc = PHt * Sci; % "normalised" gain vc = Sci'*v; % "normalised" innovation % Update x = x + Wc*vc; P = P - Wc*Wc'; % Update weight D = size(v,1); numer = -0.5 * vc'*vc; if logflag ~= 0 denom = 0.5*D*log(2*pi) + sum(log(diag(Sc))); w = numer - denom; else denom = (2*pi)^(D/2) * prod(diag(Sc)); w = exp(numer) / denom; end