Exactly Sparse Extended Information Filters for Feature-based SLAM

  • Published on

  • View

  • Download

Embed Size (px)


<ul><li><p> http://ijr.sagepub.com/Robotics Research</p><p>The International Journal of</p><p> http://ijr.sagepub.com/content/26/4/335The online version of this article can be found at:</p><p> DOI: 10.1177/0278364906075026</p><p> 2007 26: 335The International Journal of Robotics ResearchMatthew R. Walter, Ryan M. Eustice and John J. Leonard</p><p>Exactly Sparse Extended Information Filters for Feature-based SLAM </p><p>Published by:</p><p> http://www.sagepublications.com</p><p>On behalf of: </p><p> Multimedia Archives</p><p> can be found at:The International Journal of Robotics ResearchAdditional services and information for </p><p> http://ijr.sagepub.com/cgi/alertsEmail Alerts: </p><p> http://ijr.sagepub.com/subscriptionsSubscriptions: </p><p> http://www.sagepub.com/journalsReprints.navReprints: </p><p> http://www.sagepub.com/journalsPermissions.navPermissions: </p><p> http://ijr.sagepub.com/content/26/4/335.refs.htmlCitations: </p><p> What is This? </p><p>- Apr 10, 2007Version of Record &gt;&gt; </p><p> at OhioLink on October 14, 2014ijr.sagepub.comDownloaded from at OhioLink on October 14, 2014ijr.sagepub.comDownloaded from </p><p>http://ijr.sagepub.com/http://ijr.sagepub.com/content/26/4/335http://www.sagepublications.comhttp://www.ijrr.org/http://ijr.sagepub.com/cgi/alertshttp://ijr.sagepub.com/subscriptionshttp://www.sagepub.com/journalsReprints.navhttp://www.sagepub.com/journalsPermissions.navhttp://ijr.sagepub.com/content/26/4/335.refs.htmlhttp://ijr.sagepub.com/content/26/4/335.full.pdfhttp://online.sagepub.com/site/sphelp/vorhelp.xhtmlhttp://ijr.sagepub.com/http://ijr.sagepub.com/</p></li><li><p>Matthew R. WalterComputer Science and Artificial Intelligence LaboratoryMassachusetts Institute of TechnologyCambridge, MA, USAmwalter@mit.edu</p><p>Ryan M. EusticeDepartment of Naval Architecture and Marine EngineeringUniversity of Michigan, Ann Arbor, MI, USAeustice@umich.edu</p><p>John J. LeonardComputer Science and Artificial Intelligence LaboratoryMassachusetts Institute of TechnologyCambridge, MA, USAjleonard@mit.edu</p><p>Exactly Sparse ExtendedInformation Filters forFeature-based SLAM</p><p>Abstract</p><p>Recent research concerning the Gaussian canonical form for Simulta-neous Localization and Mapping (SLAM) has given rise to a handfulof algorithms that attempt to solve the SLAM scalability problem forarbitrarily large environments. One such estimator that has receiveddue attention is the Sparse Extended Information Filter (SEIF) pro-posed by Thrun et al., which is reported to be nearly constant time, ir-respective of the size of the map. The key to the SEIFs scalability is toprune weak links in what is a dense information (inverse covariance)matrix to achieve a sparse approximation that allows for efficient,scalable SLAM. We demonstrate that the SEIF sparsification strat-egy yields error estimates that are overconfident when expressed inthe global reference frame, while empirical results show that relativemap consistency is maintained.</p><p>In this paper, we propose an alternative scalable estimator basedon an information form that maintains sparsity while preserving con-sistency. The paper describes a method for controlling the populationof the information matrix, whereby we track a modified version of theSLAM posterior, essentially by ignoring a small fraction of temporalmeasurements. In this manner, the Exactly Sparse Extended Informa-tion Filter (ESEIF) performs inference over a model that is conserv-ative relative to the standard Gaussian distribution. We compare ouralgorithm to the SEIF and standard EKF both in simulation as wellas on two nonlinear datasets. The results convincingly show that our</p><p>The International Journal of Robotics ResearchVol. 26, No. 4, April 2007, pp. 335359DOI: 10.1177/0278364906075026c2007 SAGE Publications</p><p>method yields conservative estimates for the robot pose and map thatare nearly identical to those of the EKF.</p><p>KEY WORDSmobile robotics, SLAM, Kalman filters, in-formation filters, robotic mapping, robotic navigation</p><p>1. Introduction</p><p>The capability to accurately navigate in a priori unknown en-vironments is critical for autonomous robotics. Using a suiteof inertial and velocity sensors, dead-reckoning provides posi-tion estimates subject to unbounded error growth with time. Insome outdoor applications, one can utilize GPS fixes to period-ically minimize this error. Unfortunately, GPS measurementsare not available in many common environments (e.g. indoorsand underwater), thus requiring an alternative means of keep-ing the error drift in check. Underwater vehicles, for example,often rely upon acoustic long-baseline (LBL) range measure-ments that are fused with motion sensor data (Whitcomb et al.1998). Utilizing LBL navigation requires the deployment andcalibration of a transponder network and limits the operatingrange of the vehicle. The need for such an infrastructure con-strains the degree of autonomy that underwater robots are ableto achieve.</p><p>Simultaneous Localization and Mapping (SLAM) offers asolution for unencumbered navigation that exploits the envi-ronment to maintain accurate pose estimates. By building a</p><p> R. M. Eustice was with the Joint Program in Oceanographic Engineering ofthe Massachusetts Institute of Technology, Cambridge, MA, and the WoodsHole Oceanographic Institution, Woods Hole, MA at the time of this work.</p><p>335</p><p> at OhioLink on October 14, 2014ijr.sagepub.comDownloaded from </p><p>http://ijr.sagepub.com/</p></li><li><p>336 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / April 2007</p><p>map on-line while using inertial and velocity measurements topredict vehicle motion, the robot utilizes observations of theenvironment to localize itself within the map. The stochas-tic nature of the vehicle motion and measurement models, to-gether with noisy sensor data, further complicates the couplingbetween navigation and mapping that is inherent to SLAM.Many successful SLAM algorithms address these issues byformulating the problem in a probabilistic manner, tracking thejoint posterior over the vehicle pose and map.</p><p>In their seminal paper, Smith et al. (1990) show howthis distribution can be modeled by a Gaussian that is com-pletely described by a mean vector and covariance matrix,and tracked via an Extended Kalman Filter (EKF). In part asa result of its relative simplicity, this model has become thestandard tool of choice for a majority of SLAM algorithms.With explicit knowledge of the correlation between the ro-bot state and the map elements, the EKF exploits observationsof the environment to improve the vehicle pose and map es-timates. Maintaining these correlations, though, imposes ann2memory requirement, where n is proportional to the sizeof the map (Moutarlier and Chatila 1989). Furthermore, whilethe EKF efficiently predicts the vehicle motion, measurementupdates for the standard EKF are quadratic in the number ofstates. As a consequence, it is well known that the standardEKF SLAM algorithm is limited to relatively small environ-ments (i.e. on the order of a few hundred features) (Thrunet al. 2005).</p><p>As robots are deployed in larger environments, extensiveresearch has focused on the scalability restrictions of EKFSLAM. An intuitive way of dealing with this limitation is todivide the world into numerous sub-environments, each com-prised of a more manageable number of l features. Thesesubmap approaches (Guivant and Nebot 2001 Williams et al.2002 Leonard and Newman 2003 Bosse et al. 2004) shedsome of the computational burden of the full EKF solution byperforming estimation based only upon the robots local neigh-borhood. The performance time for the Kalman updates is thenl2 rather than the standard n2. One tradeoff of focus-ing on individual local maps is that some methods forgo animmediate estimate for the global map and, in general, are be-lieved to sacrifice convergence speed (Leonard and Newman2003). Alternatively, the FastSLAM algorithm (Montemerloet al. 2002) takes advantage of the conditional independenceof the map elements given knowledge of the robot pose to im-prove scalability. Employing a particle filter representation forthe pose distribution, FastSLAM efficiently tracks a map foreach particle with a collection of independent EKFs, one forevery feature. The computational cost is proportional to thenumber of particles used and is low in situations where rela-tively few particles are sufficient to describe the robot pose.With larger uncertainties, though, the efficiency benefits arenot as obvious as an increased number of particles is necessary(Paskin 2002) to characterize the pose distribution. Addition-ally, there is the problem of particle depletion (van der Merwe</p><p>et al. 2000) as particles describing the true trajectory are lostdue to resampling.</p><p>Recently, strategies have emerged that offer the promise ofscalability through the canonical parametrization of the SLAMdistribution. Rather than a dense covariance matrix and meanvector, the canonical form completely describes the Gaussianusing the information (inverse covariance) matrix and infor-mation vector. Analogous to the EKF, the evolution of theposterior is tracked over time via a two-step process compris-ing the so-called Extended Information Filter (EIF) (Maybeck1979). The dual of the covariance form, the EIF update step isefficient as it is quadratic in the number of measurements1 andnot the size of the map. On the other hand, the time projectionstep is, in general, quadratic in the number of landmarks. Also,recovering the mean from the information vector and matrixrequires a costlyn3matrix inversion. Together, these char-acteristics would seem to rule out information parametrizationas a viable remedy to the scalability problem of the standardEKF and are largely the reason for its limited application toSLAM.</p><p>Pivotal insights by Thrun et al. (2004) and Frese andHirzinger (2001) reveal that the canonical form is, in fact, par-ticularly beneficial in the context of feature-based SLAM asa majority of the off-diagonal elements in the normalized in-formation matrix are inherently very small. By essentially ap-proximating these entries as being zero, Thrun et al. take ad-vantage of what is then a sparse information matrix, present-ing the Sparse Extended Information Filter (SEIF), an adapta-tion of the EIF. In addition to incorporating new observationsefficiently, the SEIF performs the time projection step with asignificant savings in costs, offering a near constant-time solu-tion to the SLAM problem. The caveat is that a subset of themean is necessary to linearize the motion and measurementmodels as well as to enforce the sparsity of the informationmatrix. To that end, the authors estimate the mean of the ro-bot pose and a limited number of features as the solution to asparse set of linear equations that is approximated using relax-ation.</p><p>Similar benefits extend from interpreting the canonicalparametrization as a Gaussian Markov Random Field (GMRF)(Speed and Kiiveri 1986) where small entries in the normal-ized information matrix correspond to weak links in the graph-ical model. By essentially breaking these weak links, Paskin(2002) and Frese (2005a) approximate the graphical modelwith a sparse tree structure. Paskins Thin Junction Tree Fil-ter (TJTF) and Freses Treemap filter are then each capableof efficient inference upon this representation, involving nand log n time, respectively.</p><p>Recently, a number of batch algorithms have been proposedthat solve for the maximum likelihood estimate (MLE) basedupon the entire history of robot motion and measurement data</p><p>1. This assumes knowledge of the mean, which is necessary for observationsthat are nonlinear in the state.</p><p> at OhioLink on October 14, 2014ijr.sagepub.comDownloaded from </p><p>http://ijr.sagepub.com/</p></li><li><p>Walter, Eustice, and Leonard / Exactly Sparse Extended Information Filters for Feature-based SLAM 337</p><p>(Duckett et al. 2000 Folkesson and Christensen 2004 Freseet al. 2005 Dellaert 2005 Thrun et al. 2005). They solve forthe MLE by optimizing a full, nonlinear log-likelihood func-tion over a series of iterations, which provides robustness tolinearization and data association errors. GraphSLAM (Thrunet al. 2005), in similar fashion to the other batch algorithms,forms a graph in which the nodes correspond to the robot posesand map elements and the edges capture the motion and mea-surement constraints. At each iteration, the linearization yieldsa sparse information matrix to which they apply the variableelimination algorithm to marginalize over the map and reducethe graph to one over only the pose history. They subsequentlysolve for the posterior over the pose history and, in turn, thecurrent ML map estimate.</p><p>Dellaert (2005) adopts much the same strategy, consideringthe posterior over the robots entire pose history, taking ad-vantage of what is then a naturally sparse information matrix(Paskin 2002 Eustice et al. 2005a). The approach formu-lates the corresponding mean as the solution to a set of linearequations for which the information matrix is the matrix ofcoefficients. The technique decomposes the information ma-trix into either its Cholesky or QR factorization, paying closeattention to variable ordering. In turn, they jointly solve forthe mean over the pose and map via back-substitution. Asthe author insightfully shows, this closely parallels aspectsof the aforementioned graphical model methods. The resultsdemonstrate promising advantages over the EKF with regardsto performance though the algorithm currently does not ad-dress some important aspects of the SLAM problem such asdata association.</p><p>Alternatively, Wang et al. (2005) treat map building andlocalization as two separate estimation problems. They repre-sent the distribution over the map as a canonical Gaussian thatis maintained using measurements of the relative pose betweenpairs of landmarks. The advantage of sacrificing robot pose in-formation is that the information matrix for the map is then nat-urally sparse. Meanwhile, the robot is continuously relocatedwithin the map based upon observations of features. This esti-mate is fused with that of a standard EKF, which concurrentlyperforms local SLAM, via covariance intersection (Julier andUhlmann 1997) to estimate the robot pose. There are a numberof similarities between this algorithm and the approach pre-sented in this paper although the two approaches have beendeveloped independently of one another.</p><p>The benefit of maintaining the joint distribution over therobot and map is that we can take advantage of dependencerelationships between landmarks and the vehicle pose. Unfor-tunately, the consequence is that, while the information matrixis relatively sparse, it is nonetheless fully populated. In this pa-per, we analyze the process by which the SEIF actively breaksweak robot-landmark links to enforce a desired level of spar-sity. We show that a consequence of the SEIF sparsification isan overconfident estimate for the global map and pose errorswhile the consistency of the local map relations is preserved.</p><p>As a remedy, we propose an efficient information-basedformulation of the SLAM problem that actively controls thepopulation of the information matr...</p></li></ul>


View more >