In order to optimally fuse two state estimates, their error cross-covariance matrix must be available. Since this matrix is often unknown to the fusion center, conservative fusion rules are employed that provide consistent fusion results irrespective of possible correlations.
A well-known conservative fusion rule is Covariance Intersection (CI), which meets certain optimality criteria as shown in [1] but can provide too conservative fusion results in typical estimation scenarios. With the introduction of Inverse Covariance Intersection (ICI), less conservative results can be computed. This improvement is achieved by tailoring ICI to the treatment of unknown common information. The derivation of ICI is explained in [2]. Recently, a more general study on ICI in [3] shows that ICI can also be applied to other types of correlations.
Two estimates \(\mvec{x}_\sA\) and \(\mvec{x}_\sB\) with corresponding error covariance matrices \(\mat{C}_\sA\) and \(\mat{C}_\sB\) can be fused with ICI according to
\[\begin{align} \mvec{x}_\ICI &= \mat{K}_\ICI\,\mvec{x}_\sA + \mat{L}_\ICI\,\mvec{x}_\sB \ ,\\ \mat{C}_\ICI\inv &= \mat{C}_\sA\inv + \mat{C}_\sB\inv - \left( \omega \mat{C}_\sA + (1-\omega)\mat{C}_\sB\right)\inv \ . \end{align}\]The fusion gains \(\mat{K}_\ICI\) and \(\mat{L}_\ICI\) are given by
\[\begin{align} \mat{K}_\ICI &= \mat{C}_\ICI \left(\mat{C}_\sA\inv -\omega \big( \omega \mat{C}_\sA + (1-\omega)\mat{C}_\sB\right)\inv \big) ,\\ \mat{L}_\ICI &= \mat{C}_\ICI \big(\mat{C}_\sB\inv -(1-\omega) \left( \omega \mat{C}_\sA + (1-\omega)\mat{C}_\sB\right)\inv \big) \ . \end{align}\]A simple Matlab implementation can be downloaded.
function [c,C,omega] = ICI(xA,CA,xB,CB)
% Inverse Covariance Intersection
%
% This function implements the ICI algorithm
% and fuses two estimates (xA,CA) and (xB,CB).
% It provides the fusion result (c,C) and the
% value of omega, which minimizes the trace of C.
f = @(w)trace(inv(inv(CA)+...
inv(CB)-inv(w*CA+(1-w)*CB)));
omega = fminbnd(f,0,1,optimset('Display','off'));
C = inv(inv(CA)+inv(CB)-...
inv(omega*CA+(1-omega)*CB));
% computations of the gains
KICI = C*(inv(CA)-...
omega*inv(omega*CA+(1-omega)*CB));
LICI = C*(inv(CB)-...
(1-omega)*inv(omega*CA+(1-omega)*CB));
c = KICI*xA + LICI*xB;