gauss_multiply.m

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

function [x,P,w] = gauss_multiply(x1,P1, x2,P2, logflag)
%function [x,P,w] = gauss_multiply(x1,P1, x2,P2, logflag)

if nargin == 4, logflag = 0; end

S = P1 + P2;
Sc  = chol(S);
Sci = inv(Sc);
Wc = P1*Sci;       % "normalised" gain
vc = Sci'*(x2-x1); % "normalised" innovation

% Update 
x = x1 + Wc*vc; 
P = P1 - Wc*Wc';

% Update weight
D = size(x,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