A Framework for Simultaneous Localization and

Mapping Utilizing Model Structure

Thomas B. Schön, Rickard Karlsson, David Törnqvist, and Fredrik Gustafsson
Division of Automatic Control
Department of Electrical Engineering
Linköping University
SE–581 83 Linköping, Sweden
Email: {schon, rickard, tornqvist, fredrik}@isy.liu.se

Abstract—This contribution aims at unifying two recent trends Simultaneous localization and mapping (SLAM) is an ex-
in applied particle filtering (PF). The first trend is the major tension of the localization or positioning problem to the case
impact in simultaneous localization and mapping (SLAM) ap- where the environment is un-modeled and has to be mapped
plications, utilizing the FastSLAM algorithm. The second one
is the implications of the marginalized particle filter (MPF) or on-line. An introduction to the SLAM problem is given in
the Rao-Blackwellized particle filter (RBPF) in positioning and the survey papers [11], [12] and the recent book [13]. From
tracking applications. An algorithm is introduced, which merges a sensor point of view, there are two ways of tackling this
FastSLAM and MPF, and the result is an MPF algorithm for problem. The first way is to use only one sensor, such as
SLAM applications, where state vectors of higher dimensions
vision, see e.g., [14]–[17] and the second way is to fuse
can be used. Results using experimental data from a 3D SLAM
development environment, fusing measurements from inertial measurements from several sensors. This work considers the
sensors (accelerometer and gyro) and vision are presented. latter.
The FastSLAM algorithm introduced in [18] has proven to
Keywords: Rao-Blackwellized/marginalized particle filter, be an enabling technology for such applications. FastSLAM
sensor fusion, simultaneous localization and mapping, can be seen as a special case of RBPF/MPF, where the map
inertial sensors, vision. state mt , containing the positions for all landmarks used in
the mapping, can be interpreted as a linear Gaussian state.
I. I NTRODUCTION The main difference is that the map vector is a constant
parameter with a dimension increasing over time, rather than
The main task in localization/positioning and tracking is
a time-varying state with a dynamic evolution over time. The
to estimate, for instance, the position and orientation of the
derivation is completely analogous to (1), and makes use of
object under consideration. The particle filter (PF), [1], [2],
the following factorization
has proven to be an enabling technology for many applications
of this kind, in particular when the observations are com- p(x1:t , mt |y1:t ) = p(mt |x1:t , y1:t )p(x1:t |y1:t ). (2)
plicated nonlinear functions of the position and heading [3].
Furthermore, the Rao-Blackwellized particle filter (RBPF) also The FastSLAM algorithm was originally devised to solve the
denoted the marginalized particle filter (MPF), [4]–[9] enables SLAM problem for mobile robots, where the dimension of
estimation of velocity, acceleration, and sensor error models by the state vector is small, typically consisting of three states
utilizing any linear Gaussian sub-structure in the model, which (2D position and a heading angle) [13]. This implies that all
is fundamental for performance in applications as surveyed platform states can be estimated by the PF.
in [10]. As described in [9], the MPF splits the state vector Parallelling the evolution of PF applications to high di-
xt into two parts, one part xpt which is estimated using the mensional state vectors, the aim of this contribution is to
particle filter and another part xkt where Kalman filters are unify the ideas presented in [9], [19] in order to extend
applied. Basically, it uses the following factorization of the the FastSLAM [18] algorithm to be able to cope with high
posterior distribution of the state vector, which follows from dimensional state vectors as well. Basically, the main result
Bayes’ rule, follows from

p(xp1:t , xkt |y1:t ) = p(xkt |xp1:t , y1:t )p(xp1:t |y1:t ), (1) p(xp1:t ,xkt , mt |y1:t )
= p(mt |xkt , xp1:t , y1:t )p(xkt |xp1:t , y1:t )p(xp1:t |y1:t ). (3)
where y1:t , {y1 , . . . , yt } denotes the measurements up to
time t. If the model is conditionally linear Gaussian, i.e., if The derived algorithm is applied to experimental data from a
the term p(xkt |xp1:t , y1:t ) is linear Gaussian, it can be optimally development environment tailored to provide accurate values
estimated using the Kalman filter, whereas for the second of ground truth. Here, a high precision industrial robot is
factor we have to resort to the PF. programmed to move, possibly using its 6 degrees-of-freedom
 The key factorization, which allows us to solve this problem
successfully is

p(xp1:t , xkt , mt |y1:t )

Mt
Y
= p(mj,t |xp1:t , xkt , y1:t )p(xkt |xp1:t , y1:t ) p(xp1:t |y1:t ) (7)
j=1
| {z } | {z }
(extended) Kalman filter particle filter

In order to devise an estimator for (6) a system model and

a measurement model are needed. The former describes the
dynamic behaviour of the platform, that is how the state
xt evolves over time. The measurement model describes the
sensors, i.e., it consists of equations relating the measurements
yt to the state xt . We want a general algorithm, which is
applicable to many different platforms (aircraft, helicopters,
Figure 1. The 6 DoF ABB IRB1400 industrial robot equipped with the
custom made sensor.
cars, etc.). Hence, the model structure should be as general as
possible, leading us to,

(DoF), while the sensor unit, consisting of three accelerom- xpt+1 = ftp (xpt ) + Apt (xpt )xkt + Gpt (xpt )wtp , (8a)
eters, three gyroscopes, and a camera is attached to it. See xkt+1 = ftk (xpt ) + Akt (xpt )xkt + Gkt (xpt )wtk , (8b)
Figure. 1 for the experimental setup. This allows us to perform mj,t+1 = mj,t , (8c)
repeatable experiments with access to the ground truth (from
y1,t = h1,t (xpt ) + Ct (xpt )xkt + e1,t , (8d)
the industrial robot), so the performance of the algorithm can
(j) (j)
be accurately assessed. y2,t = h2,t (xpt ) + Hj,t (xpt )mj,t + e2,t , (8e)
In Section II the problem under consideration is formulated
in more detail. The proposed algorithm is given and explained where j = 1, . . . , Mt and the noise for the platform states is
in Section III. This algorithm is then applied to an application assumed white and Gaussian distributed with
example in Section IV. Finally, the conclusions are given in  p !
wt Qpt Qpk
t
Section V. wt = ∼ N (0, Qt ), Qt = . (8f)
wtk (Qpk
t )
T
Qkt
II. P ROBLEM F ORMULATION
To simplify the notation in the rest of the paper, denote ftp (xpt )
The aim of this work is to solve the SLAM problem when with ftp , Apt (xpt ) withApt and so on. The measurement noise
the state dimension of the platform is too large to be estimated is assumed white and Gaussian distributed according to
by the PF. This section provides a more precise problem
formulation and introduces the necessary notation. e1,t ∼ N (0, R1,t ), (8g)
The total state vector to be estimated at time t is (j) j

T e2,t ∼ N (0, R2,t ), j = 1, . . . , Mt . (8h)
xt = (xpt )T (xkt )T mTt , (4)
Finally, xk0 is Gaussian,
where xpt denotes the states of the platform that are estimated
by the particle filter, and xkt denotes the states of the platform xk0 ∼ N (x̄0 , P̄0 ), (8i)
that are linear-Gaussian given information about xpt . These
states together with the map mt are estimated using Kalman and the density for xp0 can be arbitrary, but it is assumed
filters. The map states mt consists of the entire map at time known.
t, i.e., There are two different measurement models, (8d) and (8e),

T where the former only measures quantities related to the
mt = mT1,t . . . mTMt ,t , (5) platform, whereas the latter will also involve the map states.
Section IV describes a detailed application example using
where mj,t denotes the position of the j th map entry and Mt
experimental data, where (8d) is used to model inertial sensors
denotes the number of entries in the map at time t.
and (8e) is used to model a camera.
The aim of this work can be formalized as trying to estimate
the following filtering probability density function (PDF),
III. PARTICLE F ILTER FOR SLAM UTILIZING S TRUCTURE
p(xpt , xkt , mt |y1:t ). (6)
In Section III-A the proposed SLAM algorithm is given and
In other words, we are trying to solve the nonlinear filtering in the subsequent sections the details of the algorithm are
problem, providing an estimate of (6). discussed.
A. Algorithm all embellishments available for the particle filter can be used
together with Algorithm 1. To give one example, the so-called
The algorithm presented in this paper draws on several
FastSLAM 2.0 makes use of an improved proposal distribution
rather well known algorithms. It is based on the RBPF/MPF
in step 6b [20].
method, [4]–[9]. The FastSLAM algorithm [18] is extended
Theorem 1: Using the model given by (8), the conditional
by not only including the map states, but also the states
probability density functions for xkt and xkt+1 are given by
corresponding to a linear Gaussian sub-structure present in the
model for the platform. Assuming that the platform is modeled
p(xkt |xp1:t , y1:t ) = N (x̂kt|t , Pt|t ), (10a)
in the form given in (8), the SLAM-method utilizing structure
is given in Algorithm 1. p(xkt+1 |xp1:t+1 , y1:t ) = N (x̂kt+1|t , Pt+1|t ), (10b)

Algorithm 1: Particle filter for SLAM utilizing structure where

x̂kt|t = x̂kt|t−1 + Kt (y1,t − h1,t − Ct x̂kt|t−1 ), (11a)

1) Initialize the particles
p,(i) Pt|t = Pt|t−1 − Kt S1,t KtT , (11b)
x1|0 ∼ p(xp1|0 ),
S1,t = Ct Pt|t−1 CtT + R1,t , (11c)
k,(i)
x1|0 = x̄k1|0 , Kt = −1
Pt|t−1 CtT S1,t , (11d)
k,(i)
P1|0 = P̄1|0 , i = 1, . . . , N,
and
where N denotes the number of particles.
2) If there is a new map related measurement available x̂kt+1|t = Ākt x̂kt|t + Gkt (Qkp T p p −1
t ) (Gt Qt ) zt
perform data association for each particle, otherwise + ftk + Lt (zt − Apt x̂kt|t ), (12a)
proceed to step 3.
3) Compute the importance weights according to Pt+1|t = Ākt Pt|t (Ākt )T + Gkt Q̄kt (Gkt )T − Lt Nt LTt , (12b)
(i) p,(i) S2,t = Apt Pt|t (Apt )T + Gpt Qpt (Gpt )T , (12c)
γt = p(yt |x1:t , y1:t−1 ), i = 1, . . . , N,
Lt = Ākt Pt|t (Apt )T S2,t
−1
, (12d)
(i) (i) P N (j)
and normalize γ̃t = γt / j=1 γt .
4) Draw N new particles with replacement (resam- where
pling) according to, for each i = 1, . . . , N
zt = xpt+1 − ftp , (13a)
(i) (j) (j)
Pr(xt|t = xt|t ) = γ̃t , j = 1, . . . , N.
Ākt = Akt − Gkt (Qkp T p p −1 p
t ) (Gt Qt ) At , (13b)
5) If there is a new map related measurement, perform kp T p −1 kp
Q̄kt = Qkt − (Qt ) (Qt ) Qt . (13c)
map estimation and management (detailed below),
otherwise proceed to step 6. Proof: See [9].
6) Particle filter prediction and Kalman filter (for each
particle i = 1, . . . , N ) B. Data Association
a) Kalman filter measurement update,
Data association is a complicated problem, but it has
k,(i) (i)
p(xkt |xp1:t , y1:t ) = N (xkt |x̂t|t , Pt|t ), been studied extensively in the literature for many tracking
k,(i) (i)
applications, see e.g., [21]–[23]. Classical methods such as
where x̂t|t and Pt|t are given in (11). the nearest neighbor (NN), probabilistic data association
b) Time update for the nonlinear particles, (PDA), joint probabilistic data association (JPDA) or multi-
p,(i) p,(i)
xt+1|t ∼ p(xt+1|t |x1:t , y1:t ). hypothesis tracking (MHT) exist for single and multiple tar-
gets. Depending on the number of targets, the false alarm
c) Kalman filter time update, rate and the probability of detection, these methods varies
in performance and ability to express these phenomena. The
p(xkt+1 |xp1:t+1 , y1:t )
methods mentioned above were originally developed to be
k,(i) (i)
= N (xkt+1|t |x̂t+1|t , Pt+1|t ), used together with estimators based on Kalman filters. The use
k,(i) (i) of particle filters opens up for other data association methods,
where x̂t+1|t and Pt+1|t are given by (12). typically more integrated with the filter.
7) Set t := t + 1 and iterate from step 2. The particle implementation of the SLAM problem will lead
to an increased complexity for the data association. This is
Note that yt denotes the measurements present at time t. The because for each particle in the filter there exist several map
following theorem will give all the details for how to compute entries. Hence, many classical association methods will be too
the Kalman filtering quantities. It is important to stress that computationally intensive for a direct implementation.
C. Likelihood Computation for the RBPF/MPF, otherwise the dimension will be much too
In order to compute the importance weights in
(i)
{γt }N large for the particle filter to handle. Using (20), the integrals
i=1
Algorithm 1, the following likelihoods have to be evaluated in (19) can now be solved, resulting in
(i) p,(i) p,(i)
γt = p(yt |x1:t , y1:t−1 ), i = 1, . . . , N. (14) p(yt |x1:t , y1:t−1 ) =
k,(i) (i)
The standard way of performing this type of computation N (y1,t − h1,t − Ct x̂t|t−1 , Ct Pt|t−1 CtT )
is simply to marginalize the Kalman filter variables xkt and Mt
{mj,t }M (j)
Y
t
j=1 , × N (y2,t −h2,t −Hj,t m̂j,t−1 , Hj,t Σj,t−1 (Hj,t )T +R2j ).
Z j=1
p,(i) p,(i)
p(yt |x1:t , y1:t−1 ) = p(yt , xkt , mt |x1:t , y1:t−1 )dxkt dmt , (21)
(15) D. Map Estimation and Map Management
where A simple map consists of a collection of map point entries
p,(i) p,(i) {mj,t }M t
j=1 , each consisting of:
p(yt , xkt , mt |x1:t , y1:t−1 ) = p(yt |xkt , mt , xt )×
• m̂j,t – estimate of the position (three dimensions).
Mt
p,(i) p,(i)
Y
p(xkt |x1:t , y1:t−1 ) p(mj,t |x1:t , y1:t−1 ). •Σj,t – covariance for the position estimate.
j=1
Note that this is a very simple map parameterization. Each
(16)
particle has an entire map estimate associated to it. Step 5
Let us consider the case where both y1,t and y2,t are present, of Algorithm 1 consists of updating these map estimates in
T T T


i.e., yt = y1,t y2,t . Note that the cases where either y1,t accordance with the new map-related measurements that are
or y2,t are present are obviously special cases. First of all, the available. First of all, if a measurement has been successfully
measurements are conditionally independent given the state, associated to a certain map entry, it is updated using the
implying that standard Kalman filter measurement update according to
Mt
 
(j)
p,(i) p,(i)
Y (j) p,(i) mj,t = mj,t−1 + Kj,t y2,t − h2,t , (22a)
p(yt |xkt , mt , xt ) = p(y1,t |xkt , xt ) p(y2,t |xt , mj,t ).
T


j=1 Σj,t = I − Kj,t Hi,t Σj,t−1 , (22b)
(17) T

−1
Kj,t = Σj,t−1 Hj,t Hj,t Σj,t−1 Hj,t + R2 . (22c)
Now, inserting (17) into (16) gives
If an existing map entry is not observed, the corresponding
p,(i)
p(yt , xkt , mt |x1:t , y1:t−1 ) = map estimate is simply propagated according to its dynamics,
p(y1,t |xkt , xt
p,(i) p,(i)
)p(xkt |x1:t , y1:t−1 )× i.e., it is unchanged
Mt
Y p,(i) (j) p,(i)
mj,t = mj,t−1 , (23a)
p(mj,t |x1:t , y1:t−1 )p(y2,t |xt , mj,t ), (18)
Σj,t = Σj,t−1 . (23b)
j=1

which inserted in (15) finally results in It has to be checked if any of the entries should be removed
Z from the map.
p,(i) p,(i) p,(i)
p(yt |x1:t , y1:t−1 ) = p(y1,t |xkt , xt )p(xkt |x1:t , y1:t−1 )dxkt Finally, initialization of new map entries have to be handled.
Mt Z
Y If h2,t (xpt , mj,t ) is bijective with respect to the map mj,t
(j) p,(i) p,(i)
× p(y2,t |xt , mj,t )p(mj,t |x1:t , y1:t−1 )dm1,t · · · dmMt ,t . this can be used to directly initialize the position from the
j=1 measurement y2,t . However, this is typically not the case,
(19) implying that there is a need for more than one measurement
All the densities present in (19) are known according to in order to be able to initialize a new map entry, utilizing for
example triangulation.
p(xkt |xp1:t , y1:t−1 ) = N (xkt |x̂kt|t−1 , Pt|t−1 ), (20a)
p(mj,t |xp1:t , y1:t−1 ) = N (mt |m̂j,t−1 , Σt−1 ), (20b) E. Approximate Algorithm
p(y1,t |xkt , xpt ) = N (y1,t |h1,t + Ct xkt , R1 ), (20c) The computational complexity of Algorithm 1 will in-
(j) (j) evitably be high, implying that it is interesting to consider
p(y2,t |xpt , mj,t ) = N (y2,t |h2,t + Hj,t mj,t , R2j ). (20d)
ideas on how to reduce it. In the present context multi-
Here it is important to note that the standard FastSLAM rate sensors (sensors providing measurements with different
approximation has been invoked in order to obtain (20d). That sampling times) are typically used. This can be exploited in
is, the measurement equation (8e) is linearized with respect to a way similar to that in [19]. The idea is that, rather than
the map states mj,t . The reason for this approximation is that running Algorithm 1 at the same frequency as the fast sensor,
we for computational reasons want to use a model suitable it is executed at the same frequency as the slow sensor, with
a nested filter handling the fast sensor measurements. This The quaternion estimates are normalized, to make sure that
approach involves approximations. they still parameterizes an orientation. Further details re-
In this way both the linear Gaussian sub-structure in the garding orientation and coordinate systems are given in Ap-
platform model and the multi-rate properties of the sensors pendix A.
are exploited in order to obtain an efficient algorithm [19]. 1) Dynamic Model: The dynamic model describes how the
platform and the map evolve over time. These equations are
IV. A PPLICATION E XAMPLE given below, in the form (8a) – (8d), suitable for direct use in
In this section we provide a rather detailed treatment of Algorithm 1.
a SLAM application, where Algorithm 1 is used to fuse
    
measurements from a camera, three accelerometers and three vt+1 I TI 0 0 0 vt
gyroscopes. The sensor has been attached to a high precision  at+1  0
   I 0 0 0  at 
 
6 DoF ABB IRB1440 industrial robot. The reason for this bω,t+1  = 0
   0 I 0 0
bω,t 

is that the robot will allow us to make repeatable 6 DoF  ba,t+1  0 0 0 I 0  ba,t 
motions and it will provide the true position and orientation ωt+1 0 0 0 0 I ωt
with very high accuracy. This enables systematic evaluation | {z } | {z } | {z }
xk
t+1 Ak
t xk
t
of algorithms. The sensor and its mounting to the industrial  
robot is illustrated in Figure. 1. 0 0 0 0  
I w1,t
The main objective is to find the position and orientation  0 0 0
 w2,t 
of the sensor from sensor data only, despite problems such as 0
+ I 0 0 (27a)
 
 w3,t 
biases in the measurements. In the surrounding neighbourhood 0 0 I 0
w4,t
features are placed in such a way that the vision system can 0 0 0 I | {z }
| {z } wk
easily extract features. These are used in SLAM to reduce the Gk
t
t
problem caused by inertial drift and bias in the IMU sensor.

A. Model  
vt
The basic part of the state vector consists of position ! a 
T2  t
   
pt ∈ R3 , velocity vt ∈ R3 , and acceleration at ∈ R3 , pt+1 pt TI 2 I 03×9 bω,t  +wtp ,
= +
all described in an earth-fixed reference frame. Furthermore, qt+1 qt 04×3 04×9 − T2 S̃(q)  
} ba,t
 
the state vector is extended with bias states for acceleration | {z } | {z } | {z
xp ftp (xp
t) Ap p ωt
ba,t ∈ R3 , and angular velocity bω,t ∈ R3 in order to account t (xt )
t+1
| {z }
for sensor imperfections. The state vector also contains the xk
t

angular velocity ωt and a unit quaternion qt , which is used to (27b)

parameterize the orientation. mj,t+1 = mj,t , j = 1, . . . , Mt , (27c)
In order to put the model in the RBPF/MPF framework, the
state vector is split into two parts, one estimated using Kalman where
filters xkt and one estimated using the particle filter xpt . Hence,
define
 
−q1 −q2 −q3

T  q0 q3 −q2 
xkt = vtT aTt (bω,t )T (ba,t )T ωtT , (24a) S̃(q) = 
−q3
, (28)
T
q0 q1 
p
xt = pTt qtT ,


(24b) q2 −q1 q0
which means xkt ∈ R15 and xpt ∈ R7 . In inertial estimation it
and where I denotes the 3 × 3 unit matrix and 0 denotes
is essential to clearly state which coordinate system any entity
the 3 × 3 zero matrix, unless otherwise stated. The process
is expressed in. Here the notation is simplified by suppressing
noise wtk is assumed to be independent and Gaussian, with
the coordinate system for the earth-fixed states, which means
covariance Qkt = diag(Qa , Qbω , Qba , Qω ).
that
2) Measurement Model – Inertial Sensors: The IMU con-
pt = pet , vt = vte , at = aet , (25a) sists of accelerometers measuring accelerations ya,t in all three
wt = wte , bω,t = bbω,t , ba,t = bba,t . (25b) dimensions, a gyroscope measuring angular velocities yω,t in
three dimensions and a magnetometer measuring the direction
Likewise, the unit quaternions represent the rotation from the to the magnetic north pole. Due to the fairly magnetic environ-
earth-fixed system to the body (IMU) system, since the IMU is ment it is just the accelerometers and gyroscopes that are used
rigidly attached to the body (strap-down), for positioning. The inertial sensors operate at 100 Hz. For

T further details on inertial sensors, see for instance [24]–[26].
qt = qtbe = qt,0 qt,1 qt,2 qt,3 . (26) The inertial measurements are related to the states according
to,
   
yω,t 0
y1,t = =
ya,t −R(qt )g e
| {z }
h(xp
t)
 
vt
   at   
03 03 I 03 R(qt ) bω,t  + e1,t , (29)

+
03 R(qt ) 03 I 03   e2,t
| {z }  ba,t  | {z }
C(xp
t) ωt et
| {z }
xk
t

which obviously is in the form required by (8). The mea-

surement noise et is assumed Gaussian with covariance Rt =
diag(Rω , Ra ).
Figure 2. The SLAM setup in the industrial robot laboratory, with the camera
3) Measurement Model – Camera: Before the camera im- and IMU sensor in the robot manipulator. The scene consists of black balls
ages are used they are adjusted according to the calibration. on a stick, making the feature extraction very simple.
This allows us to model the camera using the pinhole model
with focal length f = 1, according to [27], [28],
The two partial derivatives in (33) are given by
1 xct
 
c !
y2,t = ymj ,t = c + e3,t , (30) ∂hc
1
0 − (zxc )2
z ytc = zc
, (34a)
| t {z } ∂mcj
c
0 z1c − (zyc )2
hc (mj,t ,pt ,qt )
∂mcj
where = R(qtcb )R(qtbe ). (34b)
 c ∂mj
xt
mcj,t = ytc  = R(qtcb )R(qtbe )(mj,t − pt ) + rc . (31) The camera model delivers point-measurements of features
ztc in the field-of-view. This can be done using several different
methods with different performance. An often used detector is
Here, rc is a fixed vector representing the translation between the Harris detector [30], which basically extracts well-defined
the camera and the IMU and qtcb is the unit quaternion describ- corners in an image. There are more elaborate detectors
ing the rotation from the IMU to the camera. The covariance available, for example the so-called scale-invariant feature
for the measurement noise is denoted Rc . transform (SIFT) [31]. It is also possible to refine the feature
This particular sensor is equipped with an internal camera, detection process even further by estimating the slope of an
which is synchronized in time with the inertial measurements. image plane [14]. Many of these have good performance,
This provides a good setup for fusing vision information with however at a rather high computational complexity. From
the inertial information. Images are available at 12.5 Hz in a computer vision perspective the current environment is
a resolution of 340 × 280 pixels. In order to use vision for rather simple, hence fast and simple corner detectors can be
feature extraction and estimation we have made use of standard successfully applied.
camera calibration techniques, see e.g., [29].
The features are not exactly in the form suitable for B. Experiment Setup
marginalization in the particle filter. Hence, we are forced to The scene surrounding the platform is equipped with easily
use an approximation in order to obtain a practical algorithm. detectable features placed in the vicinity as depicted in Fig-
The standard approximation [13] is in this case simply to ure. 2. In Table I the parameters in the SLAM example are
linearize the camera measurement equations according to, presented.
In the example presented here, the noise level in the
ymj,t = hc (mj,t , pt , qt ) + e3,t (32a) measurements is such that a simplified version of the NN
≈ hcj (m̂j,t|t−1 , pt , qt ) − Hj,t m̂j,t|t−1 method can be used to solve the data association problem.
Hence, the idea is to associate only one measurement to a
| {z }
h(xp
t)
track (map entry) at any given time. Instead of optimizing
+ Hj,t mj,t + e3,t , j = 1, . . . , Mt , (32b)
globally (global nearest neighbor (GNN)), this is done sub-
where the Jacobian matrix Hj,t is straightforwardly computed optimally, assigning the closest one first. This seems to work
using the chain rule, i.e., perfectly well for the present application.
The experiment begins with zero velocity at a given point.
∂hc ∂hc ∂mcj
Hj,t = = , (33) Before starting the experiment the IMU sensor was calibrated,
∂mj ∂mcj ∂mj in order to correct for initial bias contribution. In Figure. 3 the
 Table I Acceleration
2
S YSTEM , FILTER , AND SENSOR PARAMETERS FOR THE EXPERIMENT.
MPF−SLAM
"Measured"
1 True
Process Noise
Qa = diag 0.52 0.52 0.52


Cov. Acceleration

ax
0
−10 −10 10−10



Cov. Bias angular rate Qbω = diag 10 10
Cov. Bias acceleration Qba = diag 0.0012 0.0012 0.0012
−1

Cov. Angular rate Qω = diag 0.0012 0.0012 0.0012

−2
Measurement Noise 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Rc = diag 0.012 0.012


Cov. Camera
Ra = diag 0.022 2 0.032



Cov. Accelerometer 0.02 2
Cov. Gyroscope Rw = diag 0.022 0.03 2 0.03 2

System 1
Sample freq. (IMU) 1/T = 100 Hz
Sample freq. camera 1/Tc = 12.5 Hz

ay
0
Camera resolution 340 × 280 pixels
Number of particles N = 100 −1

−2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Time [s]

true trajectory is depicted. Note that the motion is only in the

horizontal plane, i.e., with a fixed height, and with constant Figure 4. The true acceleration (from the industrial robot), the measured
acceleration (from the IMU sensor) and the estimates from Algorithm 1.
orientation.

C. Experimental Results
MPF - SLAM , and the ground truth from the industrial robot are
In this section results from the experiment are presented. In depicted for the experiment.
Figure. 3 the result from dead-reckoning, i.e., direct double The vision system is used to build up the map (feature tracks
integration of the IMU acceleration data (after appropriate of landmarks), which is depicted in Figure. 5 for a specific
rotation from sensor to the earth-fixed system) is depicted time. Note that the uncertainty is visible in the spread of the
together with the estimated trajectory using the MPF-SLAM particle cloud.
method. The ground truth is provided from accurate measure-
ments in the robot. Since the motion was rather smooth, high t=65

frequency dynamics in the robot arm was not excited, hence

yielding a very accurate ground truth value. As can be seen
the performance in position is significantly improved when the
MPF- SLAM method is used, compared to only dead-reckoning.
The reason is the filter, where the orientation and acceleration
coupling together with the map features improve the estimates.
In the current experiment the bias terms were not used.
In Figure. 4 the measurement from the accelerometers, the

0.4 True
Dead−reckoning
MPF−SLAM

0.2

Figure 5. Vision measurements from the feature extraction (cross) and current
0 map represented by the particle cloud (red dots). Please note that this figure
y (m)

has to be viewed in color.

−0.2

V. C ONCLUSION
−0.4
This contribution has introduced a Rao-Black-
wellized/marginalized particle filtering framework for
−0.6
solving the SLAM problem, enabling high dimensional state
0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 vectors for the moving platform. The idea draws on the
x (m)
FastSLAM algorithm, but rather than using only the map, we
Figure 3. The ground truth position from the industrial robot, dead-reckoned
also include all the states corresponding to a linear Gaussian
position from inertial sensor data (5 seconds in 100 Hz), the estimate from sub-structure in the analytically solvable Kalman filter part.
the MPF-SLAM algorithm with vision measurements. The approach was validated using a simple application
example, where inertial measurements were fused with vision [9] T. Schön, F. Gustafsson, and P.-J. Nordlund, “Marginalized particle
measurements. The correctness of the result was assessed filters for mixed linear/nonlinear state-space models,” IEEE Transactions
on Signal Processing, vol. 53, no. 7, pp. 2279–2289, Jul. 2005.
using the ground truth. [10] T. B. Schön, R. Karlsson, and F. Gustafsson, “The marginalized particle
filter in practice,” in Proceedings of IEEE Aerospace Conference, Big
ACKNOWLEDGMENT Sky, MT, USA, Mar. 2006.
The authors would like to thank MOVIII (Modeling, Visu- [11] T. Bailey and H. Durrant-Whyte, “Simultaneous localization and map-
ping (SLAM): Part II,” IEEE Robotics & Automation Magazine, vol. 13,
alization and Information Integration), the Swedish research no. 3, pp. 108–117, Sep. 2006.
council (VR) and the MATRIS project (EU research project, [12] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and map-
contract number IST-002013) for funding this work. ping (SLAM): Part I,” IEEE Robotics & Automation Magazine, vol. 13,
no. 2, pp. 99–110, Jun. 2006.
[13] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, ser. Intelligent
A PPENDIX A Robotics and Autonomous Agents. Cambridge, MA, USA: The MIT
C OORDINATE S YSTEMS Press, 2005.
[14] A. J. Davison, I. Reid, N. Molton, and O. Strasse, “MonoSLAM:
The following convention is used to rotate a vector from a Real-time single camera SLAM,” Accepted for publication in IEEE
coordinate system A to a coordinate system B, Transactions on Patterns Analysis and Machine Intelligence, 2007.
[15] A. J. Davison, “Real-time simultaneous localisation and mapping with
xB = RBA xA . a single camera,” in Proceedings Ninth IEEE International Conference
on Computer Vision, vol. 2, Nice, France, Oct. 2003, pp. 1403–1410.
where RBA is used to denote the rotation matrix describing the [16] A. J. Davison, Y. G. Cid, and N. Kita, “Real-time 3D SLAM with wide-
rotation from A to B. Hence, we can get from system A to C, angle vision,” in Proceedings of the 5th IFAC/EUCON Symposium on
Intelligent Autonomus Vehicles, Lisaboa, Portugal, Jul. 2004.
via B according to [17] E. Eade and T. Drummond, “Scalable monocular SLAM,” in Proceed-
ings of IEEE Computer Society Conference on Computer Vision and
RCA = RCB RBA . Pattern Recognition (CVPR), New York, NY, USA, Jun. 2006, pp. 469–
476.
This can also be expressed using unit quaternions qA , [18] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM a
factored solution to the simultaneous localization and mapping prob-
uB = q̄A uA qA , lem,” in Proceedings of the AAAI National Comference on Artificial
Intelligence, Edmonton, Canada, 2002.
where uA is the quaternion extension of the vector xA , i.e., [19] T. B. Schön, D. Törnqvist, and F. Gustafsson, “Fast particle filters for
uA = (0 xTA )T and represents quaternion multiplica- multi-rate sensors,” in 15th European Signal Processing Conference
(EUSIPCO), Poznan’, Poland, Sep. 2007, accepted for publication.
tion. Furthermore, ū denotes the quaternion conjugate. See [20] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM 2.0:
e.g., [32], [33] for an introduction to unit quaternions and An improved particle filtering algorithm for simultaneous localization
other rotation parameterizations. and mapping that provably converges,” in Proceedings of the Sixteenth
International Joint Conference on Artificial Intelligence (IJCAI), Aca-
It is straightforward to convert a given quaternion into the pulco, Mexico, 2003.
corresponding rotation matrix, [21] Y. Bar-Shalom and T. E. Fortmann, Tracking and Data Association, ser.
 2 2 2 2  Mathematics in science and engineering. Orlando, FL, USA: Academic
(q0 + q1 − q2 − q3 ) 2(q1 q2 − q0 q3 ) 2(q1 q3 + q0 q2 )
Press, 1988.
R(q) = 2(q1 q2 + q0 q3 ) 2 − q2 + q2 − q2 )
(q0 1 2 3 2(q2 q3 − q0 q1 ) .
2(q1 q3 − q0 q2 ) 2
2(q2 q3 + q0 q1 ) 2 2 2
(q0 − q1 − q2 + q3 ) [22] Y. Bar-Shalom and X.-R. Li, Estimation and Tracking: Principles,
Techniques, and Software. Norwood, MA, USA: Artech House, 1993.
The following coordinate systems are used in this paper. An [23] S. S. Blackman and R. Popoli, Design and Analysis of Modern Tracking
earth-fixed (denoted with e), body or inertial sensor system Systems. Norwood, MA, USA: Artech House, Inc., 1999.
[24] D. H. Titterton and J. L. Weston, Strapdown inertial navigation technol-
(denoted with b), and camera system (denoted c). ogy, ser. IEE radar, sonar, navigation and avionics series. Stevenage,
UK: Peter Peregrinus Ltd., 1997.
R EFERENCES [25] M. S. Grewal, L. R. Weill, and A. P. Andrews, Global Positioning
[1] N. J. Gordon, D. J. Salmond, and A. F. M. Smith, “Novel approach to Systems, Inertial Navigation, and Integration. New York, USA: John
nonlinear/non-Gaussian Bayesian state estimation,” in IEE Proceedings Wiley & Sons, 2001.
on Radar and Signal Processing, vol. 140, 1993, pp. 107–113. [26] A. Chatfield, Fundamentals of High Accuracy Inertial Navigation,
[2] A. Doucet, N. de Freitas, and N. Gordon, Eds., Sequential Monte Carlo 3rd ed. USA: American Institute of Aeronautics and Astronautics,
Methods in Practice. New York, USA: Springer Verlag, 2001. 1997, vol. 174.
[3] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, [27] Y. Ma, S. Soatto, J. Kosecka, and S. S. Sastry, An invitation to 3-D
R. Karlsson, and P.-J. Nordlund, “Particle filters for positioning, nav- vision – from images to geometric models, ser. Interdisciplinary Applied
igation and tracking,” IEEE Transactions on Signal Processing, vol. 50, Mathematics. Springer, 2006.
no. 2, pp. 425–437, Feb. 2002. [28] R. Hartley and A. Zisserman, Multiple View Geometry in computer
[4] A. Doucet, S. J. Godsill, and C. Andrieu, “On sequential Monte Carlo vision, 2nd ed. Cambridge, UK: Cambridge University Press, 2003.
sampling methods for Bayesian filtering,” Statistics and Computing, [29] Z. Zhang, “A flexible new technique for camera calibration,” IEEE
vol. 10, no. 3, pp. 197–208, 2000. Transactions on Pattern Analysis and Machine Intelligence, vol. 22,
[5] G. Casella and C. P. Robert, “Rao-Blackwellisation of sampling no. 11, pp. 1330–1334, Nov. 2000.
schemes,” Biometrika, vol. 83, no. 1, pp. 81–94, 1996. [30] C. Harris and M. Stephens, “A combined corner and edge detector,” in
[6] A. Doucet, N. Gordon, and V. Krishnamurthy, “Particle filters for state Proceedings of the 4th Alvey Vision Conference, Manchester, UK, 1988,
estimation of jump Markov linear systems,” IEEE Transactions on pp. 147–151.
Signal Processing, vol. 49, no. 3, pp. 613–624, 2001. [31] D. G. Lowe, “Distinctive image features from scale-invariant keypoints,”
[7] R. Chen and J. S. Liu, “Mixture Kalman filters,” Journal of the Royal International Journal of Computer Vision, vol. 60, no. 2, pp. 91–110,
Statistical Society, vol. 62, no. 3, pp. 493–508, 2000. 2004.
[8] C. Andrieu and A. Doucet, “Particle filtering for partially observed [32] M. D. Shuster, “A survey of attitude representations,” The Journal of
Gaussian state space models,” Journal of the Royal Statistical Society, the Austronautical Sciences, vol. 41, no. 4, pp. 439–517, Oct. 1993.
vol. 64, no. 4, pp. 827–836, 2002. [33] J. B. Kuipers, Quaternions and Rotation Sequences. Princeton Univer-
sity Press, 1999.