Reformulations of Ellipsoidal Intersection

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
function [c,C,Gamma] = EI(xA,CA,xB,CB)
% This function implements the EI algorithm and fuses two estimates
% (a,A) and (b,B) together to provide a new estimate (c,C). A common
% estimate (gamma, Gamma) is computed and subtracted from the fusion
% result.
Ai = inv(CA);
Bi = inv(CB);

[TA,DA] = eig(CA);
BT = sqrt(DA)\inv(TA)*CB*TA/sqrt(DA);
[SBT,DBT] = eig(BT);
DBT = max(DBT,eye(length(CA)));
Gamma = TA*sqrt(DA)*(SBT*DBT/SBT)*sqrt(DA)/TA;

% New covariance
C = inv(Ai+Bi-inv(Gamma));
% New mean
R = eps*eye(length(C)); % regularization
gamma = inv(Ai+Bi-2*inv(Gamma)+2*R)*((Bi-inv(Gamma)+R)*xA + (Ai-inv(Gamma)+R)*xB);
c = C*(CA\xA+CB\xB-Gamma\gamma);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
function [c,C] = EIv2(xa,CA,xb,CB)
% Ellipsoidal Intersection 
%
% This function implements the EI algorithm 
% and fuses two estimates (xA,CA) and (xB,CB). 

[TA,DA] = eig(CA);
BT = sqrt(DA)\inv(TA)*CB*TA/sqrt(DA);
[TBT,DBT] = eig(BT);
DBT = min(DBT,eye(length(CA)));

C = TA*sqrt(DA)*(TBT*DBT/TBT)*sqrt(DA)/TA;
% c = xa + (CA-C)/(CA+CB-2*C)*(xb-xa); % inversion of possibly singular matrix 
R = C + eps*eye(length(C)); % regularization
c = xa + (CA-R)/(CA+CB-2*R)*(xb-xa);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
function [c,C] = EIv3(xA,CA,xB,CB)
% Ellipsoidal Intersection 
%
% This function implements the EI algorithm 
% and fuses two estimates (xA,CA) and (xB,CB). 

[TA,DA] = eig(CA);
BT = sqrt(DA)\inv(TA)*CB*TA/sqrt(DA);
[TB,DB] = eig(BT);
DBT = min(DB,eye(length(CA)));
T = inv(TA*sqrt(DA)*(TB));

C = T\DBT/T';
DA = eye(length(CA));

KA = diag((DA<DB) + 0.5*(DA==DB));
KB = diag((DA>DB) + 0.5*(DA==DB));

c = T\(KA.*(T*xA) + KB.*(T*xB));