A Game-Theoretic Approach to Robust Fusion
and Kalman Filtering Under Unknown Correlations
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 cross-
correlations 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 es-
timators 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 lo-
calization 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 corre-
lations 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 indepen-
dence 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,2The authors are with the Department of Computer and Informa-
tion 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 EKF-
based 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 re-
quire 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 ap-
proach 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.
arXiv:1610.01045v1  [cs.SY]  4 Oct 2016
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 vec-
tor with expectation E[z] = z. An estimate ez of z is
another random vector. The associated error covariance is
denoted eΣzz .= Cov (ez −z). The pair (ez, Σzz) is consistent
if E[ez] = z and
Σzz ⪰eΣzz
(1)
Problem Statement 1 (Consistent fusion): Given
two
consistent
estimates
(ex, Σxx),
(ey, Σ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 (ex, Σxx), (ey, Σyy) in a single consistent
estimate (ez, Σzz), where ez is of the form
ez = Wxex + Wyey
(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
bounds Σxx ⪰eΣxx, Σyy ⪰eΣyy the Covariance Intersection
equations read
ez = Σzz

ωΣ−1
xx ex + (1 −ω)Σ−1
yy ey
	
Σ−1
zz = ωΣ−1
xx + (1 −ω)Σ−1
yy
(3)
where ω
∈
[0, 1]. It can be immediately seen that
Σzz

ωΣ−1
xx + (1 −ω)Σ−1
yy
	
= I which implies E[ez] = z.
Moreover, it is easy to check that (ez, Σ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
relaxed requirements. Let Sn
+ denote the positive semidefinite
cone, that is the set of n × n positive semidefinite matrices.
First, recall that a function f : Sn
+ →R is called Sn
+-
nondecreasing [2] if
X ⪰Y ⇒f(X) ≥f(Y )
(4)
for any X, Y
∈Sn
+. An example of such a function is
f(X) = tr(X). Now, we are ready to introduce the notion
of consistency with respect to a Sn
+-nondecreasing function.
Definition 2.2 (f-Consistency): Let f
: Sn
+ →R be
a nondecreasing function (with respect to Sn
+) satisfying
f(0) = 0. Let z be a random vector with expectation
E[z] = z and ez be an estimate of z with associated error
covariance eΣzz. The pair (ez, Σzz) is f-consistent if E[ez] = z
and
f(Σzz) ≥f(eΣzz)
(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 (ex, Σxx), (ey, Σ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 (ex, Σxx), (ey, Σyy) in a single
trace-consistent estimate (ez, Σzz), where ez is a linear
combination of x and y and
tr(Σzz) ≥tr(eΣzz)
(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 pay-
off 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 (ex, Σxx), (ey, Σ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
ez = (I −K)ex + Key
(7)
and the associated error covariance eΣzz .= Cov (ez −z) is
given by
eΣzz =

I −K
K

"
eΣxx
eΣxy
eΣT
xy
eΣyy
# I −KT
KT

(8)
However, eΣxx, eΣyy are not known. Therefore, we define
Σzz .=
I −K
K Σxx
Σxy
ΣT
xy
Σyy
 
I −KT
KT

(9)
where we have the following Linear Matrix Inequality (LMI)
constraint on Σxy
Σxx
Σxy
ΣT
xy
Σyy

⪰0
(10)
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 convex-
concave 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
sup
Σxy
tr(Σzz)
subject to
Σxx
Σxy
ΣT
xy
Σyy

⪰0
(11)
Then, the fused estimated and the associated error covariance
are given by
ez = (I −K⋆)ex + K⋆ey
Σ⋆
zz =
I −K⋆
K⋆ Σxx
Σ⋆
xy
Σ⋆T
xy
Σyy
 I −K⋆T
K⋆T

(12)
Naturally, we have the following lemma.
Lemma 3.1: If (ex, Σxx) and (ey, Σyy) are consistent, then
the pair (ez, Σ⋆
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[x] = x and E[y] = y. We have some estimates ex and ey
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 + η
(13)
where η is a zero-mean noise process with covariance Ση. We
assume that the measurement noise process η is independent
to the estimates ex and ey. As in the classic Kalman filter
derivation, we are looking for an update step of the form
ex+ = ex + K(z −ez)
(14)
where ez .= Cex + Dey. The error of the update is given by
ex+ −x = (I −KC)(ex −x) −KD(ey −y) + Kη
(15)
and the associated error covariance is defined as eΣ+
xx
.=
Cov (ex+ −x) and is given by
eΣ+
xx =
I −KC
−KD
"
eΣxx
eΣxy
eΣT
xy
eΣyy
# 
I −CT KT
−DT KT

+ KΣηKT
(16)
However, the true error covariances eΣxx and eΣyy are not
known. Therefore, we define
Σ+
xx
.=

I −KC
−KD
 Σxx
Σxy
ΣT
xy
Σyy
 I −CT KT
−DT KT

+ KΣηKT
(17)
where Σxy should satisfy (10) in order to be a valid cross-
correlation. To alleviate notation, let X = KT and define
f(X, Σxy) .= tr(Σ+
xx)
(18)
By rewriting (10) using Schur complement, the minimax
formulation is written as follows
minimize
X
sup
Q
f(X, Q)
subject to
Σ−1/2
yy
QT Σ−1
xx QΣ−1/2
yy
−I ⪯0
(19)
Let (X⋆, Q⋆) be the optimal solution of problem (19) and
let (K⋆, Σ⋆
xy) = (X⋆T , Q⋆). Then,
ex+ = (I −K⋆C)ex −K⋆Dey
Σ+⋆
xx =
I −K⋆C
−K⋆D Σxx
Σ⋆
xy
Σ⋆T
xy
Σyy
 I −CT K⋆T
−DT K⋆T

+ KΣηKT
(20)
Naturally, we have the following lemma.
Lemma 4.1: If (ex, Σxx) and (ey, Σyy) are consistent, then
the pair (ex+, Σ+⋆
xx ) given by (20) is trace-consistent
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 convex-
concave pay-off function are
∇uf(u⋆, v⋆) = 0,
∇vf(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
u
maximize
u
f(u, v)
(23)
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) = [∇uf(u, v)T , ∇vf(u, v)T ]T . Then, a back-
tracking 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) = [∇uf(u, v)T , ∇vf(u, v)T ]T
2. Compute Newton steps by solving
Dr(u, v)[∆unt, ∆vnt] = −r(u, v)
3. Backtracking line search on ∥r∥2.
t = 1.
ut = u + t∆unt, vt = v + t∆vnt.
While ∥r(ut, vt)∥2 > (1 −αt)∥r(u, v)∥2
t = βt.
ut = u + t∆unt, vt = v + t∆vnt.
EndWhile
4. Update: u = u + t∆unt, v = v + t∆vnt.
until ∥r(u, v)∥2 ≤ϵ
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 uncon-
strained 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))
(25)
where f(X, Q) as defined in (18) and
f1(Q) = Σ−1/2
yy
QT Σ−1
xx QΣ−1/2
yy
−I
(26)
Intuitively, 1
t 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
∇Xft(X⋆, Q⋆) = 0,
∇Qft(X⋆, Q⋆) = 0
(27)
where explicit expressions for ∇Xft and ∇Qft are presented
in AppendixVIII-B along with the linear equations for com-
puting ∆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ΣxxCT + DΣyyDT + Ση)X0 = CΣxx
(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 (Fr´echet)
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).
VI. NUMERICAL EXAMPLES
In this section, we present two numerical examples which
shed light on the differences between the Covariance In-
tersection (CI) and the proposed Robust Fusion (RF) ap-
proaches. First, consider the example of fusing two random
variables with means ex = ey =
0
0T and covariances
Σxx =
5
0
0
5

, Σyy =
3
0
0
7

(29)
Let (ezCI, ΣCI) and (ezRF , ΣRF ) be the fused estimates and
the corresponding error covariances obtained from Covari-
ance Intersection and Robust Fusion. We have that ezCI =
ezRF = [0 0]T and
ΣCI =

3.79
0
0
5.79

, ΣRF =

3
0
0
5

,
(30)
−5
0
5
−5
0
5
CI
RF
MLE
1
Fig. 1.
Confidence ellipses: given a covariance matrix Σ we draw the set
{x : xT Σ−1x = 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
two ellipsoids {x : xT Σ−1
xx x ≤1} and {x : xT Σ−1
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
ex =
0
0

, Σxx =
5
0
0
5

(31)
and C =
1
0
, z = ez = 0, Σyy = 1, D = 1 and Ση = 0.
Both Covariance Intersection and Robust Fusion yield ez+ =
0 but
ΣCI =
3
0
0
6

, ΣRF =
1
0
0
5

,
(32)
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 covari-
ance along a predefined direction only. Although tr(ΣCI) <
tr(Σxx), the bound on the true error covariance estimated by
Covariance Intersection is very conservative.
−8
−6
−4
−2
0
2
4
6
8
−6
−4
−2
0
2
4
6
RF
CI
MLE
1
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 exper-
iment 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)
vi(t + 1)

=
I
I
0
I
 xi(t)
vi(t)

+
0
I

wi(t)
(33)
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) .=

xi(t)T
vi(t)T T , then let A, B such that
xi(t + 1) = Axi(t) + Bwi(t)
(34)
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)
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
yij(t) = xj(t) −xi(t) + ηij(t)
(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
1
2
3
4
Fig. 3.
Network topology.
prediction step is the same as the Kalman Filter (KF)
prediction step, that is
exi(t + 1|t) = Aexi(t|t)
(37)
Σi(t + 1|t) = AΣi(t + 1|t)AT + BQi(t)BT
(38)
where exi(t+1|t) denotes the estimate of agent i for its state
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 Cen-
tralized 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 decen-
tralized 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−6I2 for
all agents, R1 = I2 and Rij = 10−2I2 for all pairwise
measurements. The Robust Fusion based estimator signifi-
cantly outperforms the Covariance Intersection based esti-
mator which produces particularly noisy velocity estimates.
TABLE I
POSITION ERRORS
Agent #
CKF
RF
CI
1
0.174 ± 0.107 m
0.222 ± 0.091 m
0.230 ± 0.093 m
2
0.166 ± 0.098 m
0.248 ± 0.108 m
0.286 ± 0.145 m
3
0.170 ± 0.099 m
0.248 ± 0.114 m
0.343 ± 0.187 m
4
0.161 ± 0.085 m
0.238 ± 0.090 m
0.285 ± 0.128 m
VIII. APPENDIX
A. Proof of lemma 3.1
First, it is easy to see that if E[ex] = E[ey] = z then E[ez] =
z. Now, one has to show that if Σxx ⪰eΣxx and Σyy ⪰eΣyy
then tr(Σ⋆
zz) ≥tr(eΣzz). We have that Σ⋆
zz −eΣzz is equal to
(I −K⋆)(Σxx −eΣxx)(I −K⋆T ) + K⋆(Σyy −eΣyy)K⋆T
+ K⋆(Σ⋆T
xy −eΣT
xy)(I −K⋆T ) + (I −K⋆)(Σ⋆
xy −eΣxy)K⋆T
⪰K⋆(Σ⋆T
xy −eΣT
xy)(I −K⋆T ) + (I −K⋆)(Σ⋆
xy −eΣxy)K⋆T
since Σxx ⪰eΣxx and Σyy ⪰eΣyy. Since trace is Sn
+-
nondecreasing, we get
tr(Σ⋆
zz −eΣzz) ≥2 tr

KT (I −K)(Σ⋆
xy −eΣxy)

≥0
Agent 1
50
100
150
200
250
300
0
2
4
6
t
Position error
RF
NF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CKF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CI
50
100
150
200
250
300
0
0.1
0.2
0.3
t
Velocity error
RF
CI
CKF
Agent 2
50
100
150
200
250
300
0
2
4
6
t
Position error
RF
NF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CKF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CI
50
100
150
200
250
300
0
0.1
0.2
0.3
t
Velocity error
RF
CI
CKF
Agent 3
50
100
150
200
250
300
0
2
4
6
t
Position error
RF
NF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CKF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CI
50
100
150
200
250
300
0
0.1
0.2
0.3
t
Velocity error
RF
CI
CKF
Agent 4
50
100
150
200
250
300
0
2
4
6
t
Position error
RF
NF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CKF
50
100
150
200
250
300
0
0.5
1
1.5
2
t
Position error
RF
CI
50
100
150
200
250
300
0
0.1
0.2
0.3
t
Velocity error
RF
CI
CKF
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
that eΣxy satisfies (10) is straightforward.
■
B. Formulas for computing the Newton steps
First of all, the differential of f1(Q) at the direction of
∆Q is given by
Df1(Q)[∆Q] = Σ−1/2
yy
∆QT Σ−1
xx Q + QT Σ−1
xx ∆Q


Σ−1/2
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
∇Q log det(−f1(Q)) = 2Σ−1
xx QΣ−1/2
yy
f1(Q)−1Σ−1/2
yy
(41)
Moreover, we have
∇Xf(X, Q) =2
C
D Σxx
Q
QT
Σyy
 CT
DT

+ Ση


X
−2(CΣxx + DQT )
(42)
and
∇Qf(X, Q) = 2(CT XXT D −XT D)
(43)
Let g1(Q) .= Σ−1/2
yy
f1(Q)−1Σ−1/2
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
Q
QT
Σyy
 CT
DT

+ Ση


∆Xnt
+2t(C∆QntDT X −D∆QT
nt(I −CT X)) = −∇Xft(X, Q)
(45)
and
2t(CT ∆XntXT D −(I −CT X)∆XT
ntD)
−2Σ−1
xx Qg1(Q)
∆QT
ntΣ−1
xx Q + QT Σ−1
xx ∆Qnt


g1(Q)
+2Σ−1
xx ∆Qntg1(Q) = −∇Qft(X, Q)
(46)
REFERENCES
[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, vol-
ume 6, pages 4398–4403. IEEE, 2001.
[2] Stephen Boyd and Lieven Vandenberghe. Convex optimization. Cam-
bridge university press, 2004.
[3] Luis C Carrillo-Arce, Esha D Nerurkar, Jos´e 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., Stan-
ford, CA, 2003.
[5] J. Hernandez, K. Tsotsos, and S. Soatto. Observability, identifiability
and sensitivity of vision-aided inertial navigation. In IEEE Interna-
tional 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 transporta-
tion systems magazine, 5(2):33–44, 2013.
[11] Agostino Martinelli. Improving the precision on multi robot local-
ization 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 con-
straint 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 linear-
complexity, consistent algorithm for slam. In 2007 IEEE/RSJ Interna-
tional 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 multi-
robot 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 covari-
ance 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.