Performance Bounds for Cooperative Simultaneous

Localization and Mapping (C-SLAM)

Anastasios I. Mourikis and Stergios I. Roumeliotis
Dept. of Computer Science & Engineering, University of Minnesota, Minneapolis, MN 55455
Email: {mourikis|stergios}@cs.umn.edu

Abstract In this paper we study the time evolution of the to localize while concurrently building a map of the environ-
position estimates covariance in Cooperative Simultaneous Lo- ment, in which case the uncertainty in their position estimates
calization and Mapping (C-SLAM), and obtain analytical upper remains bounded [4]. This introduces the problem of Coopera-
bounds for the positioning uncertainty. The derived bounds
provide descriptions of the asymptotic positioning performance tive Simultaneous Localization And Mapping (C-SLAM) that
of a team of robots in a mapping task, as a function of the has recently attracted the interest of many researchers. The
characteristics of the proprioceptive and exteroceptive sensors of number of potential applications that require robots to perform
the robots, and of the graph of relative position measurements C-SLAM is continuously growing, as autonomous vehicles are
recorded by the robots. A study of the properties of the Riccati employed for tasks ranging from planetary exploration and
recursion which describes the propagation of uncertainty through
time, yields (i) the guaranteed accuracy for a robot team in a environmental monitoring, to construction and transportation.
given C-SLAM application, as well as (ii) the maximum expected In this work, C-SLAM is considered within the Stochastic
steady-state uncertainty of the robots and landmarks, when the Mapping framework [5], [6]. We assume that the mobile robots
spatial distribution of features in the environment can be modeled move continuously and randomly in a planar environment,
by a known distribution. The theoretical results are validated by while recording measurements of the relative positions (i.e.,
simulation experiments.
range and bearing) of other robots in the team, and of point
landmarks that exist in the environment. A means of describing
I. I NTRODUCTION
the exteroceptive measurements that are recorded at each
In order for a multirobot team to coordinate while navigat- time step is the associated Relative Position Measurement
ing autonomously within an area, all robots must be able to Graph (RPMG), i.e., the graph whose vertices represent the
determine their positions with respect to a common frame of robots and landmarks, while its directed edges correspond to
reference. In an ideal scenario, each robot would have direct the robot-to-robot and robot-to-landmark measurements (cf.
access to measurements of its absolute position, such as those Fig. 1(a)). We impose the constraint that the RPMG is a
provided by a GPS receiver, or those inferred by detecting connected graph, i.e., that there exists a path between any
previously mapped features. However, reliance on GPS is two of its nodes. This constraint arises naturally and is not
not feasible in a number of situations, since GPS signals a restrictive one, since if an RPMG is not connected, then it
are not available everywhere (e.g., indoors), or, triangulation can always be decomposed into smaller, connected sub-graphs.
techniques based on them may provide erroneous results due Each of these sub-graphs corresponds to an isolated group of
to multiple reflections (e.g., in the vicinity of tall structures robots and/or landmarks, whose position estimation problem
and buildings). Moreover, compiling a detailed map of the can be studied independently.
environment is a tedious and time-consuming process, while This paper presents the first derivation of analytical upper
numerous applications require robots to operate in unknown bounds on the positioning uncertainty during C-SLAM for
surroundings, whose structure cannot be determined in ad- a possibly heterogeneous team of mobile robots navigating
vance. This suggests that additional means are required for within a 2D environment populated with point features. The
aiding odometry when groups of mobile robots localize. metric used to describe the localization uncertainty is the
In situations where absolute position information is not covariance matrix associated with the errors in the position
available, the robots of a team can still improve their local- estimates for the robots and the mapped features. The closed-
ization accuracy, by recording robot-to-robot relative position form expression of Lemma 4.3 establishes the guaranteed
measurements, and processing them in order to update their accuracy attainable by a robot team in a given mapping
position estimates (e.g., [1], [2], [3]). This method results in application, as a functional relation of the noise parameters
a substantial improvement in estimation accuracy, compared of the robots sensors and the topology of the RPMG. Fur-
to simple Dead-Reckoning localization schemes. However thermore, the result of Lemma 4.5 demonstrates how prior
performing Cooperative Localization (CL) solely based on information about the spatial density of landmarks can be
relative position measurements has the limitation that the utilized in order to compute a tight upper bound on the
uncertainty of the robots position estimates continuously expected covariance of the positioning errors. To the best
increases, and the attained accuracy may not be sufficient for of our knowledge, the proposed bounds constitute the only
certain applications. An alternative approach is for the robots existing analytical tools for predicting the mapping precision,
as well as the accuracy of the robots localization in a given C- Under the additional assumption that the robots receive noise-
SLAM application. Hence, they facilitate the selection of the free odometry measurements, it is proven that at steady state,
required sensor parameters, in order to satisfy task-imposed all of the vehicle and feature position estimates become fully
performance constraints. correlated and lower bounds for the covariance of all vehicles
and features are derived. However, the proposed lower bounds
II. R ELATED W ORK cannot be employed for determining the performance of C-
Most of the existing approaches to both single- and multi- SLAM in the case of robots in motion exploring an unknown
robot SLAM have been inspired by the seminal papers of area. In such a scenario, the global coordinate frame can be
Moutarlier and Chatila [6] and of Smith, Self, and Cheese- arbitrarily defined, thus at least one of the robots has perfect
man [5], [7] that introduced the notion of the Stochastic Map knowledge of its initial position, and the described lower
and emphasized the importance of properly accounting for the bound reduces to zero.
correlations between all the robots and landmarks position The main contribution of the work presented in this paper is
estimates. However, maintaining all the cross-correlation ele- the characterization of the steady-state accuracy of the position
ments of the covariance matrix in EKF-based SLAM results in estimates in C-SLAM. This is achieved by deriving analytical
algorithms with computational complexity quadratic in the size upper bounds for the worst-case value as well as for the
of the state vector. Thus, the majority of subsequent research expected value of the steady-state covariance matrix of the
on SLAM has focused on devising scalable algorithms, that position estimates. What distinguishes these results from previ-
achieve performance comparable to that of an EKF-based ous ones is that the analysis is based on the actual (non-linear)
approach to SLAM that accounts for the cross-correlation system and measurement equations for robots navigating in
terms, at a smaller computational cost. 2D. Besides the naturally arising assumption of connectedness,
A number of estimation algorithms have been proposed no additional assumptions on the structure of the RPMG
specifically for the C-SLAM problem, as alternatives to the are imposed. Furthermore, the analysis encompasses the case
EKF estimator [8]. In [9], a Set Membership (SM) technique of a heterogeneous (in terms of sensor accuracy) group of
is developed based on the premise that all sensor errors are robots and is thus applicable to the study of a broad class of
bounded. This assumption allows for the definition of the set applications.
of feasible state vectors, which is propagated through time
using Linear Programming and set approximation methods. III. P ROBLEM F ORMULATION
SM provides guaranteed uncertainty regions for all robots Consider a group of M mobile robots, denoted as r1 ,
and landmarks at each time step. In the work of Thrun [10], r2 , ..., rM , moving on a planar surface, in an environment
an algorithm that integrates Maximum Likelihood (ML) in- that contains N landmarks, denoted as L1 , L2 , ..., LN . The
cremental map building with Monte Carlo localization is robots use proprioceptive measurements (e.g., from odometric
proposed. The pose of the robots is propagated using a or inertial sensors) to propagate their state (position) estimates,
particle filter representation of their belief functions, while and are equipped with exteroceptive sensors (e.g., laser range
the map is compiled in a distributed manner among robots, finders) that enable them to measure the relative position of
using laser range data. In [11], the constrained local submap other robots and landmarks. All the measurements are fused
filter is employed, enabling each vehicle to build a map of using an Extended Kalman Filter (EKF) in order to produce
its surroundings independently from the rest of the team. estimates of the position of the robots and the landmarks. In
In this case, a global map is created by periodically fusing our formulation, it is assumed that an upper bound for the
the vehicles submaps. In [12], [13], an elevation map of an variance of the errors in the robots orientation estimates can
outdoor area is created using multiple robots that localize be determined a priori. This allows us to decouple the task
in a common coordinate frame. Finally, in [14] a manifold of position estimation from that of orientation estimation and
representation of 2D space, and a ML estimator are employed. facilitates the derivation of an analytical upper bound on the
This approach offers a method for alleviating the problem of positioning uncertainty.
map inconsistency in environments containing loops, at the The robots orientation uncertainty is bounded when, for
expense of increased complexity. example, absolute orientation measurements from a compass
Our work does not address the aforementioned implemen- or sun sensor are available, or when the perpendicularity of
tation issues of C-SLAM. We assume perfect data association the walls in an indoor environment is used to infer orientation.
and seek to characterize the theoretically attainable estimation In cases where neither approach is possible, our analysis still
accuracy by providing bounds for the steady state covariance holds under the condition that a conservative upper bound
of the position estimates. To the best of our knowledge, the for the orientation uncertainty of each robot is determined by
properties of the estimates covariance matrix in C-SLAM are alternative means, e.g., by estimating the maximum orientation
only studied in the work of Fenwick et al. [4], [8]. Linear time- error accumulated, over a certain period of time, due to the
invariant models for both the propagation and measurement integration of noise in the odometric measurements [15]. It
equations are employed, and it is shown that the determinant should be noted that the requirement for bounded orientation
of any principal submatrix of the maps covariance matrix error covariance is not too restrictive: In the EKF framework,
decreases monotonically as successive observations are made. the nonlinear state propagation and measurement equations
are linearized around the estimates of the robots orientation. From Eq. (3), we deduce that the covariance matrix of the
If the errors in these estimates are allowed to increase un- system noise affecting the i-th robot is:
bounded, the linearization will unavoidably become erroneous
and the estimates will diverge. Furthermore, large errors in Qri (k) = E{Gri (k)Wi (k)WiT (k)GTri (k)}
2 2
the estimates for the robots orientation in SLAM result in t Vi 0
= C(i (k)) C T (i (k)) (4)
erroneous data association, that may have detrimental effects 0 t2 Vm2 i (k)2 i
on the filter stability. Thus, in the vast majority of practical
situations, provisions are made in order to constrain the robots where C(i ) denotes the 2 2 rotation matrix associated with
orientation uncertainty within given limits. i .The landmarks are modeled as static points in 2D space,
In our formulation, the metric employed for describing the and therefore the state propagation equations are
accuracy of position estimation in C-SLAM is the covariance XLi (k + 1) = XLi (k), for i = 1 . . . N
matrix of the position estimates. It is well known that the time
evolution of the covariance matrix in the EKF framework is Similarly, the estimates for the landmark positions are prop-
described by the propagation and update equations (cf. Eqs. (5) agated using the relations X L k+1|k = X L k|k , for i =
i i
and (16)). Combining these equations yields the Riccati re- 1 . . . N while the errors are propagated by X eL k+1|k =
i
cursion (cf. Eq. (17)), whose solution is the covariance of e
XLi k|k , for i = 1 . . . N . The state vector for the entire
the error in the state estimate at each time step, right after system is defined as the stacked vector comprising of the
the propagation phase of the EKF. In the case of C-SLAM, positions of the M robots and N landmarks, i.e.,
the matrix coefficients in this recursion are time-varying T
and a general closed-form expression for the time evolution X = XrT1 XrTM XLT1 XLTN
of the covariance matrix does not exist. We thus resort to
deriving upper bounds for the covariance, by exploiting the Hence, the state transition matrix for the entire system at time
convexity and monotonicity properties of the Riccati recursion step k is k = I(2M +2N )(2M +2N ) and the covariance matrix
(cf. Lemmas 4.1 and 4.4). These properties allow for the of the system noise is:

formulation of constant coefficient Riccati recursions, whose Qr1 (k) 022
solutions provide upper bounds for the positioning uncertainty .. .. ..
. 02M 2N
in C-SLAM. Q(k) = . . = GQr (k)GT
022 QrM (k)
A. Position propagation 02N 2M 02N 2N
The discrete-time kinematic equations for the i-th robot are
where Qr (k) = Diag (Qri (k)), with Diag() denoting a block
xri (k + 1) = xri (k) + Vi (k)t cos(i (k)) (1) diagonal matrix, and

yri (k + 1) = yri (k) + Vi (k)t sin(i (k)) (2) I2M 2M
G=
where Vi (k) denotes the robots translational velocity at time k 02N 2M
and t is the sampling period. In the Kalman filter framework, The equation for propagating the covariance matrix of the state
the estimates of the robots position are propagated using error is written as:
the measurements of the robots velocity, Vmi (k), and the
estimates of the robots orientation, i (k). By linearizing Pk+1|k = Pk|k + Q(k) = Pk|k + GQr (k)GT (5)
Eqs. (1) and (2), the error propagation equation for the robots ek+1|k XeT
position is derived: where Pk+1|k = E{X k+1|k } and Pk|k =
" # " # E{Xek|k Xe T } are the covariance of the error in the estimate
k|k
x
erik+1|k 1 0 xerik|k of X (k + 1) and X (k) respectively, after measurements up to
=
yerik+1|k 0 1 yerik|k time k have been processed.

t cos(i (k)) Vmi (k)t sin(i (k)) wVi (k) B. Measurement Model
+
t sin(i (k)) Vmi (k)t cos(i (k)) ei (k)
At every time step, the robots perform robot-to-robot and
X er er
= I22 X + Gr (k)Wi (k), i = 1 . . . M (3) robot-to-landmark relative position measurements. The relative
ik+1|k ik|k i

position measurement between robots ri and rm is given by:

where1 wVi (k) is a zero-mean white Gaussian noise sequence
of variance V2 i , affecting the velocity measurements and ei (k) zri rm = C T (i ) (Xrm Xri ) + nzri rm (6)
is the error in the robots orientation estimate at time k. This
is modeled as a zero-mean white Gaussian noise sequence of where ri (rm ) is the observing (observed) robot, and nzri rm
variance 2 i . is the noise affecting this measurement. Similarly, the mea-
surement of the relative position between ri and Ln is given
1 Throughout this paper, 0
mn denotes the m n matrix of zeros, 1mn
by:
denotes the m n matrix of ones, and Inn denotes the n n identity
matrix. zri Ln = C T (i ) (XLn Xri ) + nzri Ln (7)
The similarity of the preceding two measurement equations position measurements recorded by this robot, and renders
allows us to treat both types of measurements in a uniform them correlated. As shown in [16], the additional covariance
manner. We denote by Tij the target of the j-th measurement term for each measurement is equal to:
performed by robot i, i.e., T
Rij (k + 1) = c ij E{i 2 }p
C T (i )J p c ij J T C(i )
Tij {r1 , r2 , , rM , L1 , L2 , , LN } \ {ri }
T
= 2 i C T (i )J p c ij J T C(i )
c ij p (11)
Thus, the general form of the relative position measurement
equation is: while the matrix of correlation between the errors in the

zij = C T (i ) XTij Xri + nzij (8) measurements zij (k + 1) and zi` (k + 1) is:
i
Assuming that the i-th robot performs Mi relative position Rj` (k + 1) = ij (k)E{nij (k + 1)nTi` (k + 1)}Ti` (k)
measurements, the index j assumes integer values in the range T
= 2 i C T (i )J p c J T C(i )
c ij p
i` (12)
[1, Mi ] to describe these measurements. By linearizing the last
expression, the measurement error equation is obtained: Since the i-th robot performs Mi relative position measure-
zeij (k + 1) = zij (k + 1) zij (k + 1) ments at each time step, the covariance matrix of these
measurements, Ri (k + 1), is defined as a block matrix whose
= Hij (k + 1)X ek+1|k + ij (k + 1)nij (k + 1)
`m-th 2 2 submatrix is i R`m (k + 1), for `, m = 1 . . . Mi ,
where defined in Eqs. (10)-(12). Substitution from these equations
and simple algebraic manipulation yields
Hij (k + 1) = C T (i (k + 1)) Hoij (9)
" # Ri (k + 1) = Ti Roi (k + 1)i (13)
022 . . . I22 . . . I22 . . . 022
Hoij = | {z } |{z}
ri Tij where i = IMi Mi C(i ), with denoting the Kronecker
h iT matrix product, and
ek+1|k =
X erT
X eT
X !
i Tij
k+1|k 2
2i
h i Roi (k + 1) = i I2Mi 2Mi Di diag DiT
ij (k) = I22 C (i (k + 1))J p
T c ij (k + 1) 2ij
+ 2i Di DiT + 2 i Di 1Mi Mi DiT (14)
0 1 nzij (k)
J= , nij (k) =
1 0 ei (k) c ij is the block
In this last expression, Di = Diag J p
c ij (k + 1) = X
p bT X br
ij k+1|k i k+1|k
diagonal matrix with diagonal elements J p c ij , j = 1 . . . Mi .
The covariance of the error in this measurement is given by The measurement matrix describing the relative position
i measurements performed by robot i at each time step is a
Rjj (k + 1) = ij (k + 1)E{nij (k + 1)nTij (k + 1)}Tij (k + 1)
matrix whose block rows are Hij , j = 1 . . . Mi , i.e.:
= Rzij (k + 1) + Rij (k + 1) (10)
H i (k + 1) = Ti H oi (15)
This expression encapsulates all sources of noise and uncer-
tainty that contribute to the measurement error zeij (k + 1). More where H oi is a constant matrix with block rows Hoij , j =
specifically, Rzij (k + 1) is the covariance component attributed 1 . . . Mi (cf. Eq. (9)).
to the measurement noise nzij (k + 1), and Rij (k + 1) is the The measurement matrix for the entire system, H(k + 1),
additional covariance term due to the error i (k + 1) in the is defined as the block matrix with block rows H i (k + 1).
orientation estimate of the measuring robot. Since the measurements performed by different robots are
Assuming that each relative position measurement is com- independent, the associated covariance matrix, R(k + 1), is a
prised of a distance measurement ij and a bearing mea- block diagonal matrix with elements Ri (k + 1). The covariance
surement ij , affected by two independent white zero-mean update equation of the EKF is written as
Gaussian noise sequences nij and nij respectively, the term
Rzij (k + 1) can be expressed as [16]: Pk+1|k+1 = Pk+1|k Pk+1|k HT (k + 1)S1 H(k + 1)Pk+1|k

Rzij (k + 1) = E{nzij (k + 1)nTzij (k + 1)} with S = H(k + 1)Pk+1|k HT (k + 1) + R(k + 1). Substitution
! from Eqs. (13) and (15) and simple algebraic manipulation
T
2i T T T
= C (i ) c c 2 c c
pij pij + i J pij pij J C(i ) leads to the orientation-dependent terms being cancelled out,
2ij and yields the expression
where time indices have been dropped for simplicity. The vari- Pk+1|k+1 = Pk+1|k Pk+1|k HTo S1
o Ho Pk+1|k (16)
ances of the noise in the distance and bearing measurements
are denoted as 2i = E{n2i } and 2i = E{n2i } respectively. with So = Ho Pk+1|k HTo + Ro (k + 1). In these equations Ho
The error in the orientation estimate of the measuring is a matrix whose block rows are H oi while Ro is a block
robot introduces an additional error component to all relative diagonal matrix with elements Roi .
IV. SLAM P OSITIONING ACCURACY C HARACTERIZATION An upper bound for Ro (k + 1) can be derived by consid-
The time evolution of the covariance matrix of the position ering the maximum distance, oi , at which relative position
estimates in C-SLAM is described by the Riccati recursion, measurements can be recorded by robot i. This distance can,
which can be derived by substituting the expression from for example, be determined by the maximum range of the
Eq. (16) into Eq. (5). The resulting expression is: robots relative position sensors, or, by the size of the area to
be mapped. It can be shown that [16]
1
Pk+1 = Pk Pk HTo Ho Pk HTo + Ro (k + 1) Ho Pk
Ri (k + 1) 2i + Mi 2 i 2o + 2i 2o I2Mi 2Mi = ri I2Mi 2Mi
+ GQr (k + 1)GT (17)
and thus an upper bound on Ro (k + 1) is computed as
where we use the substitutions Pk = Pk+1|k and Pk+1 =
Pk+2|k+1 to simplify the notation. We note that the matrices Ro (k + 1) Diag (ri I2Mi 2Mi ) = Ru
Qr (k + 1) and Ro (k + 1) in this Riccati recursion are time
Having derived upper bounds for Qr (k) and Ro (k + 1), mere
varying, and thus a closed form expression for P cannot
substitution in Eq. (19) and numerical evaluation of the
be derived for the general case. However, by exploiting the
solution to the resulting recursion, yields an upper bound on
monotonicity and concavity properties of the Riccati recursion,
the maximum possible uncertainty of the position estimates
we are able to derive upper bounds for the worst-case value, as
in C-SLAM, at any time instant after the deployment of the
well as for the average value of the covariance matrix at steady
robot team. However, for many applications it is important
state. At this point we note that in the ensuing derivations, the
to evaluate the performance of SLAM at steady state, i.e.,
initial covariance matrix of the position estimates is assumed
when the covariance of the map has converged to a constant
to be equal to
value ([4]). To this end, it is necessary to evaluate the solution
Prr0 02M 2N to the recursion in Eq. (19) after sufficient time, i.e., as
P0 = (18)
02N 2M PLL0 k . This computation is simplified by employing the
following Lemma, adapted from [17]:
i.e., we assume that the position estimates for the robots and
the map features are initially uncorrelated, which is the case u(0)
Lemma 4.2: Suppose Pk is the solution to the discrete
at the onset of a mapping task within an unknown area.
time Riccati recursion in Eq. (19) with initial value Pu0 = 0.
A. Bound on Worst-Case Steady State Covariance Then the solution to the same Riccati recursion but with an
arbitrary initial condition P0 is given by the identity
In this section, we derive an upper bound for the steady
u(0) 1
state covariance matrix in C-SLAM. It can be shown that the Puk+1 Pk+1 = (0, k + 1) (I + P0 Jk+1 ) P0 T (0, k + 1)
right-hand side of Eq. (17) is a matrix-increasing function of
the covariance matrices Q(k + 1) and Ro (k + 1), as well as of where = 2M + 2N , and (0, k + 1) is given by
the state covariance matrix Pk . These properties allow us to
(0, k + 1) = (I Kp Ho )k+1 (I + PJk+1 )
prove the following lemma [16]:
In these expressions, P is any solution to the Discrete Alge-
Lemma 4.1: If Ru and Qru are matrices such that Ru braic Riccati Equation (DARE):
Ro (k) and Qru Qr (k), for all k 0, then the solution to
the Riccati recursion P = P + GQru GT PHTo (Ho PHTo + Ru )1 Ho P,
1 1
Puk+1 = Puk Puk HTo Ho Puk HTo + Ru Ho Puk and Kp = PHTo Ru + Ho PHTo . Jk denotes the solution
+ GQru GT (19) to the dual Riccati recursion:

with the initial condition Pu0 = P0 , satisfies Puk Pk for all Jk+1 = Jk Jk G(Q1 T
ru + G Jk G) G Jk + HTo R1
1 T
u Ho
k 0. with zero initial condition, J0 = 0.

In order to derive an upper bound for Qr (k), we note Lemma 4.2 simplifies the evaluation of the steady-state value
that since C(i ) is an orthonormal matrix, the eigenvalues of Puk , since the solution to the Riccati recursion with zero
of Qri (k) are equal to t2 V2 i and t2 Vm2 i (k)2 i (cf. Eq. (4)). initial condition is easily derived. We note that the Riccati
Assuming that the velocity of each robot is approximately recursion in Eq. (19) describes the time evolution of the
constant and equal to Vi , we denote covariance for a deduced C-SLAM scenario, in which both the
robots kinematic equations and the measurement equations
qi = max t2 V2 i , t2 Vm2 i (k)2 i ' max t2 V2 i , t2 Vi2 2 i
(20) are time invariant. Zero initial covariance of the landmarks
This definition states that qi is the largest eigenvalue of position estimates, corresponds to a perfectly known map. In
Qri (k + 1), and therefore this case the robots essentially perform cooperative localiza-
tion, while the robot-to-landmark measurements are equivalent
Qri (k) qi I22 Qr (k) Diag (qi I22 ) = Qru (21) to absolute position measurements. Thus, the resulting system
is observable and the steady-state solution to the Riccati the area. However, when considering the type of features
equation is: of the environment to be treated as landmarks (e.g., visual
features, prominent geometric features), it is beneficial to
u(0) Purr 02M 2N
P = (22) select them so that they are abundant in the environment and
02N 2M 02N 2N
evenly distributed throughout it. This way, a more detailed
with and accurate map of an area can be created. In such cases, the
r
1 1 1 density of landmarks in the environment can be modeled a
Purr = Q1/2
ru U diag + + UT Q1/2
ru priori, for example, by a uniform probability density function
2 4 i
(pdf). Knowledge of the distribution of the relative positions
In the last expression the quantities U and i , i = 1 . . . 2M between the robots and landmarks allows us to compute the
are defined as the matrix of eigenvectors and the eigenvalues average value of the matrix Ro (k + 1). This information can
1/2 1/2
of C = Qru Ir Qru respectively, where be exploited in order to compute a tighter upper bound for the

I2M 2M expected steady state covariance of the position estimates.
Ir = [I2M 2M 02M 2N ] HTo R1 u H o Specifically, it can be shown that the right hand side
02N 2M
of Eq. (17) is a concave function of the matrices Pk and
It is interesting to note that HTo R1
u Ho represents the informa- Ro (k + 1). This property enables us to employ Jensens
tion matrix associated with the measurements in the deduced inequality ([18]) to prove, by induction, the following
linear time-invariant system and Ir is the submatrix expressing lemma [16]:
the information about the robots positions.
The rest of the derivations for the upper bound on the steady Lemma 4.4: If R = E{Ro (k)} and Q r = E{Qr (k)} then
state covariance matrix involve only algebraic manipulations, the solution to the following Riccati recursion
and are not included here due to space limitations. The
k+1 = P
P k P k HT Ho P
k HT + R 1 Ho P
k
interested reader is referred to [16] for the details of the o o
intermediate steps. The final result is stated in the following + GQ rG T
(25)
lemma: 0 = P0 , satisfies P
k E{Pk } for
with initial condition P
all k 0.
Lemma 4.3: The worst-case covariance matrix in C-SLAM
is bounded above by the matrix
The average value of the system noise covariance matrix is
Pu = Pu(0)
+ 1(M +N )(M +N )
1
(23) easily computed by averaging over all values of orientation of
u(0)
the robots:
where P is defined in Eq. (22) and
t2 V2 i + t2 Vi2 2 i
E{Qri } = I22 = qi I22 (26)
= (11N I22 ) P1
LL0 (1N 1 I22 ) 2
1 1
+ (11M I22 ) Jrr + Prr0 (1M 1 I22 ) and thus
r = E{Qr (k + 1)} = Diag (
Q qi I22 ) (27)
with
r !
i 2i In order to evaluate the expected value of Ro (k + 1), we
Jrr = Q1/2
ru U diag + + i UT Q1/2
ru (24) assume a uniform distribution for the positions of the robots
2 4
and landmarks in a rectangular area of side . Using the
definition of Roi (k + 1) in Eq. (14), it can be shown that [16]
Note that the first term in Eq. (23) depends only on the
2 2 2
RPMG and the accuracy of the robots sensors, while the i = E{Ro } = 2 + 2 + 2 I2M 2M
R i i i i i i
second term encapsulates the effect of the initial uncertainty. 2 6 12
A case of particular interest in C-SLAM is that of a robot 2
team building a map of an area for which no prior knowledge + 2 i 12Mi 2Mi
12
exists. We model this scenario by setting PLL0 = I, with i ). At this point, we note
and thus R = E{Ro } = Diag(R
, which yields the following simplified expression for
that the uniform distribution employed in the calculation of
matrix : was deemed an appropriate model for the positions of the
R,
1
= (11M I22 ) J1 rr + Prr0 (1M 1 I22 ) robots and landmarks in the simulation experiments presented
in Section V. However, the analysis holds for any pdf. If a
B. Bound on the average steady state covariance different pdf is used, the value of R will not, in general, be
The expression in Eq. (23) provides an upper bound on given by the preceding expression.
the covariance matrix of C-SLAM for a robot team with The upper bound for the expected steady-state covariance
a given set of sensors and a known RPMG. This bound can be computed by evaluating the solution to the recursion in
holds under any possible configuration of the landmarks in Eq. (25) after sufficient time. The derivation process followed
space, and regardless of the trajectories of the robots within is analogous to the one presented in the previous section. The
 and Q
only difference is that matrices R r , instead of Ru and exteroceptive measurements provide only a small amount of
Qru , respectively, are used. The final result is synopsized in positioning information during the crucial first few updates. In
the following lemma: Fig. 1(c), the time evolution of the covariance of the position
estimates for the robots and landmarks is shown and compared
Lemma 4.5: The expected steady-state covariance of the to the theoretically-derived steady-state performance bound.
position estimates in C-SLAM, when the spatial density of Clearly, the upper bound is indeed larger than the steady-
landmarks is described by a known pdf, is bounded above by state covariance of the landmarks and robots. It is also worth
the matrix noting that the covariance of the position estimates converges

P rr 02M 2N to the same value for all landmarks, while the accuracy of
P = 1
+ 1(M +N )(M +N )
02N 2M 02N 2N the position estimates varies between robots. These differences
result from the non-symmetric topology of the RPMG, which
with r
1 1 1 causes each robot to have access to positioning information of
rr = Q
P 1/2 U
diag + + TQ
U 1/2
r
2 4 r different quality.
i
and Although the bound of Lemma 4.3 accounts for the worst-
= case accuracy of C-SLAM, it does not yield a sufficient per-
(11N I22 ) P1
LL0 (1N 1 I22 )
1 1 formance description when the map features are more evenly
+ (11M I22 ) J
rr + Prr0 (1M 1 I22 ) distributed in space. In such cases, employing Lemma 4.5 re-
sults in a tighter bound on the average positioning uncertainty,
The quantity Jrr appearing in this last expression is:
r ! as demonstrated in Fig. 2. In this plot, the average values (over
i
2
50 runs) of the covariance in C-SLAM, are compared against

Jrr = Qr 1/2
U diag + i
+ i U TQ 1/2 (28)
r the theoretically derived bounds on the expected uncertainty.
2 4
For each run of the algorithm, the locations of the landmarks,
where U and i , i = 1 . . . 2M are defined based on the as well as the initial positions for the robots, were selected
singular value decomposition of the matrix Ir : using samples from a uniform distribution. Note that the scale
of the axes in Fig. 2 has been changed compared to Fig. 1(c),
Ir = [I2M 2M 02M 2N ] HT R 1 Ho I2M 2M in order to preserve clarity. Mere comparison of the values
o 02N 2M
for the covariance of the robots and landmarks position

= U diag(i )U T
estimates with the corresponding bounds demonstrates that
In the special case where the map is initially unknown, matrix when available information about the distribution of the land-
assumes the value
marks is exploited, i.e., by employing the expressions from
1 1
= (11M I22 ) J
Lemma 4.5, a better characterization of the expected accuracy
rr + Prr0 (1M 1 I22 )
of the position estimates is achieved.
V. S IMULATION R ESULTS
A series of experiments in simulation were conducted, VI. C ONCLUSIONS
in order to validate the preceding theoretical analysis. The In this paper, we have presented a method for predicting
simulated robots move in an arena of dimensions 10 10m, the positioning performance of C-SLAM, without the need to
within which point landmarks are located. The velocity of resort to extensive simulations or experimentation. The derived
the robots is kept constant at V = 0.25m/s, while their expressions enable us to determine guaranteed steady-state
orientation changes randomly, using samples drawn from a accuracy values for a robot team with a given set of sensors,
uniform distribution. To simplify the presentation, a homoge- mapping an area of known size (cf. Lemma 4.3). Moreover,
neous robot team is assumed. The standard deviation of the when a model of the distribution of the landmarks in the area
velocity measurement noise is equal to V = 0.05V and the is available, application of Lemma 4.5 yields a tight upper
standard deviation of the errors in the orientation estimates is bound for the expected value of the steady-state covariance
equal to = 2 , for all robots. Similarly, the values selected of the position estimates of the robots and landmarks. The
for the standard deviations of the exteroceptive measurements aforementioned Lemmas provide functional relations for the
of the robots are = 2 , for the bearing measurements, positioning accuracy in terms of the number of landmarks, the
and = 0.05m, for the range measurements. For the results size of the robot team, the accuracy of the robots sensors, and
presented in this section, the RPMG shown in Fig. 1(a) is used. the topology of the RPMG. Thus, they facilitate the prediction
It is assumed that initially the robots have perfect knowledge of the performance of a robot team in a mapping application.
of their positions, while the landmark positions are unknown. Certainly, the most restrictive assumption employed in the
In order to demonstrate the validity of the bound on the current work is that the topology of the RPMG does not
worst-case covariance of C-SLAM, provided in Lemma 4.3, change, which implies that certain pairs of robots or robots
a particularly adverse scenario for the placement of the land- and landmarks maintain line-of-sight contact at all times.
marks is considered. Specifically, all the landmarks form a Although this is not possible in many real-world applications,
cluster at one corner of the arena, while the robots begin their the presented analysis can serve as a basis for extensions to
exploration at the opposite corner (Fig. 1(b)). In this case the cases where the topology of the RPMG changes dynamically.
 5
Robot Initial robot positions
Landmark Landmark positions True Covariancerobots
4 Upper boundrobots
0.016 True CovarianceLandmaks
Upper boundLandmarks
3
0.014

2
0.012

Covariance (m2)
0.01

y(m)
0

0.008
1

0.006
2

0.004
3

4 0.002

5 0
5 4 3 2 1 0 1 2 3 4 5 0 20 40 60 80 100 120 140 160 180 200
x(m) Time (sec)

(a) (b) (c)

Fig. 1. (a) The RPMG used for the simulation experiments (b) The initial positions and part of the trajectories of the robots for an adverse C-SLAM
scenario. (c) Comparison of the actual covariance of the position estimates against the worst-case performance bound, for the scenario in (b). The plotted
lines correspond to the mean of the covariance along the two coordinate axes.

3
x 10
3.5
True Covariancerobots
[4] J. W. Fenwick, P. M. Newman, and J. J. Leonard, Cooperative Con-
Upper boundrobots current Mapping and Localization, in Proceedings of the 2002 IEEE
True CovarianceLandmaks
3 Upper boundLandmarks International Conference on Robotics and Automation, Washington
D.C., 11-15 May 2002, pp. 18101817.
[5] R. C. Smith, M. Self, and P. Cheeseman, Autonomous Robot Vehicles.
2.5
Springer-Verlag, 1990, ch. Estimating Uncertain Spatial Relationships
in Robotics, pp. 167193.
Covariance (m2)

2 [6] P. Moutarlier and R. Chatila, Stochastic multisensory data fusion for

mobile robot location and environment modeling, in Fifth International
Symposium of Robotics Research, H. Miura and S. Arimoto, Eds., Tokyo,
1.5
Japan, 28-31 Aug. 1989, pp. 8594.
[7] R. C. Smith, M. Self, and P. Cheeseman, Estimating uncertain spatial
1 relationships in robotics, in Proc. of the 2nd Workshop on Uncertainty
in Artificial Intelligence (AAAI), Aug. 1986, pp. 121.
0.5
[8] J. W. Fenwick, Collaborative Concurrent Mapping and Localization,
Masters thesis, Massachusetts Institute of Technology, June 2001.
[9] M. D. Marco, A. Garulli, A. Giannitrapani, and A. Vicino, Simultane-
0
0 50 100 150 200 250 300 ous localization and map building for a team of cooperating robots: a set
Time (sec) membership approach, IEEE Transactions of Robotics and Automation,
vol. 19, no. 2, pp. 238248, Dec. 2003.
Fig. 2. Comparison of the average true covariance of the position estimates [10] S. Thrun, A probabilistic online mapping algorithm for teams of mobile
against the corresponding upper bound. Landmark positions and the initial robots, International Journal of Robotics Research, vol. 20, no. 5, pp.
robot positions are selected using samples from a uniform distribution. 335363, 2001.
Averages over 50 runs of C-SLAM are computed. [11] S. B. Williams, G. Dissanayake, and H. Durrant-Whyte, An efficient
approach to the simultaneous localisation and mapping problem, in
Proc. of the 2002 IEEE International Conference on Robotics and
Automation, Washington, DC, May 11-15 2002, pp. 40611.
ACKNOWLEDGEMENTS [12] R. Madhavan, K. Fregene, and L. E. Parker, Distributed heterogeneous
outdoor multi-robot localization, in Proceedings of the 2002 IEEE
This work was supported by the University of Minnesota International Conference on Robotics and Automation, Washington, DC,
(DTC), the Jet Propulsion Laboratory (Grant No. 1263201), May 2002, pp. 374381.
[13] , Distributed cooperative outdoor multirobot localization and map-
and the National Science Foundation (ITR-0324864, MRI- ping, Autonomous Robots, Special Issue on Analysis and Experiments
0420836). in Distributed Multi-Robot Systems, vol. 17, no. 1, pp. 2339, July 2004.
[14] A. Howard, Multi-robot mapping using manifold representations, in
Proceedings of the 2004 IEEE International Conference on Robotics
R EFERENCES and Automation, New Orleans, LA, April 2004, pp. 41984203.
[15] A. Kelly, General solution for linearized systematic error propagation
[1] R. Kurazume, S. Nagata, and S. Hirose, Cooperative positioning with
in vehicle odometry, in Proceedings of the IEEE/RSJ International
multiple robots, in Proceedings of the IEEE International Conference
Conference on Intelligent Robots and Systems (IROS), Maui, HI, Oct.29-
in Robotics and Automation, Los Alamitos, CA, May 8-13 1994, pp.
Nov.3 2001, pp. 193845.
12501257.
[16] A. I. Mourikis and S. I. Roumeliotis, Performance bounds for co-
[2] S. I. Roumeliotis and G. A. Bekey, Distributed multirobot localization, operative simultaneous localization and mapping C-SLAM, Dept. of
IEEE Transactions on Robotics and Automation, vol. 18, no. 5, pp. 781 Computer Science and Engineering, University of Minnesota, Tech.
795, Oct. 2002. Rep., 2004, http://www.cs.umn.edu/mourikis/TR MultiSLAM.pdf.
[3] I. M. Rekleitis, G. Dudek, and E. Milios, Multi-robot cooperative [17] B. Hassibi, Indefinite metric spaces in estimation, control and adaptive
localization: A study of trade-offs between efficiency and accuracy, in filtering, Ph.D. dissertation, Stanford University, August 1996.
Proceedings of the IEEE/RSJ International Conference on Intelligent [18] S. Boyd and L. Vandenberghe, Convex Optimization. Cambridge
Robots and Systems, Lausanne, Switzerland, Sep.30-Oct.4 2002, pp. University Press, 2004.
26902695.