A Game-Theoretic Approach to Robust Fusion
and Kalman Filtering Under Unknown Correlations

arXiv:1610.01045v1 [] 4 Oct 2016

Spyridon Leonardos1 and Kostas Daniilidis2
Abstract— This work addresses the problem of fusing two
random vectors with unknown cross-correlations. We present a
formulation and a numerical method for computing the optimal
estimate in the minimax sense. We extend our formulation
to linear measurement models that depend on two random
vectors with unknown cross-correlations. As an application
we consider the problem of decentralized state estimation
for a group of agents. The proposed estimator takes crosscorrelations into account while being less conservative than
the widely used Covariance Intersection. We demonstrate the
superiority of the proposed method compared to Covariance
Intersection with numerical examples and simulations within
the specific application of decentralized state estimation using
relative position measurements.

I. INTRODUCTION
State estimation is one of the fundamentals problem in
control theory and robotics. The most common state estimators are undoubtedly the Kalman filter [9], which is
optimal in the minimum mean squared error for the case of
linear systems, and its generalizations for nonlinear systems:
the Extended Kalman Filter (EKF) [16] and the Uscented
Kalman Filter (UKF) [6].
In multi-agent systems, the task of state estimation takes
a collaborative form in the sense that it involves inter-agent
measurements and constraints. Examples are cooperative localization in robotics [15] using relative pose measurements,
camera network localization using epipolar constraints [17]
and many more. On the one hand, a decentralized solution
that scales with the number of agents is necessary. On the
other hand, the state estimates become highly correlated as
information flows through the network. Ignoring these correlations has grave consequences: estimates become optimistic
and result in divergence of the estimator. This phenomenon
is analogous to the rumor spreading in social networks.
Unknown correlations may be present in other scenarios
as well. A popular simplification, that significantly reduces
computations and enables the use of EKF-based estimators,
is that noise sources are independent. For instance, a common
assumption in vision-aided inertial navigation is independence of the image projection noises for each landmark [5],
[12] although in reality, they are coupled with the motion of
the camera sensor. In other cases, it might be impractical to
store the entire covariance matrix due to storage limitations.
For instance, in Simultaneous Localization and Mapping
(SLAM) problems, there have been several approaches that
decouple the sensor state estimate from the estimates of the
1,2 The authors are with the Department of Computer and Information Science, University of Pennsylvania Philadelphia, PA 19104, USA

{spyridon,kostas}@seas.upenn.edu

landmark positions [8], [12] to increase the efficiency of the
estimator and reduce the storage requirements.
The most popular algorithm for fusion under the presence
of unknown correlations is the Covariance Intersection (CI)
method which was introduced by Julier and Uhlmann [7].
In its simplest form, the Covariance Intersection algorithm
is designed to fuse two random vectors whose correlation
is not known by forming a convex combination of the two
estimates in the information space. Covariance Intersection
produces estimates that are provably consistent, in the sense
that estimated error covariance is an upper bound of the
true error covariance. However, it has been observed [1],
[13], [18] that Covariance Intersection produces estimates
that are too conservative which may decrease the accuracy
and convergence speed of the overall estimator when used
as a component of an online estimator.
One of the most prominent applications of the proposed
fusion algorithm is distributed state estimation in an EKFbased framework. However, the problem of distributed state
estimation is far from new. There have been numerous
approaches for EKF-based distributed state estimation and
EKF-based cooperative localization. Yet, some of them require that each agent maintains the state of the entire network
[1], [15], which is impractical and does not scale with the
number of agents, while others ignore correlations [11], [14]
in order to simplify the estimation process or use Covariance
Intersection and variations of it [3], [10] despite its slow
convergence.
The contributions of this work are summarized as follows.
First of all, we propose a method for fusion of two random
vectors with unknown cross-correlations. The proposed approach is less conservative than the widely used Covariance
Intersection (CI) while taking cross-correlations into account.
Second of all, we extend our formulation for the case of
a linear measurement model. Finally, we present numerical
examples and simulations in a distributed state estimation
scenario which demonstrate the validity and comparative
performance of the proposed approach compared with the
Covariance Intersection.
The paper is structured as follows: in Section II we
include definitions of consistency and related notions and we
introduce the problem at hand. The game-theoretic approach
to fusing two random variables with unknown correlations
is the topic of Section III which is generalized for arbitrary
linear measurement models in Section III. In Section V
we include details on the implemented numerical algorithm.
Numerical examples and simulation results are presented in
Sections VI and VII respectively.

II. PROBLEM FORMALIZATION
In this section, we formalize the problem at hand. First,
we need a precise definition of consistency.
Definition 2.1 (Consistency [7]): Let z be a random vector with expectation E[z] = z. An estimate ze of z is
another random vector. The associated error covariance is
.
e zz =
denoted Σ
Cov (e
z − z). The pair (e
z , Σzz ) is consistent
if E[e
z ] = z and
e zz
Σzz  Σ
(1)
Problem Statement 1 (Consistent fusion): Given
two
consistent estimates (e
x, Σxx ), (e
y , Σyy ) of z, where
Σxx , Σyy are known upper bounds on the true error
covariances. The problem at hand consists of fusing the two
consistent estimates (e
x, Σxx ), (e
y , Σyy ) in a single consistent
estimate (e
z , Σzz ), where ze is of the form
ze = Wx x
e + Wy ye

(2)

with Wx + Wy = I in order to preserve the mean.
The most widely used solution of the above problem
is the Covariance Intersection algorithm [7]. Given upper
e xx , Σyy  Σ
e yy the Covariance Intersection
bounds Σxx  Σ
equations read

ze = Σzz ωΣ−1
e + (1 − ω)Σ−1
e
xx x
yy y
(3)
−1
−1
−1
Σzz = ωΣxx + (1 − ω)Σyy
where
 ω ∈ [0, 1]. It−1can be immediately seen that
Σzz ωΣ−1
= I which implies E[e
z ] = z.
xx + (1 − ω)Σyy
Moreover, it is easy to check that (e
z , Σzz ) is consistent.
The above can be easily generalized for the case of more
than 2 random variables, for partial measurements and for
the linear measurement model we consider in Section IV.
Usually, ω is chosen such that either tr(Σzz ) or log det(Σ−1
zz )
is minimized.
Next, we introduce a notion related to consistency but with
n
denote the positive semidefinite
relaxed requirements. Let S+
cone, that is the set of n × n positive semidefinite matrices.
n
n
First, recall that a function f : S+
→ R is called S+
nondecreasing [2] if
X  Y ⇒ f (X) ≥ f (Y )

(4)

n
for any X, Y ∈ S+
. An example of such a function is
f (X) = tr(X). Now, we are ready to introduce the notion
n
of consistency with respect to a S+
-nondecreasing function.

n
Definition 2.2 (f -Consistency): Let f : S+
→ R be
n
a nondecreasing function (with respect to S+
) satisfying
f (0) = 0. Let z be a random vector with expectation
E[z] = z and ze be an estimate of z with associated error
e zz . The pair (e
covariance Σ
z , Σzz ) is f -consistent if E[e
z] = z
and
e zz )
f (Σzz ) ≥ f (Σ
(5)

Remark 1: Observe that consistency implies f consistency. However, the converse in not necessarily
true.

Problem Statement 2 (Trace-consistent fusion): Given
two consistent estimates (e
x, Σxx ), (e
y , Σyy ) of z, where
Σxx , Σyy are known upper bounds on the true error
variances. The problem at hand consists of fusing the
two consistent estimates (e
x, Σxx ), (e
y , Σyy ) in a single
trace-consistent estimate (e
z , Σzz ), where ze is a linear
combination of x and y and
e zz )
tr(Σzz ) ≥ tr(Σ

(6)

Next, we introduce a game-theoretic formulation for the
problem of trace-consistent fusion. Relaxing the consistency
constraint to the trace-consistency constraint enables us to
estimate the weighting matrices Wx , Wy according to some
optimality criterion, which is none other than the minimax
of the trace of the covariance matrix.
Remark 2: No assumptions on the distribution of the
estimates x̃ and ỹ have been made so far.
III. ROBUST FUSION
The goal of this section is the derivation of our minimax
approach. First, we need some basic notions from game
theory. A zero-sum, two-player game on Rm × Rn is defined
by a pay-off function f : Rm × Rn → R. Intuitively, the first
player makes a move u ∈ Rm then, the second player makes
a move v ∈ Rm and receives payment from the first player
equal to f (u, v). The goal of the first player is to minimize its
payment and the goal of the second player is to maximize the
received payment. The game is convex-concave if the payoff function f (u, v) is convex in u for fixed v and concave
in v for fixed u. For a review minimax and convex-concave
games in the context of convex optimization, we refer the
reader to [4].
Let z be a random vector with expectation E[z] = z.
Assume we have two estimates (e
x, Σxx ), (e
y , Σyy ) of z where
Σxx , Σyy are approximations to the true error covariances
e xx , Σ
e yy . Based on the discussion of Section II, the fused
Σ
estimate is of the form
ze = (I − K)e
x + K ye

(7)
.
= Cov (e
z − z) is

e zz
and the associated error covariance Σ
given by
"
#

e xx Σ
e xy I − K T

 Σ
e
Σzz = I − K K e T
e yy
KT
Σxy Σ

(8)

e xx , Σ
e yy are not known. Therefore, we define
However, Σ



 Σxx Σxy I − K T
. 
Σzz = I − K K
(9)
ΣTxy Σyy
KT
where we have the following Linear Matrix Inequality (LMI)
constraint on Σxy


Σxx Σxy
0
(10)
ΣTxy Σyy
Remark 3: It can be seen that tr(Σzz ) is convex in K
for a fixed Σxy satisfying (10). Therefore, the supremum of
tr(Σzz ) over all Σxy satisfying (10) is a convex function

of K. Moreover, for a fixed K, tr(Σzz ) is linear, and thus
concave as well, in Σxy with a convex domain defined by
(10). It follows that tr(Σzz ) is a convex-concave function in
(K, Σxy ).
As anticipated, we formulate the problem of finding the
weighting matrix K as a zero-sum, two-player convexconcave game: the first player chooses K to minimize
tr(Σzz ) whereas the second player chooses Σxy to maximize
tr(Σzz ). More specifically, let (K ? , Σ?xy ) be the solution to
the following minimax optimization problem
minimize
K

subject to

sup tr(Σzz )
Σxy


Σxx Σxy
0
ΣTxy Σyy

(11)



Σ?xy I − K ?T
Σyy
K ?T

(12)

Naturally, we have the following lemma.
Lemma 3.1: If (e
x, Σxx ) and (e
y , Σyy ) are consistent, then
the pair (e
z , Σ?zz ) given by (12) is trace-consistent.
A proof of lemma 3.1 is presented in AppendixVIII-A.
The problem of numerically solving problem (11) is the
topic of subsequent sections. The case under consideration
in this section can be viewed as a special case of the next
section.
IV. ROBUST LINEAR UPDATE
In this section, we explore a more general setting. We
assume we have two random vectors x, y with expectations
e and ye
E[x] = x and E[y] = y. We have some estimates x
of x and y respectively with associated error covariances
e xx and Σ
e yy . As before, we assume that the true error
Σ
covariances are only approximately known. Let Σxx and Σyy
denote these approximate values. We assume we have a linear
measurement model of the form
z = Cx + Dy + η

+ KΣη K T

e xx and Σ
e yy are not
Σ
Σxy
Σyy



I − CT KT
−DT K T
(17)

where Σxy should satisfy (10) in order to be a valid crosscorrelation. To alleviate notation, let X = K T and define
.
f (X, Σxy ) = tr(Σ+
(18)
xx )
By rewriting (10) using Schur complement, the minimax
formulation is written as follows
minimize sup f (X, Q)

Then, the fused estimated and the associated error covariance
are given by
ze = (I − K ? )e
x + K ? ye


 Σxx
?
?
?
K
Σzz = I − K
Σ?T
xy

However, the true error covariances
known. Therefore, we define


 Σxx
+ .
Σxx = I − KC −KD
ΣTxy

(13)

where η is a zero-mean noise process with covariance Ση . We
assume that the measurement noise process η is independent
to the estimates x
e and ye. As in the classic Kalman filter
derivation, we are looking for an update step of the form
x
e+ = x
e + K(z − ze)

(14)
.
where ze = C x
e + De
y . The error of the update is given by
x
e+ − x = (I − KC)(e
x − x) − KD(e
y − y) + Kη

(15)
.
e+
and the associated error covariance is defined as Σ
xx =
Cov (e
x+ − x) and is given by
"
#

e xx Σ
e xy I − C T K T

 Σ
+
e xx = I − KC −KD
Σ
eT Σ
e yy
−DT K T
Σ
xy
+ KΣη K T

(16)

X

Q
−1/2 T −1
Σyy
Q Σxx QΣ−1/2
−I 0
yy

subject to

(19)

Let (X ? , Q? ) be the optimal solution of problem (19) and
let (K ? , Σ?xy ) = (X ?T , Q? ). Then,
x
e+ = (I − K ? C)e
x − K ? De
y



Σxx
+?
?
?
Σxx = I − K C −K D
Σ?T
xy
+ KΣη K T

Σ?xy
Σyy



I − C T K ?T
−DT K ?T
(20)

Naturally, we have the following lemma.
Lemma 4.1: If (e
x, Σxx ) and (e
y , Σyy ) are consistent, then
)
given
by
(20)
is trace-consistent
the pair (e
x+ , Σ+?
xx
The proof of lemma 4.1 is exactly analogous to the proof of
lemma 3.1 presented in AppendixVIII-A.
Remark 4: When C = I, D = −I, Ση = 0, we recover
the case of the simple fusion of two random vectors.
V. INTERIOR POINT METHODS FOR
CONVEX-CONCAVE GAMES
In this section, we describe the numerical method we use
to solve Problem (19). First, we will look at the simpler
case of an unconstrained convex-concave game with pay-off
function f (u, v). A point (u? , v ? ) is a saddle point for an
unconstrained convex-concave game with pay-off function
f (u, v) if
f (u? , v) ≤ f (u? , v ? ) ≤ f (u, v ? )

(21)

and the optimality conditions for differentiable convexconcave pay-off function are
∇u f (u? , v ? ) = 0,

∇v f (u? , v ? ) = 0

(22)

We use the infeasible start Newton method [2], outlined in
Algorithm 1, to find the optimal solution of the unconstrained
problem:
minimize maximize f (u, v)
(23)
u

u

Intuitively, at each step the directions ∆unt , ∆vnt are the
solutions of the first order approximation
0 = r(u+∆unt , v+∆vnt ) ≈ r(u, v)+Dr(u, v)[∆unt , ∆vnt ]
(24)

where r(u, v) = [∇u f (u, v)T , ∇v f (u, v)T ]T . Then, a backtracking line search is performed on the norm of the residual
along the previously computed directions.
Algorithm 1 Infeasible start Newton method.
given: starting points u, v ∈ domf ,
tolerance  > 0, α ∈ (0, 1/2), β ∈ (0, 1).
Repeat
1. r(u, v) = [∇u f (u, v)T , ∇v f (u, v)T ]T
2. Compute Newton steps by solving

Let (e
zCI , ΣCI ) and (e
zRF , ΣRF ) be the fused estimates and
the corresponding error covariances obtained from Covariance Intersection and Robust Fusion. We have that zeCI =
zeRF = [0 0]T and




3.79
0
3 0
ΣCI =
, ΣRF =
,
(30)
0
5.79
0 5

Dr(u, v)[∆unt , ∆vnt ] = −r(u, v)
3. Backtracking line search on krk2 .
t = 1.
ut = u + t∆unt , vt = v + t∆vnt .
While kr(ut , vt )k2 > (1 − αt)kr(u, v)k2
t = βt.
ut = u + t∆unt , vt = v + t∆vnt .
EndWhile
4. Update: u = u + t∆unt , v = v + t∆vnt .
until kr(u, v)k2 ≤ 

(25)

where f (X, Q) as defined in (18) and
T −1
−1/2
f1 (Q) = Σ−1/2
−I
yy Q Σxx QΣyy

(26)

Intuitively, 1t ft approaches f as t → ∞. Note that ft (X, Q)
is still convex-concave for t > 0. The optimality conditions
for a fixed t > 0 are given by
∇X ft (X ? , Q? ) = 0,

∇Q ft (X ? , Q? ) = 0

(27)

where explicit expressions for ∇X ft and ∇Q ft are presented
in AppendixVIII-B along with the linear equations for computing ∆Xnt , ∆Qnt .
Finally, the structure of the problem allows us to easily
identify a strictly feasible initial point (X0 , Q0 ) where Q0 =
0 and X0 is given by
(CΣxx C T + DΣyy DT + Ση )X0 = CΣxx

CI
RF
MLE

5

However, the problem at hand is slightly more complicated
since it involves a linear matrix inequality. Therefore, we
use the barrier method [2]. Intuitively, a sequence of unconstrained minimization problems is solved, using the last point
iteration is the starting point for the next iteration. Define for
t > 0, the cost function ft (X, Q) by
ft (X, Q) = tf (X, Q) + log det(−f1 (Q))

VI. NUMERICAL EXAMPLES
In this section, we present two numerical examples which
shed light on the differences between the Covariance Intersection (CI) and the proposed Robust Fusion (RF) approaches. First, consider the example

Tof fusing two random
variables with means x
e = ye = 0 0 and covariances




5 0
3 0
Σxx =
, Σyy =
(29)
0 5
0 7

(28)

For details on the convergence of the infeasible start
Newton method and the barrier method for convex-concave
games, we refer the reader to [2], [4].
Remark 5: Notation: Df (x)[h] denotes the (Fréchet)
derivative or differential of f at x along h. Similarly,
Df (x, y)[hx , hy ] denotes the differential of f at (x, y) along
(hx , hy ).

0

−5
−5

0

5

Fig. 1. Confidence ellipses: given a covariance matrix Σ we draw the set
{x : xT Σ−1 x = 1}. Initial confidence ellipse (black), Maximum likelihood
Estimate (MLE) confidence ellipse (gray dashed) for various values of
correlation, CI confidence ellipse (green) and RF confidence ellipse (red).
The confidence ellipses obtained from MLE lie in the intersection of the
T −1
two ellipsoids {x : xT Σ−1
xx x ≤ 1} and {x : x Σyy x ≤ 1}. Since the
proposed approach is equivalent to MLE for some worst-case correlation,
the RF confidence ellipse lies in the intersection of the two ellipsoids as well.
When correlation increases, the trace of the covariance of MLE approaches
the trace of ΣRF . CI is not maximum likelihood for any value of correlation
but produces a guaranteed upper bound on the true error covariance.

In the second example, we consider the case of partial
measurements. More specifically, using notation of Section
IV, let
 


0
5 0
x
e=
, Σxx =
(31)
0
0 5


and C = 1 0 , z = ze = 0, Σyy = 1, D = 1 and Ση = 0.
Both Covariance Intersection and Robust Fusion yield ze+ =
0 but




3 0
1 0
ΣCI =
, ΣRF =
,
(32)
0 6
0 5
Observe that despite we have a measurement of only the
first coordinate, the error variance of the second coordinate
increased! The reason for this phenomenon is that the CI
updates the current estimate and the associated error covariance along a predefined direction only. Although tr(ΣCI ) <
tr(Σxx ), the bound on the true error covariance estimated by
Covariance Intersection is very conservative.
1

3

2

−2

4

1

−4

Fig. 3.

6

RF
CI
MLE

4
2
0

−6

−8

−6

−4

−2

0

2

4

6

8

prediction step is the same as the Kalman Filter (KF)
prediction step, that is

Fig. 2. Illustration of the second numerical example. Initial confidence
ellipse (black), Maximum likelihood Estimate (MLE) confidence ellipse
(gray dashed) for various values of correlation, CI confidence ellipse (green)
and RF confidence ellipse (red).

VII. SIMULATIONS
Finally, we consider an application in distributed state
estimation using relative position measurements. We experiment with a group of n = 4 agents on the plane with
a communication network topology as depicted in Fig. 3.
If there is an edge from i to j, then agent i transmits its
current state estimate and the corresponding error covariance
estimate to agent j which upon receipt, takes a measurement
of the relative position and updates its own state estimate
and associated error covariance estimate. All agents have
identical dynamics described by

 

  
xi (t + 1)
I I xi (t)
0
=
+
w (t)
(33)
vi (t + 1)
0 I vi (t)
I i
where xi (t) ∈ R2 and vi (t) ∈ R2 denote respectively
the position and velocity of agent i at time instance t
.
and wi (t) ∼ N (0, Qi (t)) is the noise process. If xi (t) =


T
xi (t)T vi (t)T , then let A, B such that
xi (t + 1) = Axi (t) + Bwi (t)

Network topology.

(34)

ei (t + 1|t) = Ae
x
xi (t|t)

(37)
T

Σi (t + 1|t) = AΣi (t + 1|t)A + BQi (t)B

T

(38)

ei (t + 1|t) denotes the estimate of agent i for its state
where x
at time t + 1 having received measurements up to time t and
Σi (t + 1|t) is the associated error covariance.
We evaluate four estimator, three decentralized and one
centralized: Naive Fusion (NF) which ignores correlations,
Robust Fusion (RF), Covariance Intersection (CI) and Centralized Kalman Filter (CKF). The Centralized Kalman Filter
(CKF) is simply a standard Kalman Filter containing all
agent states. It serves as a measure of how close the decentralized estimators are to the optimal centralized estimator.
Results can be seen in Figure 4 and Table I. We used the
following values for the noise parameters: Qi = 10−6 I2 for
all agents, R1 = I2 and Rij = 10−2 I2 for all pairwise
measurements. The Robust Fusion based estimator significantly outperforms the Covariance Intersection based estimator which produces particularly noisy velocity estimates.
TABLE I
P OSITION ERRORS
Agent #
1
2
3
4

CKF
0.174 ± 0.107
0.166 ± 0.098
0.170 ± 0.099
0.161 ± 0.085

RF
m
m
m
m

0.222 ± 0.091
0.248 ± 0.108
0.248 ± 0.114
0.238 ± 0.090

CI
m
m
m
m

0.230 ± 0.093
0.286 ± 0.145
0.343 ± 0.187
0.285 ± 0.128

m
m
m
m

1

Only agent 1 is equipped with global position system
(GPS), that is we have a measurement of the form
y1 (t) = x1 (t) + η1 (t)

(35)

VIII. APPENDIX
A. Proof of lemma 3.1

where η1 (t) ∼ N (0, R1 ). Agent 1 performs a standard
Kalman Filter update step after a GPS measurement. For
each edge (i, j) we have a pairwise measurement of the form

First, it is easy to see that if E[e
x] = E[e
y ] = z then E[e
z] =
e xx and Σyy  Σ
e yy
z. Now, one has to show that if Σxx  Σ
e zz ). We have that Σ?zz − Σ
e zz is equal to
then tr(Σ?zz ) ≥ tr(Σ

yij (t) = xj (t) − xi (t) + ηij (t)

e xx )(I − K ?T ) + K ? (Σyy − Σ
e yy )K ?T
(I − K ? )(Σxx − Σ
?T
eT
e xy )K ?T
+ K ? (Σ?T
) + (I − K ? )(Σ?xy − Σ
xy − Σxy )(I − K

(36)

where ηij (t) ∼ N (0, Rij ). Updates of the state estimates
can be performed by either ignoring cross-correlations (Naive
Fusion) or by one of Covariance Intersection or the proposed
Robust Fusion.
Each agent maintains only its one state and communicates
its to each neighbors at each time instance. The individual

?T
eT
e xy )K ?T
 K ? (Σ?T
) + (I − K ? )(Σ?xy − Σ
xy − Σxy )(I − K

e xx and Σyy  Σ
e yy . Since trace is S n since Σxx  Σ
+
nondecreasing, we get


e zz ) ≥ 2 tr K T (I − K)(Σ?xy − Σ
e xy ) ≥ 0
tr(Σ?zz − Σ

2

200

250

0.5

0

300

50

100

t

150

200

250

0

300

2

200

250

0.5

0

300

50

100

150

200

250

100

150

200

250

1

0.5

50

100

150

200

250

200

250

150

200

250

300

0

300

0.5

0

50

t

100

150

50

100

200

250

150

200

300

RF
CI
CKF

1

0.5

50

100

150

200

250

0.2

0.1

0

300

50

100

150

200

250

300

t
0.3
RF
CI
CKF

1.5

1

0.5

0

300

250

0.3

RF
CI

1

300

t

2

1.5

250

0.1

t

Position error

Position error

Agent 4

Position error
150

100

RF
CKF

2

100

50

1.5

0

300

2

4

200

RF
CI

1.5

0

300

150

0.2

t

RF
NF

50

0.5

t

6

0

1

2

t

100

RF
CI
CKF

1.5

0

300

Position error

Position error

Position error

Agent 3

2

50

t

RF
CKF

4

0

300

0.3

t
RF
NF

50

250

Velocity error

1

2

0

200

0.1

RF
CI

1.5

t
6

150

2

Position error

Position error

Position error

Agent 2

2

150

100

RF
CKF

4

100

50

0.2

t

RF
NF

50

0.5

t

6

0

1

Velocity error

150

1

1.5

Velocity error

100

1.5

RF
CI
CKF

Velocity error

2

50

0.3
RF
CI

Position error

Position error

Position error

Agent 1

4

0

2
RF
CKF

RF
NF

6

50

100

t

150

200

250

0.2

0.1

0

300

50

100

t

150

200

250

300

t

Fig. 4.
Comparison of the four methods. The Naive Fusion estimator quickly diverges, whereas the Robust Fusion and Covariance Intersection do
not. Clearly the Robust Fusion estimator is more accurate than the Covariance Intersection and produces less noisy estimates due to its less conservative
nature. The steady state error covariance estimate of the Covariance Intersection is much larger than the actual error covariance making the estimator more
susceptible to measurement noise.



since tr K ?T (I − K ? )Σ?xy ≥ tr K ?T (I − K ? )Σxy for
every Σxy satisfying (10) due to optimality of Σ?xy . Verifying
e xy satisfies (10) is straightforward.
that Σ

B. Formulas for computing the Newton steps
First of all, the differential of f1 (Q) at the direction of
∆Q is given by
 −1/2
T −1
Df1 (Q)[∆Q] = Σ−1/2
∆QT Σ−1
yy
xx Q + Q Σxx ∆Q Σyy
(39)
For small ∆X, we have the first order approximation [2]:
log det(X + ∆X) ≈ log det(X) + tr(X −1 ∆X)

(40)

and thus, using the chain rule, we obtain
−1/2
−1 −1/2
∇Q log det(−f1 (Q)) = 2Σ−1
Σyy
(41)
xx QΣyy f1 (Q)

Moreover, we have

∇X f (X, Q) =2 C


 Σxx
D
QT

−2(CΣxx + DQT )

Q
Σyy



CT
T + Ση X
D



(42)

and
∇Q f (X, Q) = 2(C T XX T D − X T D)

(43)

. −1/2
−1/2
Let g1 (Q) = Σyy f1 (Q)−1 Σyy . Using
(X + ∆X)−1 ≈ X −1 − X −1 ∆XX −1

(44)

for small ∆X and the chain rule, we obtain the following
system of linear equations for (∆Xnt , ∆Qnt ):

2t C

D


 Σxx
QT

Q
Σyy



CT
T + Ση ∆Xnt
D



+2t(C∆Qnt DT X − D∆QTnt (I − C T X)) = −∇X ft (X, Q)
(45)
and
T
2t(C T ∆Xnt X T D − (I − C T X)∆Xnt
D)


T
−1
T −1
−2Σ−1
xx Qg1 (Q) ∆Qnt Σxx Q + Q Σxx ∆Qnt g1 (Q)
+2Σ−1
xx ∆Qnt g1 (Q) = −∇Q ft (X, Q)

(46)

R EFERENCES
[1] Pablo O Arambel, Constantino Rago, and Raman K Mehra. Covariance
intersection algorithm for distributed spacecraft state estimation. In
American Control Conference, 2001. Proceedings of the 2001, volume 6, pages 4398–4403. IEEE, 2001.
[2] Stephen Boyd and Lieven Vandenberghe. Convex optimization. Cambridge university press, 2004.
[3] Luis C Carrillo-Arce, Esha D Nerurkar, José L Gordillo, and Stergios I
Roumeliotis. Decentralized multi-robot cooperative localization using
covariance intersection. In 2013 IEEE/RSJ International Conference
on Intelligent Robots and Systems, pages 1412–1417. IEEE, 2013.
[4] A Ghosh and S Boyd. Minimax and convex-concave game. lecture
notes for course EE392:‘Optimization Projects” Stanford Univ., Stanford, CA, 2003.
[5] J. Hernandez, K. Tsotsos, and S. Soatto. Observability, identifiability
and sensitivity of vision-aided inertial navigation. In IEEE International Conference on Robotics and Automation (ICRA), pages 2319–
2325, May 2015.
[6] Simon J Julier and Jeffrey K Uhlmann. New extension of the
kalman filter to nonlinear systems. In AeroSense’97, pages 182–193.
International Society for Optics and Photonics, 1997.
[7] Simon J Julier and Jeffrey K Uhlmann. A non-divergent estimation
algorithm in the presence of unknown correlations. In In Proceedings
of the American Control Conference. Citeseer, 1997.
[8] Simon J Julier and Jeffrey K Uhlmann. Using covariance intersection
for slam. Robotics and Autonomous Systems, 55(1):3–20, 2007.
[9] Rudolph Emil Kalman. A new approach to linear filtering and
prediction problems. Journal of basic Engineering, 82(1):35–45, 1960.
[10] Hao Li and Fawzi Nashashibi. Cooperative multi-vehicle localization
using split covariance intersection filter. IEEE Intelligent transportation systems magazine, 5(2):33–44, 2013.
[11] Agostino Martinelli. Improving the precision on multi robot localization by using a series of filters hierarchically distributed. In 2007
IEEE/RSJ International Conference on Intelligent Robots and Systems,
pages 1053–1058. IEEE, 2007.
[12] Anastasios I Mourikis and Stergios I Roumeliotis. A multi-state constraint kalman filter for vision-aided inertial navigation. In Proceedings
2007 IEEE International Conference on Robotics and Automation,
pages 3565–3572. IEEE, 2007.
[13] Esha D Nerurkar and Stergios I Roumeliotis. Power-slam: A linearcomplexity, consistent algorithm for slam. In 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 636–643.
IEEE, 2007.
[14] Stefano Panzieri, Federica Pascucci, and Roberto Setola. Multirobot
localisation using interlaced extended kalman filter. In 2006 IEEE/RSJ
International Conference on Intelligent Robots and Systems, pages
2816–2821. IEEE, 2006.
[15] Stergios I Roumeliotis and George A Bekey. Distributed multirobot localization. IEEE Transactions on Robotics and Automation,
18(5):781–795, 2002.
[16] Harold Wayne Sorenson. Kalman filtering: theory and application.
IEEE, 1985.
[17] Roberto Tron and Rene Vidal. Distributed 3-d localization of camera
sensor networks from 2-d image measurements. Automatic Control,
IEEE Transactions on, 59(12):3325–3340, 2014.
[18] Xun Xu and Shahriar Negahdaripour. Application of extended covariance intersection principle for mosaic-based optical positioning and
navigation of underwater vehicles. In Robotics and Automation, 2001.
Proceedings 2001 ICRA. IEEE International Conference on, volume 3,
pages 2759–2766. IEEE, 2001.

