FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges

1 Introduction

Simultaneous localization and mapping (SLAM) is a highly active research area in robotics and AI. The SLAM problem arises when a moving vehicle (e.g. a mobile robot, submarine, or drone) simultaneously estimates a map of its environment and its pose relative to that map. In the absence of global po­ sition information, the vehicle's pose estimate will become in­ creasingly inaccurate, as will its map. Since maps may con­ tain thousands of entities, acquiring large, accurate maps is a challenging statistical estimation problem, especially when performed in real-time.

Most present-day research on SLAM originates from a sem­ inal paper by Smith and Cheeseman [21], which proposed the use of the extended Kalman filter (EKF) for solving SLAM. This paper is based on the insights that errors in the map and pose errors are naturally correlated, and that the covariance ma­ trix maintained by the EKF expresses such correlations. New­ mann [18] recently proved that the EKF converges for linear SLAM problems, where the motion model and observation model are linear functions with Gaussian noise (see below).

Unfortunately, EKF covariance matrices are quadratic in the size of the map, and updating them requires time quadratic in the number of landmarks N. This quadratic complexity has long been recognized to be a major obstacle in scaling SLAM algorithms to maps with more than a few hundred features. It also limits the applicability of SLAM algorithms to problems with ambiguous landmarks, which induces a data association problem [2; 22]. Today's most robust algorithms for SLAM with unknown data association maintain multiple hypotheses (tracks), which increase their computational complexity.

Consequently, there has been a flurry on research on more efficient SLAM techniques (see e.g., [11]). One group of researchers has developed techniques that recursively divide maps into submaps, thereby confining most computation to small regions. Some of these approaches still maintain global

Daphne Koller and Ben Wegbreit

Computer Science Department Stanford University Stanford, CA 94305-9010

koller@cs.stanford. edu, ben@wegbreit. com

correlations among those submaps, hence are quadratic but with a much reduced constant factor [1; 7; 22; 25]. Others re­ strict the update exclusively to local maps [ 12], hence operate in constant time (assuming known data association).

A second group of researchers has developed techniques that represent maps through potential functions between ad­ jacent landmarks, similar to Markov random fields. The re­ sulting representations require memory linear in the number of landmarks [19; 23] . Under appropriate approximations, such techniques have been shown to provide constant time updat­ ing (again for known data association). Unfortunately, no con­ vergence proof exists for any of these extensions of the EKF, even for the generic case oflinear SLAM. Furthermore, ifland­ marks are ambiguous, all of these approaches have to perform search to find appropriate data association hypotheses, adding a logarithmic factor to their update complexity.

The FastS LAM algorithm, proposed in [ 15] as an efficient approach to SLAM based on particle filtering [6], does not fall into either of the categories above. FastSLAM takes advan­ tage of an important characteristic of the SLAM problem (with known data association): landmark estimates are conditionally independent given the robot's path [17] . FastSLAM uses a par­ ticle filter to sample over robot paths. Each particle possesses N low-dimensional EKFs, one for each of the N landmarks. This representation requires 0( N M) memory, where M is the number of particles in the particle filter. Updating this filter requires O(M log N) time, with or without knowledge of the data associations. However, the number of particles M needed for convergence is unknown and has been suspected to be ex­ ponential in the size of the map, in the worst-case.

This paper proposes an improved version of the FastSLAM algorithm. The modification is conceptually simple: When proposing a new robot pose-an essential step in FastSLAM's particle filter-our proposal distribution relies not only on the motion estimate (as is the case in FastSLAM), but also on the most recent sensor measurement. Such an approach is less wasteful with its samples than the original FastSLAM algo­ rithm, especially in situations where the noise in motion is high relative to the measurement noise.

To obtain a suitable proposal distribution, our algorithm lin­ earizes the motion and the measurement model in the same manner as the EKF. As a result, the proposal distribution can be calculated in closed form. This extension parallels prior work by Doucet and colleagues, who proposed a similar modifica­ tion for general particle filters [6] and Markov Chain Monte Carlo techniques for neural networks [4]. It is similar to the arc reversal technique proposed for particle filters applied to Bayes networks [10], and it is similar to recent work by van der Merwe [24], who uses an unscented filtering step [9] for generating proposal distributions that accommodate the mea­ surement.

While this modification is conceptually simple, it has impor­ tant ramifications. A key contribution of this paper is a conver­ gence proof for linear SLAM problems using a single particle. The resulting algorithm requires constant updating time. To our knowledge, the best previous SLAM algorithm for which convergence was shown requires quadratic update time. Fur­ thermore, we observe experimentally that our new FastSLAM algorithm, even with a single particle, yields significantly more accurate results on a challenging real-world benchmark [7] than the previous version of the algorithm. These findings are of significance, as many mobile robot systems are plagued by control noise, but possess relatively accurate sensors. More­ over, they contradict a common belief that maintaining the en­ tire covariance matrix is required for convergence [5].

2 Simultaneous Localization and Mapping

SLAM addresses the problem of simultaneously recovering a map and a vehicle pose from sensor data. The map contains N features (landmarks) and shall be denoted e = e 1' ... ' eN. The path of the vehicle will be denoted st = s1 , ... , St, where tis a time index and St is the pose of the vehicle at timet.

Most state-of-the-art SLAM algorithms calculate (or ap­ proximate) variants of the following posterior distribution:

where z t = z1 , . .. , Zt is a sequence of measurements (e.g., range and bearing to nearby landmarks), and ut = u 1 , ... , Ut is a sequence of robot controls (e.g., velocities for robot wheels). (As usual, we assume without loss of generality that only a single landmark is observed at each timet.) The vari­ ables nt = n 1 , . .. , nt are data association variables- each nt specifies the identity of the landmark observed at time t. Initially, we assume nt is known; we relax this assumption be­ low.

To calculate the posterior (I), the vehicle is given a proba­ bilistic motion model, in the form of the conditional probability distribution p(st I Ut, St_1 ). This distribution describes how a control Ut, asserted in the time interval [t - 1; t), affects the resulting pose. Additionally, the vehicle IS given a probabilis­ tic measurement model, denoted p(Zt I St, e, nt), describing how measurements evolve from state. In accordance to the rich SLAM literature, we will model both models by nonlinear functions with independent Gaussian noise:

Here g and hare nonlinear functions, and ct and Ot are Gaus­ sian noise variables with covariance R1 and Pt. respectively.

3 FastS LAM

of each other (hence the product over n ). In practice, of course, one does not know the vehicle's path. Nevertheless, the inde­ pendence makes it possible to factor the posterior into a term that estimates the probability of each path, and N terms that estimate the position of the landmarks, conditioned on each (hypothetical) path.

FastSLAM samples the path using a particle filter. Each particle has attached its own map, consisting of N extended Kalman filters. Formally, the m-th particle S)ml contains a path st,[m) along with N Gaussian landmark estimates, described by the mean J.L~~J and covariance I;~~J:

We briefly review the key equations of the regular Fast­ SLAM algorithm, and refer the reader to [15]. Each update in FastSLAM begins with sampling new poses based on the most recent motion command u 1:

Note that this proposal distribution only uses the motion com­ mand u1, but ignores the measurement z1.

Next, FastSLAM updates the estimate of the observed land­ mark(s), according to the following posterior. This posterior takes the measurement Zt into consideration:

Here TJ is a constant. This posterior is the normalized product of two Gaussians as indicated. However, if g is non-linear, the product will not be Gaussian in general. To make the result Gaussian, FastSLAM employs the standard EKF "trick" [13]: g is approximated by a linear function (see below). Under this approximation, (7) is equivalent to the measurement update equation familiar from the EKF literature [13].

In a final step, FastSLAM corrects for the fact that the pose sample siml has been generated without consideration of the most recent measurement. It does so by resampling the parti­ cles [20]. The probability for the m-th particle to be sampled (with replacement) is given by the following variable w!ml, commonly referred to as importance factor:

As shown in [15], the resampling operation can be imple­ mented in O(M log N) time using trees, where M is the num­ ber of samples and N the number of landmarks in the map. However, the number of particles M needed for convergence remains an open question.

FastS LAM [ 15] is based on the important observation [ 17] that the posterior can be factored

This factorization is exact and universal in SLAM problems. It states that if one (hypothetically) knew the path of the vehi­ cle, the landmark positions could be estimated independently

FastSLAM has been extended to SLAM with unknown data associations [14]. If the data association is unknown, each particle m in FastSLAM makes its own local data association de­ (4) cision nim), by maximizing the measurement likelihood. The formula for finding the most likely data association maximizes the resulting importance weight:

Here wlml (nt) makes the dependence of the factor wlml on the variable nt explicit. A key characteristic of FastSLAM is that each particle makes its own local data association. In contrast, EKF techniques must commit to a single data association hy­ pothesis for the entire filter. Results in [14] show empirically that this difference renders FastSLAM significantly more ro­ bust to noise than EKF-style algorithms.

4 FastSLAM 2.0

Our new FastSLAM algorithm is based on an obvious ineffi­ ciency arising from the proposal distribution of regular FastSLAM. In regular FastSLAM, the pose s~mJ is sampled in ac­ cordance to the prediction arising from the motion command Ut, as specified in (6). It does not consider the measurement z1 acquired at time t; instead, the measurement is incorporated through resampling. This approach is particularly troublesome if the noise in the vehicle motion is large relative to the mea­ surement noise. In such situations, sampled poses will mostly fall into areas of low measurement likelihood, and will subse­ quently be terminated in the resampling phase with high proba­ bility. Unfortunately, many real-world robot systems are char­ acterized by relatively high motion noise. As illustrated in the experimental results section of this paper, the waste incurred by this inefficient sampling scheme can be significant.

4.1 Sampling The Pose

FastSLAM 2.0 implements a single new idea: Poses are sam­ pled under consideration of both the motion Ut and the mea­ surement Zt. This is formally denoted by the following sam­ pling distribution, which now takes the measurement Zt into consideration:

In comparison to (6), incorporating the measurement only makes sense if we incorporate our current estimate of the observed landmark--obtained from the variables 8 t - l ,[mJ,ut- 1,zt-1,nt-1 (which are included of the condi­ tioning variables above). So in essence, the difference to Fast­ SLAM is that the measurement Zt is incorporated. However, this change has important ramifications.

The proposal distribution (9) can be reformulated as follows:

That is, the proposal distribution is the product of two fac­ tors: the familiar next state distribution p(st I s~~L Ut), and the probability of the measurement Zt. Calculating the latter involves an integration over possible landmark locations en, .

Unfortunately, sampling directly from this distribution is im­ possible in the general case; it does not even possess a closed form. Luckily, a closed form solution can be attained if g is approximated by a linear function (h may remain non-linear!):

where zlml = g(et'l' s[ml) denotes the predicted measure. [m] h( [m] ) h d' d b d e'[m] ment,s 1 = s 1 _ 1 ,ut t epre 1cte ro otpose, an n = J.L~~~-l the predicted landmark location. The matrices G11 and c; are the Jacobians (first derivatives) of g with respect to e and s, respectively:

Under this EKF-style approximation, the proposal distribution (9) is Gaussian with the following parameters:

where the matrix Q[ml is defined as follows:

4.2 Updating The Observed Landmark Estimate

The updating step remains the same as in FastSLAM (see (7)). As stated in the previous section, g is linearized to retain Gaus­ sianity of the posterior. This leads to the following update equations, whose derivation is equivalent to that of the stan­ dard EKF measurement update [ 13]:

4.3 The Importance Weights

Resampling is necessary even in our new version of Fast­ SLAM, since the particles generated do not yet match the de­ sired posterior. The culprit is the normalizer 7) [m] in (1 0), which may be different for different particles m. This normal­ izer is the inverse of the probability of the measurement under them-thparticle: 7)[m] =p(z 1 I st- 1 ,[mJ , ut ,zt- l ,nt) - 1 . To account for this mismatch, our algorithm resamples in propor­ tion to the following importance factor:

This expression can once again be approximated as a Gaus­ sian by linearizing g. The mean of this Gaussian is Zt , and its covariance is

4.4 Unknown Data Associations

The approach for handling data association is similar to the one in regular FastSLAM: Again, we select the data association n1 that maximizes the probability of the measurement Zt for the m-th particle:

Convergence At first glance, one may be tempted to substitute wim] for 5 the probability on the right-hand side, as in regular Fast- A key result in this paper is the fact that our new version of FastSLAM converges forM =l particle, for a restricted class of linear Gaussian problems (the same for which KFs con­ verge [5; 18]). Specifically, our result applies to SLAM prob­ lems characterized by the following linear form: SLAM. However, w!ml does not consider the sampled pose s!ml, whereas the expression here does. This leads to a slightly different probability, which is calculated as follows.

Linearization of g leads to a Gaussian over Zt with mean ( [m] [m]) d · Q[m] h fi . f g J.Ln, ,t- l, st an covanance t . Bot are unctiOns o the data association variable nt.

4.5 Feature Management

Finally, in cases with unknown data associations, features have to be created dynamically. As is common for SLAM algo­ rithms [5], our approach creates new features when the mea­ surement probability in (20) is below a threshold. However, real-world data with frequent outliers will generate spurious landmarks using this rule. Following [5], our approach re­ moves such spurious landmarks by keeping track of their pos­ terior probability of existence. Our mechanism analyzes mea­ surement to the presence and absence of features. Observing a landmark provides positive evidence for its existence, whereas

not observing it when J.Lhm] falls within the robot's perceptual range provides negative evidence. The posterior probability of landmark existence is accumulated by the following Bayes filter, whose log-odds form is familiar from the literature on occupancy grid maps [ 16]:

Here TJ.ml are the log-odds of the physical existence of land­ mark eJ,mJ in map m, and p(ihm] I s!ml' Zt, n!m 1 ) is the prob­ abilistic evidence provided by a measurement. Under appro­ priate definition of the latter, this rule provides for a simple evidence counting rule. If the log odds drops below a prede­ fined threshold, the corresponding landmark is removed from the map. This mechanism enables particles to free themselves of spurious features.

Linear SLAM can be thought of as a robot operating in a Carte­ sian space equipped with a noise-free compass, and sensors that measure distances to features along the coordinate axes. The following theorem, whose proof can be found in the ap­ pendix, states the convergence of our new FastSLAM variant:

Theorem. For linear SLAM, FastSLAM with M =1 parti­ cles converges in expectation to the correct map if all features are observed inti nitely often, and if the location of one feature is known in advance.

This theorem parallels a similar result previously published for the Kalman filter [5; 18]. However, this result applies to the Kalman filter, whose update requires time quadratic in the number of landmarks N. With M=l, the resampling step becomes obsolete and each update takes constant time. To our knowledge, our result is the first convergence result for a constant-time SLAM algorithm. It even holds if all features are arranged in a large loop, a situation often thought of as the worst case for SLAM problems [8] .

6 Experimental Results

Systematic experiments showed that FastSLAM 2.0 provides excellent results with surprisingly few particles, including M = 1. Most of our experiments were carried out using a benchmark data set collected with an outdoor vehicle in Victo­ ria Park, Sydney [7]. The vehicle path is 3.5km long, and the map is 320 meters wide. The vehicle is equipped with differ­ ential GPS that is used for evaluation only. Fig. 1a shows the map of the terrain, along with the path obtained by raw odome­ try (which is very poor, the average RMS error is 93.6 meters). This data set is presently the most popular benchmark in the SLAM research community [3] .

Figs. 1 b&c show the result of applying FastS LAM with M = 1 particle to the data set, without (Fig. 1 b) and with (Fig. 1c) the feature management approach described in Sec­ tion 4.5. In both cases, the estimated vehicle path is shown Number ol Particles Fig. 2 plots the RMS error of the vehicle position esti­ mate as function of the number of particles for the Victoria data set (panel a) and for synthetic simulation data (panel b) taken from [15]. While our new algorithm does approximately equally well for any number of particles, regular FastSLAM performs poorly for very small particle sets. We suspect that the poor performance of regular FastSLAM is due to the fact that the vehicle possesses relatively inaccurate odometry (see Fig. Ia), yet uses a low-noise range finder for landmark detec­ tion (a common configuration in outdoor robotics), leading to the generation of many particles of low likelihood.

The small number of examples needed to obtain state-of­ the-art estimation translates to unprecedented efficiency of the new filter. The following table shows the results required to process the Victoria Park data set on a lGHz Pentium PC: 7,807 sec I

ln comparison, the data acquisition required 1,550 seconds. Thus, while EKFs cannot be run in real-time, our new algo­ rithm requires less than 4% of the vehicle's trajectory time.

7 Discussion

This paper describes a modified FastSLAM algorithm that is uniformly superior to the FastSLAM algorithms proposed in [15]. The new FastSLAM algorithm utilizes a different pro­ posal distribution which incorporates the most recent measure­ ment in the pose prediction process. In doing so, it makes more

,, .

efficient use of the particles, particularly in situations in which the motion noise is high in relation to the measurement noise.

A main contribution of this paper is a convergence proof for FastSLAM with a single particle. This proof is an improve­ ment over previous formal results, which applied to algorithms much less efficient than the current one. In fact, this result is a first convergence result for a constant time SLAM algorithm.

The theoretical finding is complemented by experimental re­ sults using a standard benchmark data set. The new algorithm is found to outperform the previous FastSLAM algorithm and the EKF approach to SLAM by a large margin. In fact, a single particle suffices to generate an accurate map of a challenging benchmark data set. Despite this surprising result, the use of multiple particles is clearly warranted in situations with am­ biguous data association. We believe that our results illustrate that SLAM can be solved robustly by algorithms that are sig­ nificantly more efficient than EKF-based algorithms.

8 Acknowledgments

This work was supported by DARPA's MARS and MARS2020 program. Daphne Koller was supported by the Office of Naval Research, Young Investigator (PECASE) grant N00014-99-l0464. We thank the Hertz Foundation for their support of Michael Montemerlo's graduate research.