A Game-Theoretic Approach to Robust Fusion and Kalman Filtering Under Unknown Correlations  Spyridon Leonardos 1   and Kostas Daniilidis 2  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 , 2 The 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    ̃ z   of   z   is another random vector. The associated error covariance is denoted    ̃ Σ zz  .  = Cov (  ̃ z   −   z ) . The pair   (  ̃ z,   Σ zz   )   is   consistent  if   E [  ̃ z ] =   z   and  Σ zz       ̃ Σ zz   (1)  Problem Statement   1 (Consistent fusion):   Given   two consistent   estimates   (  ̃ x,   Σ xx ) ,   (  ̃ 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   (  ̃ x,   Σ xx ) ,   (  ̃ y,   Σ yy   )   in a single consistent estimate   (  ̃ z,   Σ zz   ) , where    ̃ z   is of the form   ̃ z   =   W x    ̃ x   +   W y    ̃ y   (2) with   W x   +   W y   =   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       ̃ Σ xx ,   Σ yy       ̃ Σ yy   the Covariance Intersection equations read   ̃ z   = Σ zz  { ω Σ − 1  xx    ̃ x   + (1   −   ω )Σ − 1  yy    ̃ y }  Σ − 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 [  ̃ z ] =   z . Moreover, it is easy to check that   (  ̃ 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 relaxed requirements. Let   S n  +   denote the positive semidefinite cone, that is the set of   n   ×   n   positive semidefinite matrices. First, recall that a function   f   :   S n  +   →   R   is called   S n  + - nondecreasing   [2] if  X      Y   ⇒   f   ( X )   ≥   f   ( Y   )   (4) for any   X, Y   ∈   S n  + . 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   S n  + -nondecreasing function .  Definition 2.2 ( f   -Consistency):   Let   f   :   S n  +   →   R   be a nondecreasing function (with respect to   S n  + ) satisfying  f   ( 0 )   =   0 . Let   z   be a random vector with expectation  E [ z ] =   z   and    ̃ z   be an estimate of   z   with associated error covariance    ̃ Σ zz   . The pair   (  ̃ z,   Σ zz   )   is   f   -consistent   if   E [  ̃ z ] =   z  and  f   (Σ zz   )   ≥   f   (  ̃ Σ 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   (  ̃ x,   Σ xx ) ,   (  ̃ 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   (  ̃ x,   Σ xx ) ,   (  ̃ y,   Σ yy   )   in   a   single trace-consistent   estimate   (  ̃ z,   Σ zz   ) ,   where    ̃ z   is   a   linear combination of   x   and   y   and  tr(Σ zz   )   ≥   tr(  ̃ Σ 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   W x , W y   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   R m   ×   R n   is defined by a pay-off function   f   :   R m   ×   R n   →   R . Intuitively, the first player makes a move   u   ∈   R m   then, the second player makes a move   v   ∈   R m   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   (  ̃ x,   Σ xx ) ,   (  ̃ y,   Σ yy   )   of   z   where  Σ xx ,   Σ yy   are approximations to the true error covariances   ̃ Σ xx ,    ̃ Σ yy   . Based on the discussion of Section II, the fused estimate is of the form   ̃ z   = ( I   −   K )  ̃ x   +   K    ̃ y   (7) and the associated error covariance    ̃ Σ zz  .  = Cov (  ̃ z   −   z )   is given by   ̃ Σ zz   =   [ I   −   K   K ] [ ̃ Σ xx    ̃ Σ xy   ̃ Σ T xy    ̃ Σ yy  ] [ I   −   K T  K T  ]  (8) However,    ̃ Σ xx ,    ̃ Σ yy   are not known. Therefore, we define  Σ zz  .  =   [ I   −   K   K ] [ Σ xx   Σ xy  Σ T xy   Σ yy  ] [ I   −   K T  K T  ]  (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   ̃ z   = ( I   −   K ? )  ̃ x   +   K ?    ̃ y  Σ ? 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   (  ̃ x,   Σ xx )   and   (  ̃ y,   Σ yy   )   are consistent, then the pair   (  ̃ 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 [ x ] =   x   and   E [ y ] =   y . We have some estimates    ̃ x   and    ̃ y  of   x   and   y   respectively with associated error covariances   ̃ Σ xx   and    ̃ Σ 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    ̃ x   and    ̃ y . As in the classic Kalman filter derivation, we are looking for an update step of the form   ̃ x +   =    ̃ x   +   K ( z   −    ̃ z )   (14) where    ̃ z .  =   C    ̃ x   +   D  ̃ y . The error of the update is given by   ̃ x +   −   x   = ( I   −   KC )(  ̃ x   −   x )   −   KD (  ̃ y   −   y ) +   Kη   (15) and the associated error covariance is defined as    ̃ Σ +  xx  .  = Cov (  ̃ x +   −   x )   and is given by   ̃ Σ +  xx   =   [ I   −   KC   − KD ] [ ̃ Σ xx    ̃ Σ xy   ̃ Σ T xy    ̃ Σ yy  ] [ I   −   C T   K T  − D T   K T  ]  +   K Σ η   K T  (16) However, the true error covariances    ̃ Σ xx   and    ̃ Σ yy   are not known. Therefore, we define  Σ +  xx  .  =   [ I   −   KC   − KD ] [ Σ xx   Σ xy  Σ T xy   Σ yy  ] [ I   −   C T   K T  − D T   K T  ]  +   K Σ η   K T  (17) where   Σ xy   should satisfy (10) in order to be a valid cross- correlation. To alleviate notation, let   X   =   K T   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   Q T   Σ − 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,   ̃ x +   = ( I   −   K ? C )  ̃ x   −   K ? D  ̃ y  Σ + ? xx   =   [ I   −   K ? C   − K ? D ] [ Σ xx   Σ ? xy  Σ ?T xy   Σ yy  ] [ I   −   C T   K ?T  − D T   K ?T  ]  +   K Σ η   K T  (20) Naturally, we have the following lemma.  Lemma 4.1:   If   (  ̃ x,   Σ xx )   and   (  ̃ y,   Σ yy   )   are consistent, then the pair   (  ̃ x + ,   Σ + ? 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  ∇ 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  u   maximize  u   f   ( u, v )   (23) Intuitively, at each step the directions   ∆ u nt ,   ∆ v nt   are the solutions of the first order approximation  0 =   r ( u +∆ u nt , v +∆ v nt )   ≈   r ( u, v )+ Dr ( u, v )[∆ u nt ,   ∆ v nt ]  (24)
where   r ( u, v ) = [ ∇ u f   ( u, v ) T   ,   ∇ v   f   ( 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   ∈   dom f   , 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  Dr ( u, v )[∆ u nt ,   ∆ v nt ] =   − r ( u, v )  3. Backtracking line search on   ‖ r ‖ 2 .  t   = 1 .  u t   =   u   +   t ∆ u nt ,   v t   =   v   +   t ∆ v nt .  While   ‖ r ( u t , v t ) ‖ 2   >   (1   −   αt ) ‖ r ( u, v ) ‖ 2  t   =   βt .  u t   =   u   +   t ∆ u nt ,   v t   =   v   +   t ∆ v nt .  EndWhile  4. Update:   u   =   u   +   t ∆ u nt ,   v   =   v   +   t ∆ v nt .  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   f t ( X, Q )   by  f t ( X, Q ) =   tf   ( X, Q ) + log det( − f 1 ( Q ))   (25) where   f   ( X, Q )   as defined in (18) and  f 1 ( Q ) = Σ − 1 / 2  yy   Q T   Σ − 1  xx   Q Σ − 1 / 2  yy   −   I   (26) Intuitively,   1  t   f t   approaches   f   as   t   → ∞ . Note that   f t ( X, Q )  is still convex-concave for   t >   0 . The optimality conditions for a fixed   t >   0   are given by  ∇ X   f t ( X ? , Q ? ) = 0 ,   ∇ Q f t ( X ? , Q ? ) = 0   (27) where explicit expressions for   ∇ X   f t   and   ∇ Q f t   are presented in AppendixVIII-B along with the linear equations for com- puting   ∆ X nt ,   ∆ Q nt   . Finally, the structure of the problem allows us to easily identify a strictly feasible initial point   ( X 0 , Q 0 )   where   Q 0   = 0   and   X 0   is given by  ( C Σ xx C T   +   D Σ yy   D T   + Σ η   ) X 0   =   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 )[ h x , h y   ]   denotes the   differential   of   f   at   ( x, y )   along  ( h x , h y   ) . 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    ̃ x   =    ̃ y   =   [ 0   0 ] T   and covariances  Σ xx   =  [ 5   0 0   5  ]  ,   Σ yy   =  [ 3   0 0   7  ]  (29) Let   (  ̃ z CI   ,   Σ CI   )   and   (  ̃ z RF   ,   Σ RF   )   be the fused estimates and the corresponding error covariances obtained from Covari- ance Intersection and Robust Fusion. We have that    ̃ z CI   =   ̃ z RF   = [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   :   x T   Σ − 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 two ellipsoids   { x   :   x T   Σ − 1  xx   x   ≤   1 }   and   { x   :   x T   Σ − 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   ̃ x   =  [ 0 0  ]  ,   Σ xx   =  [ 5   0 0   5  ]  (31) and   C   =   [ 1   0 ] ,   z   =    ̃ z   = 0 ,   Σ yy   = 1 ,   D   = 1   and   Σ η   = 0 . Both Covariance Intersection and Robust Fusion yield    ̃ z +   = 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  [ x i ( t   + 1)  v i ( t   + 1)  ]  =  [ I   I  0   I  ] [ x i ( t )  v i ( t )  ]  +  [ 0  I  ]  w i ( t )   (33) where   x i ( t )   ∈   R 2   and   v i ( t )   ∈   R 2   denote respectively the position and velocity   of agent   i   at time   instance   t  and   w i ( t )   ∼ N   (0 , Q i ( t ))   is the noise process. If   x i ( t )   .  =  [ x i ( t ) T   v i ( t ) T   ] T   , then let   A, B   such that  x i ( t   + 1) =   A x i ( t ) +   Bw i ( t )   (34) Only agent   1   is equipped with global position system (GPS), that is we have a measurement of the form  y 1 ( t ) =   x 1 ( t ) +   η 1 ( t )   (35) where   η 1 ( t )   ∼   N   (0 , R 1 ) . 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  y ij   ( t ) =   x j   ( t )   −   x i ( t ) +   η ij   ( t )   (36) where   η ij   ( t )   ∼ N   (0 , R ij   ) . 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   ̃ x i ( t   + 1 | t ) =   A  ̃ x i ( t | t )   (37)  Σ i ( t   + 1 | t ) =   A Σ i ( t   + 1 | t ) A T   +   BQ i ( t ) B T   (38) where    ̃ x i ( 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:   Q i   = 10 − 6 I 2   for all agents,   R 1   =   I 2   and   R ij   = 10 − 2 I 2   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 P OSITION 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 [  ̃ x ] =   E [  ̃ y ] =   z   then   E [  ̃ z ] =  z . Now, one has to show that if   Σ xx       ̃ Σ xx   and   Σ yy       ̃ Σ yy  then   tr(Σ ? zz   )   ≥   tr(  ̃ Σ zz   ) . We have that   Σ ? zz   −    ̃ Σ zz   is equal to  ( I   −   K ? )(Σ xx   −    ̃ Σ xx )( I   −   K ?T   ) +   K ? (Σ yy   −    ̃ Σ yy   ) K ?T  +   K ? (Σ ?T xy   −    ̃ Σ T xy   )( I   −   K ?T   ) + ( I   −   K ? )(Σ ? xy   −    ̃ Σ xy   ) K ?T     K ? (Σ ?T xy   −    ̃ Σ T xy   )( I   −   K ?T   ) + ( I   −   K ? )(Σ ? xy   −    ̃ Σ xy   ) K ?T  since   Σ xx       ̃ Σ xx   and   Σ yy       ̃ Σ yy   . Since trace is   S n  + - nondecreasing, we get  tr(Σ ? zz   −    ̃ Σ zz   )   ≥   2 tr  (  K T   ( I   −   K )(Σ ? xy   −    ̃ Σ 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    ̃ Σ xy   satisfies (10) is straightforward.     B. Formulas for computing the Newton steps  First of all, the differential of   f 1 ( Q )   at the direction of  ∆ Q   is given by  Df 1 ( Q )[∆ Q ] = Σ − 1 / 2  yy  ( ∆ Q T   Σ − 1  xx   Q   +   Q T   Σ − 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( − f 1 ( Q )) = 2Σ − 1  xx   Q Σ − 1 / 2  yy   f 1 ( Q ) − 1 Σ − 1 / 2  yy   (41) Moreover, we have  ∇ X   f   ( X, Q ) =2 ([ C   D ] [ Σ xx   Q Q T   Σ yy  ] [ C T  D T  ]  + Σ η  ) X  − 2( C Σ xx   +   DQ T   )  (42) and  ∇ Q f   ( X, Q ) = 2( C T   XX T   D   −   X T   D )   (43) Let   g 1 ( Q )   .  = Σ − 1 / 2  yy   f 1 ( 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   (∆ X nt ,   ∆ Q nt ) :  2 t ([ C   D ] [ Σ xx   Q Q T   Σ yy  ] [ C T  D T  ]  + Σ η  ) ∆ X nt  +2 t ( C ∆ Q nt D T   X   −   D ∆ Q T nt ( I   −   C T   X )) =   −∇ X   f t ( X, Q )  (45) and  2 t ( C T   ∆ X nt X T   D   −   ( I   −   C T   X )∆ X T nt D )  − 2Σ − 1  xx   Qg 1 ( Q ) (   ∆ Q T nt Σ − 1  xx   Q   +   Q T   Σ − 1  xx   ∆ Q nt  ) g 1 ( Q ) +2Σ − 1  xx   ∆ Q nt g 1 ( Q ) =   −∇ Q f t ( 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 , 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.