www.pudn.com > gmm_utilities.zip > 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