kf_update_w.m

来自「高斯滤波器 matlab toolbox For GMMs and Gauss」· M 代码 · 共 34 行

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