An evaluation of the sequential Monte Carlo technique for simultaneous localisation and map-building

Abstract

Simultaneous localisation and map-building (SLAM) can be considered as a combined state and parameter estimation problem. Instead of using extended Kalman filtering, a more flexible Sequential Monte Carlo method is considered. Multiple generic particle filters are initialised to estimate the robot and obstacle positions concurrently. Simulation results based on a simple robot environment, which represents obstacles by line segments, indicate the feasibility of the proposed method.

Full text

Page 1

An Evaluation of the Sequential Monte Carlo Technique for
Simultaneous Localisation and Map-building
David C.K. Yuen and Bruce A. MacDonald
Department of Electrical and Electronic Engineering,
University of Auckland, Private Bag 92019, Auckland, New Zealand.
d.yuen@auckland.ac.nz, b.macdonald@auckland.ac.nz
Abstract
Simultaneous Localisation and Map-building (SLAM)
can be considered as a combined state and parameter esti-
mation problem. Instead of using Extended Kalman Fil-
tering, the more flexible Sequential Monte Carlo method
is considered. Multiple Generic Particle Filters are ini-
tialised to estimate the robot and obstacle positions con-
currently. Simulation results based on a simple robot en-
vironment, which represents obstacles by line segments,
indicate the feasibility of the proposed method.
1 Introduction
Simultaneous Localisation And Map-building (SLAM),
also known as Concurrent Mapping and Localisation
(CML), was introduced originally by Motarlier and
Chatila [11] and Smith et.al. [15] to estimate both the
robot and obstacle positions at the same time. There
is a strong coupling between these two estimation tasks:
without a reliable map, the robot cannot determine its
position precisely; without a good robot position esti-
mate, it is difficult to place the detected obstacle onto a
map. This is the major difficulty in SLAM.
Bayes Theorem [13] is the foundation of estimation the-
ory. It states the relationship between the hidden states
(robot positions) and the observations in form of condi-
tional probabilities. Many estimation algorithms, includ-
ing Kalman filtering [16] and the Sequential Monte Carlo
(SMC) method [1], are based on Bayes Theorem. For
SLAM applications, Extended Kalman filtering (EKF) is
the prevalent solution because of its near real time per-
formance. In addition to ranging sensors, EKF has also
been demonstrated to work with active stereo vision [4, 3]
or with multiple sensor types [2]. The convergence prop-
erty and steady state behaviour of EKF based SLAM
have been established by Dissanayake et.al. [5]. While
the storage requirement of EKF is O(N2)1, with N be-
ing the number of obstacle edges in the map, Guivant
and Nebot [8] showed an optimal solution with O(N2
a)
1The computation complexity is O(N3) due to the matrix inver-
sion step [14].
by considering only the obstacle edges on the sub-map
(Na). However, EKF works well only if the system under
examination can be represented by the first order Taylor
approximation. Like the original Kalman filter, it also ex-
pects noise sources to be Gaussian. Due to sensor charac-
teristics and partial obstacle occlusion, the sensory data
perceived by the robot can be highly nonlinear. The lim-
itations of EKF can thus become quite apparent.
The Sequential Monte Carlo (SMC) method [1], also
known as particle filtering, is a recursive Bayesian fil-
ter implemented by Monte Carlo simulation. It is easy
to construct and is not restricted by the linear Gaus-
sian state-space assumption. It provides an attractive
simulation-based approach to compute the posterior dis-
tribution.Fox et.al. [6] introduced a family of SMC
based algorithms, called Monte Carlo Localisation (MCL)
methods. They are developed to solve many different
forms of localisation problems, including position track-
ing, global localisation and even the difficult kidnapped
robot problem [7]. Using a Rao-Blackwellised particle fil-
ter, Murphy [12] solved a simple form of SLAM problem
with a solution domain limited to a small 10 × 10 grid
world.
This paper outlines an SMC SLAM architecture that ini-
tialises multiple particle filters for discrete time, continu-
ous state–space systems to estimate the robot and obsta-
cle positions simultaneously. A simple line obstacle model
is introduced for the SLAM application. Preliminary sim-
ulation results are provided to evaluate the feasibility of
the method.
2 SMC SLAM
2.1 Introduction to SMC
A brief overview of SMC and its application to localisa-
tion is provided here. Please refer to Fox et.al. [7] and
Arulampalam et.al. [1] for more detailed analysis.
In many situations, raw sensor data do not give the most
relevant information to the user. For example, while it
is useful to obtain the robot position during navigation,
the most common ranging sensor only gives obstacle dis-
p. 1

Page 2

tance. The estimation process is intended to retrieve the
value of a desired hidden state variable x ∈ Rnx, from the
available observation data z ∈ Rnz, where nxand nzare
the dimensionality of the hidden state and measurement
data respectively. A fixed sampling interval is assumed.
The subscript represents the sampling time, e.g. x0is the
initial state and zkis the measurement taken at time k.
As mentioned in the last section, Bayes Theorem is the
foundation of estimation theory. It updates the posterior
probability density function (pdf) of the hidden state2
p(xk+1|zk+1) from the previous estimates using the pro-
vided measurement data. When expressed in terms of the
posterior distribution, Bayes Theorem takes the form of:
p(xk+1|zk+1) = p(xk)
p(zk+1|xk+1)
?p(zk+1|xi
k)p(xi
k)dxi
k
(1)
In general the state variable represents all the informa-
tion that it is desired to estimate. The full state in mobile
robot SLAM application is an estimate of the robot’s en-
vironment, represented as a map of the obstacle positions,
plus an estimate of the robot position, all combined in one
single state vector. The general robot position and orien-
tation can be expressed by the 3-D Cartesian coordinates
and the roll, yaw, and pitch angles. In this study, we con-
sider only the 2-D coordinates on a flat plane, plus the
heading direction, which is the yaw angle, for the robot
position. The measurement data set z is divided into the
perceptual data y such as ultrasonic range measurement
and odometry/controller input u. In our notation, the
latest perceptual and odometry data at time k + 1 are
yk+1and ukrespectively.
In much robotics and AI literature, the posterior is often
called the Belief, Bel(xk+1) = p(xk+1|yk+1,uk). Bayes
Theorem can be re-expressed in terms of Bel(xk) as
shown in Equation 2, where η represents a normalisa-
tion constant. Please refer to [7] for the derivation of this
expression.
?
Bel(xk+1) = η p(yk+1|xk+1)
p(xk+1|xk,uk)Bel(xk) dxk
(2)
Equation 2 can be realised with various types of particle
filters. A filter contains a set of P particles. In addi-
tion to the estimated state x, each particle also has an
importance factor f. The particle set Ψ is defined as:
Ψ = {xi,fi}i=1:P.
(3)
During initialisation, the particles are assigned to random
positions in the work space. The importance factors are
initialised to
1
P.
2To be more exact, the posterior distribution should be defined
as p(x0:k+1|z0:k+1), where the subscript 0 : k + 1 refers to all the
samples from start to time k. The expression is simplified after ap-
plying the Markov assumption, which states that zkis conditionally
independent with the previous measurements.
A simple implementation such as Algorithm 1 is vulner-
able to degeneracy, which happens when the importance
factors for all but one of the particles become negligible.
Resampling is usually required, which may in turn lead to
sample impoverishment after successive iterations. Vari-
ous improved resampling schedules and particle filter ar-
chitectures are thus introduced to cope with degeneracy
and other associated problems. Despite the apparent dif-
ferences, all of these filters share the same core structure
as illustrated in Algorithm 1.
Algorithm 1 Realisation of Bayes Theorem
for each particle in Ψ do
1. Importance sampling
2. State estimation update
3. Importance factor update
end for
Algorithm 1 effectively evaluates the Belief expression
(Equation 2) from right to left. At time k + 1, a particle
i is first sampled from Ψ. The chance of being selected
is proportional to the importance factor fk. Then, the
hidden state estimation is updated by sampling the tran-
sition probability p(xk+1|xk,uk). Finally, the likelihood
of the particular observation p(yk+1|xk+1) is evaluated.
The new importance factor fk+1 is obtained by scaling
the previous factor fkby p(yk+1|xk+1). The procedure is
repeated sequentially for each particle in each round of
filter update.
2.2 State versus parameter estimation
In SLAM, there is a subtle difference between the estima-
tion of robot and obstacle position. While the robot posi-
tion is supposed to be time-varying, the obstacles should
remain at the same place. Therefore, the update of robot
position is a typical state estimation problem whereas the
update of obstacle position should be considered as pa-
rameter estimation. SMC is often implemented for state
estimation. Combined state and parameter estimation
has received relatively little attention. Since most of the
original SMC methods assume the estimates are time-
varying, using them for parameter estimation may re-
sult in over-dispersion of the solution. Liu and West [9]
combine both the state and parameter estimates into the
same particle. A shrinking kernel is then applied so that
an increasing attenuation factor is imposed onto the pos-
sible further change to the parameter estimates.
For systems with a small number of parameters, it is rea-
sonable to augment the estimated state variable directly
with the additional parameters. However, for any siz-
able map, the number of obstacle positions may be very
large and the parameter portion may increase the dimen-
sionality of the state vector by orders of magnitude over
the dimensionality of the robot position. The high di-
mensionality of the resulting particles means that many
more individual particles must be included in each fil-
ter. Moreover, many obstacles are outside the detection
p. 2

Page 3

range from the initial robot position. Once the updating
has been started, it is not clear what importance factors
should be assigned when adding extra hidden states.
2.3 The Multiple Filter Extension to SLAM
This paper proposes that in applications such as robot lo-
calisation, a novel extension is made by splitting the full
state estimation vector x into smaller components and de-
veloping a separate estimator for each component. This
is similar to the idea of using a multiple model bootstrap
filter for fast manoeuvring target tracking [10]. However,
each of the filters in [10] estimates the same state while
our filters estimate different components of the full state.
In our case, it is useful to first separate the state into
two components, x = (s,ϕ), where s is an estimate of
the time–varying portion, in this case the robot position,
and ϕ is an estimate of the time–invariant portion (the
parameters) in this case the map of obstacles. Secondly,
it is sensible to further separate the estimate of param-
eters into each parameter, in this case dividing the ob-
stacle map state component ϕ into a separate estimate
ϕj=1:Lfor each of the L obstacles. The intention is that
a sensible subdivision of the estimation problem will lead
to computational advantages as a result of the degree of
independence between the components.
Separate particle filters are thus introduced for the posi-
tion estimation of the robot and for each of the obstacles.
The filter that estimates the robot position is denoted as
the state filter Ψ0and the filters assigned for the estima-
tion of the L obstacle positions are known as parameter
filters Ψj=1:L. The presence of dynamic objects has not
been explicitly considered. The dependence of each par-
ticle filter on the estimated positions of other objects is
accounted for by randomly selecting a single particle from
each filter, as needed.
Algorithm 2 outlines the initialisation and updating pro-
cedures for our SMC SLAM approach.
of a system model sk+1 = Fsys(sk,uk) and perceptual
model yk+1= Fperceptual(xk+1) is assumed. The percep-
tual model relies on a map of the environment as well as
the robot position; the map is assumed to be empty for
SLAM initially. Detected obstacles are gradually added
to the map as the SLAM operation proceeds. The state
filter and the individual parameter filters are closely re-
lated. For example, the computation of the measurement
likelihood p(yk|xk) would require the current robot and
obstacle positions. Neither the robot nor the obstacle po-
sitions can be assumed as completely reliable at any time
for SLAM, and so a probabilistic sampling approached is
adopted.
The presence
At the start, a particle filter Ψ0is initialised to track the
robot position. Since SLAM makes no assumption to any
external reference frame, rather than spreading uniformly
across the whole work space, it is reasonable to assign the
particles randomly around an arbitrary starting point.
No obstacle filter is assigned.
The new obstacle detection and filter update algorithms
are then repeated at each time step.
(k +1)-th iteration, the first step is to find the mean ob-
stacle positions ¯ ϕj=1:L
k
by examining each of the parame-
ter filters. The map associated with the perceptual model
is updated with each ¯ ϕj
evaluated in a similar manner. The projected robot posi-
tion sprj
tem model Fsys(sk,uk), which is in turn being substituted
into the perceptual model Fperceptual(xk+1) and yields the
projected measurements yprj
tween the actual and projected measurements is a good
indicator for the discovery of new obstacle. The approxi-
mated obstacle position can be found as the mean robot
position and actual measurements are available. The ap-
proximated obstacle position is then applied to initialise a
new parameter filter. The internal map is basically empty
during the early phase of the operation. Any obstacle be-
ing detected would result in large difference between the
actual and projected measurements. This triggers the
initialisation of the parameter filters. The approximate
obstacle position will then be added as a temporary ob-
ject to the map at the start of next iteration. When the
same obstacle is being observed again, the measurement
difference should be substantially smaller and thus avoid
the creation of duplicate obstacle filters.
Considering the
k. The mean robot position ¯ skis
k+1is obtained by substituting ¯ skand ukto the sys-
k+1. Significant difference be-
The next step is to update all the filters. When updat-
ing the Ψjfilter, a random particle is selected from each
of the other filters, Ψi=0:L,i?=j, and each random parti-
cle’s obstacle position estimate is added into the map as
a temporary object. The robot position is taken from the
state filter particle just drawn. This provides the neces-
sary coupling between these dependent variables and is
an important part of the algorithm.
Once an obstacle under observation leaves the detection
zone of the robot, the estimation may drift away from
the correct value due to the lack of further observations.
To prevent this problem, any parameter filter Ψj with a
mean obstacle position ¯ ϕj
limit of the mean robot position ¯ sk+1will be suspended
from updating temporarily. Once it comes within range
again it will be reactivated; hopefully before a large dis-
crepancy with projected values causes a new filter to be
created as a duplicate of the same obstacle. The state
filter and all the valid parameter filters are updated one
after another one. The order of updating is unimportant,
which allows for parallel implementation in the future.
kthat is outside the detection
The obstacle filters are examined and considered for re-
moval after each complete round of filter update. This
procedure not only reduces computation, but also sta-
bilises the estimation. The parameter filter removal rules
are described in more detail in the next section. Basi-
cally, a parameter filter will be deleted without further
p. 3

Page 4

consideration if its creation is likely to be triggered acci-
dentally by noise. On the other hand, the mean obstacle
position ¯ ϕj
the map if the progress of the estimation is considered
as satisfactory. The associated parameter filter is deleted
afterwards.
kwill be extracted and stored permanently on
Algorithm 2 SMC SLAM
Initialise state filter Ψ0 for robot position
L = 0 {L is the total number of new obstacles}
repeat
Update perceptual map by ¯ ϕj=1:L
Calculate ¯ sk, sprj
Compare yk+1with yprj
if new obstacle is identified then
L = L + 1
Initialise new parameter filter ΨL
end if
Update all the filters Ψi=0:L
for all Ψi=1:Ldo
Check for possible removal of Ψi
end for
k
k+1and yprj
k+1
k+1
until the last time step
Generic Particle Filter (GPF) [1] is selected for the imple-
mentation of both state and parameter filters. Its update
procedure is shown in Algorithm 3. GPF shares the ba-
sic particle filter structure shown in Algorithm 1, but the
Importance Sampling is replaced by a systematic Resam-
pling stage. The Resampling stage follows the algorithm
described by Arulampalam [1]. It takes a new set of sam-
ples in accordance to the cumulative distribution function
of the importance factors. Particles are resampled only if
the effective particle number Peff falls below a threshold
of PT. In this study, PTis set to P/5, where P is the total
number of particles in a particular filter. The selection of
GPF is due to its simplicity and can be substituted with
more advanced particle filters, such as Auxiliary Parti-
cle Filter. In order to prevent sample impoverishment,
a small amount of Gaussian noise is added during the
update of the state estimates.
Algorithm 3 Update of Generic Particle Filter
[{xi
for i = 1 : P do
State estimation update
Importance factor update
end for
Calculate: fsum=
for i = 1 : P do
Normalise: fi
k/fsum
end for
Calculate: Peff=
i=1(fi
if Peff< PT then
[{xi
end if
k,fi
k}P
i=1] =GPF[{xi
k−1,fi
k−1}P
i=1,uk,yk]
?P
i=1fi
k
k= fi
1
?P
k)2
k,fi
k}P
i=1] =Resample[{xi
k,fi
k}P
i=1]
2.4 Implementation details
This section covers the system dependent implementa-
tion details. To evaluate the SMC SLAM approach, a
simple simulated environment is adopted. The obstacle
v
w
φ
Figure 1: The convention for the coordinate system
boundaries are approximated by line segments, of which
the start and end points are stored as parameters.
The simulated robot has 2 degrees-of-freedom, (move for-
ward/backward and “turn-on-the-spot”). The robot po-
sition s = [v,w,φ] can be represented by the 2-D coordi-
nates v,w and the heading direction φ. The convention
is shown in Figure 1. The system model Fsys(sk,uk) con-
sists of the following kinematic equations:
v(k + 1) = v(k) + u1(k)cosφ(k)
w(k + 1) = w(k) + u1(k)sinφ(k)
φ(k + 1) = φ(k) + u2(k)
(4)
(5)
(6)
u1and u2are the control inputs for the linear and angular
motions respectively. The state variables are corrupted
by independent zero mean Gaussian noise streams.
The robot is equipped with a ring of 24 ultrasonic-like
range finders.The perceptual model Fperceptual(xk+1)
adopted is quite simple. It relies on the use of a map.
In addition to already mapped objects, the obstacle po-
sition still under estimation should also be included. A
sample is randomly drawn from each parameter filter and
added to the map as temporarily obstacles. The percep-
tual model returns 24 readings, which represents the min-
imum obstacle distance from the current robot position
sk+1in each of the 24 directions. Since the sensors have
only a limited range, the maximum allowable reading is
the same as the detection limit. It is taken as 4 m in this
study. The measurement of obstacle distance is affected
by a mixture of zero mean Gaussian G(σ) and Laplace
L(λ) noise components. The former models the normal
sensor noise whereas the latter represents the more catas-
trophic events such as multiple reflection or cross talk
between the sensors. The Laplace distribution, with a
pdf of1
nential distribution. It has long tails on both sides. For
the mixed noise source M = aG(σ) + bL(λ), the 4 pa-
rameters a,σ,b,λ are selected as 0.05, 1.00, 0.05 and 1.00
respectively.
λexp−|x/λ|, is also known as the two-sided expo-
When updating the importance factor, it is necessary to
find the measurement likelihood p(yk+1|xk+1). Since the
projected measurement yprj
using the perceptual model, the measurement likelihood
is equivalent to p(yk+1|yprj
tion estimate sk+1is accurate and the robot environment
has been thoroughly mapped, the error distribution of
individual sensor reading should follow the mixed noise
k+1can be calculated from xk+1
k+1). Suppose the robot posi-
p. 4

Page 5

?????????????
?????????????
2
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
?????????????
1
actual measurements
predicted by
perceptual model
Figure 2: New obstacle detection. If there exists consecu-
tive mismatched measurements (2nd dotted cir-
cle), the chance of detecting a genuine obstacle is
very high. On the other hand, an odd measure-
ment (1st dotted circle) may just be caused by
noise.
2
1
????????????????????????
???????????? ????????????
????????????????????????
???????????? ????????????
???????????? ????????????
???????????? ????????????
???????????? ????????????
Existing Line
New Line
Figure 3: Merge similar lines. The robot is moved from
Point 1 to 2. It detects a new line segment at
Point 2. In fact, this line should be considered as
the extension of an existing line. It is because the
2 lines have almost the same slope and their end
points are close.
model M described in the last paragraph. For multiple
sensor readings, it is possible to derive a pdf for the sum
of squared errors S = M2
independent variables. The S distribution would be iden-
tical to Chi-squared if the error source contains only the
Gaussian component. The probability density function of
S is generated by drawing a large number of samples with
a distribution of M before calculating the sum squared.
A few obstacle filter management rules are established to
trigger the addition and removal of the parameter filters.
In this work, the obstacle edges are modelled as straight
lines. A new parameter filter will be initialised if the dif-
ferences between the actual and projected measurements
(yk+1, yprj
utive readings (see Figure 2). A line is fit through these
measurement points using the least square method. The
start and end measurement points are projected onto the
best fit line to determine the end points for the new line
segment. These end points functions like the foci of an
ellipse. Particles are generated at random position near
them. Suppose the robot is moving along a long corri-
dor, it will initialise new parameter filters once the mea-
surement difference is getting large. Multiple short line
segments, instead of a pair of continuous parallel lines,
will be extracted. It is undesired to generate unnecessary
filters due to computation requirements. Also, existing
filters need more samples to train. Therefore, a new line
will be combined to an existing one if they share the same
end point and with more or less the same slope (see Fig-
1+ M2
2+ ... + M2
24for these
k+1) are sufficiently large for at least three consec-
1 m
Start point
End point
Figure 4: The actual and estimated robot trajectories. The
track which joined together with a thin line is the
actual trajectory, with the other one being the
estimated trajectory.
ure 3).
If a genuine obstacle is detected, the particle distribu-
tion of the filter will become more compact after a few
iterations and this can be judged from the standard devi-
ation. The mean obstacle position ¯ ϕj
manently on the map before the filter removal. Random
error sometimes triggers the construction of a parameter
filter. Some of the particles may contain parameters that
represents longer line segments, while the others represent
the shorter ones. Since the obstacle does not exist, the
importance factor of the particles that represents shorter
line segments will increase. After a few iterations, the
length of the mean line segment (¯ ϕj
short. These filters can then be identified and removed.
kwill be stored per-
k), will become very
3 Results and Discussions
A simple simulation environment with mainly polygonal
obstacles has been established. The system and percep-
tual model and the associated error models are explained
with the parameters documented in the last section. The
map attached to the robot perceptual model is empty
initially. The number of particles assigned for the state
and each of the parameter filters are 2000 and 200 re-
spectively. The algorithm is implemented in C++ with
extensive use of STL. The average computation time per
iteration is 5.0 seconds in a single desktop PC. A few rel-
ative simple code optimisations can be carried out and
should speed up the computation to near real-time.
At the start, the robot has no idea about its actual posi-
tion and environment. It assumes that its initial position
is at (0.0,0.0) facing 0.0o. To compare the actual and
estimated robot trajectories, the estimated trajectory is
translated and rotated so that the initial estimate over-
laps with the actual initial position. The trajectories are
p. 5