gauss_divide.m

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

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

if nargin == 4, logflag = 0; end

switch 1
case 1 % obvious implementation
    P1inv = inv(P1);
    P2inv = inv(P2);
    P = inv(P1inv - P2inv);
    x = P*(P1inv*x1 - P2inv*x2);    
case 2 % simple KF form, possibly better numerics
    S = P1 - P2; % note: typically S is not positive definite
    W = P1*inv(S);
    P = P1 - W*S*W';
    x = x1 + W*(x2-x1);    
otherwise
    error('Invalid selection')
end

P = (P+P')/2; % force symmetry

w = gauss_evaluate(x2-x, P2+P, logflag);
if logflag
    w = -w;
else
    w = 1/w;
end