covariance_intersect.m

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

function [x,P,a] = covariance_intersect(x1,P1, x2,P2, a)
%function [x,P,a] = covariance_intersect(x1,P1, x2,P2, a)
%
% For the time being I implement only a simple form of CI.
% There is no transform between spaces and the numerics are basic.
% The value for a is to minimise determinant.
%
% TODO: How does this derive from Bayes theorem? Is there any equivalent
% of the denominator p(z=z0) normalising term for the CI?
%
% TODO: 
%   - update with linear transform H
%   - closed form optimisation of a, or without optimisation (a as an input parameter)
%   - more numerically stable implementation

P1i = inv_posdef(P1);
P2i = inv_posdef(P2);

if nargin == 4
    a = fminbnd(@det_ci, 0, 1, [], P1i, P2i);
end

P = inv_posdef(a*P1i + (1-a)*P2i);
x = P*(a*P1i*x1 + (1-a)*P2i*x2);

%
%

function d = det_ci(a, P1i, P2i)
Ri = a*P1i + (1-a)*P2i;
d = 1 / det(Ri); % det(R) == 1/det(inv(R))