The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008...
Transcript of The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008...
![Page 1: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/1.jpg)
1J. Neira, Vibot 2007-2008
The EKF Solution to
Simultaneous Localization and Mapping
José NeiraUniversidad de Zaragoza
![Page 2: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/2.jpg)
2J. Neira, Vibot 2007-2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 3: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/3.jpg)
3J. Neira, Vibot 2007-2008
Simultaneous Localizationand Mapping
Is it possible to use a vehicle, starting at an
• unknown initial location, in an
• unknown environment, to
• incrementally
build a map of theenvironment,
• and at the same time
use the map to determine thevehicle location?
Chicken and eggproblem?
(image: Paul Newman)
![Page 4: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/4.jpg)
4J. Neira, Vibot 2007-2008
Simultaneous Localizationand Mapping
• There is a large amount of potential applications• It gives the vehicle real autonomy• A solution is indeed possible
The solution to the SLAM problem is, in many respects, a ‘Holy Grail’ of the autonomous vehicle research community, as the ability to build a map and navigate simultaneously would indeed make a robot ‘autonomous’.
(Newman 1999, Leonard 2000, Thrun 2001)
![Page 5: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/5.jpg)
5J. Neira, Vibot 2007-2008
From Wikipedia:
• Simultaneous localization and mapping (SLAM) is a technique used by robots and autonomous vehicles to build up a map within an unknown environment while at the same time keeping track of its current position.
• This is not as straightforward as it might sound due to inherent uncertainties in discerning the robot's relative movement from its various sensors.
• … Some of the statistical techniques used in SLAM include Kalman filters, particle filters and scan matching of range data.
• Pioneering work in this field was conducted by Hugh F. Durrant-Whyte.
• Much of the SLAM work is based on concepts imported from computer vision…
• SLAM has not yet been fully perfected, but it is starting to be employed in unmanned aerial vehicles, autonomous underwater vehicles, planetary rovers and newly emerging domestic robots.
![Page 6: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/6.jpg)
6J. Neira, Vibot 2007-2008
Mobile Robots
![Page 7: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/7.jpg)
7J. Neira, Vibot 2007-2008
Mobile Robots
![Page 8: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/8.jpg)
8J. Neira, Vibot 2007-2008
Mobile Robots
![Page 9: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/9.jpg)
9J. Neira, Vibot 2007-2008
Mobile Sensors
![Page 10: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/10.jpg)
10J. Neira, Vibot 2007-2008
Origins: the localizationproblem
OtilioTriton
Vision
Laser
Sonar
Wheel Encoders
![Page 11: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/11.jpg)
11J. Neira, Vibot 2007-2008
A Simple Example: MonoRob• A robot moves in a mono-dimensional space:
• Odometry:
• Uncertainty model:
Uncertainty in Robot Position
Measured
Error
True value
![Page 12: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/12.jpg)
12J. Neira, Vibot 2007-2008
A Simple Example
• Gaussianity assumption:
(image: Duda, Hart, Stork)
![Page 13: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/13.jpg)
13J. Neira, Vibot 2007-2008
• Odometry error model:
• Example: move 0.9m
Robot Odometry: Example
![Page 14: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/14.jpg)
14J. Neira, Vibot 2007-2008
Robot Odometry: Example• Robot moves again 0.85m:
• New estimation:
It grows!
![Page 15: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/15.jpg)
15J. Neira, Vibot 2007-2008
Odometry
0
0.1
0.2
0.3
0.4
0.5
0.6
1 3 5 7 9
11 13 15 17 19 21 23 25 27 29
Odometry error grows unbounded(with the square root of n)
![Page 16: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/16.jpg)
16J. Neira, Vibot 2007-2008
Vehicle motion in 2D
Composition:
Inversion:
![Page 17: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/17.jpg)
17J. Neira, Vibot 2007-2008
Odometry in 2D
Composition:Odometry model:
![Page 18: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/18.jpg)
18J. Neira, Vibot 2007-2008
Odometry in 2D
Jacobians:
![Page 19: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/19.jpg)
19J. Neira, Vibot 2007-2008
Odometry in 2D
How can we avoid drift?
![Page 20: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/20.jpg)
20J. Neira, Vibot 2007-2008
The need for SLAM
![Page 21: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/21.jpg)
21J. Neira, Vibot 2007-2008
Odometry in 2D
(image: Thrun, Burgard, Fox)
For different rotation and translational errors
![Page 22: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/22.jpg)
22J. Neira, Vibot 2007-2008
Odometry in 2D
(image: Thrun, Burgard, Fox)
We are linearizing errors!
![Page 23: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/23.jpg)
23J. Neira, Vibot 2007-2008
Map-based Localization• Cumulative odometry
errors• A priori Map + Perception
Laser
Vision
J. Neira, J.D. Tardós, J. Horn and G. Schmidt: Fusing Range and Intensity Images for Mobile Robot Localization, IEEE Trans. Robotics and Automation, Vol. 15, No. 1,Feb 1999, pp 76-84.
![Page 24: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/24.jpg)
24J. Neira, Vibot 2007-2008
Multisensor Robot Localization• Results (uncertainty x30)
VisionLaser
Laser + vision
![Page 25: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/25.jpg)
25J. Neira, Vibot 2007-2008
• In many applications the environment is unknown • A priori maps usually are:
–Costly to obtain–Inaccurate–Incomplete–Out of date
The need for SLAM
Map Building
![Page 26: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/26.jpg)
26J. Neira, Vibot 2007-2008
Localization and Mapping elements
(image: Durrant-Whyte)
• M: environment features• U: control inputs• X: vehicle locations• Z: sensor measurements
![Page 27: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/27.jpg)
27J. Neira, Vibot 2007-2008
Map-based localization
(image: Durrant-Whyte)
• Given M, U, Z• Compute X
![Page 28: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/28.jpg)
28J. Neira, Vibot 2007-2008
Mapping
(image: Durrant-Whyte)
• Given X, Z• Compute M
![Page 29: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/29.jpg)
29J. Neira, Vibot 2007-2008
SLAM
(image: Durrant-Whyte)
• Given U, Z• Compute X, M
![Page 30: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/30.jpg)
30J. Neira, Vibot 2007-2008
The need for SLAM
• Stochastic map + Extended Kalman Filter:
• First proposed by:R. Smith and P. Cheeseman, “On the Representation and Estimation of
Spatial Uncertainty”, Int. J. Robotics Research 5(4), pp. 56-68, 1986.R. Smith, M. Self and P. Cheeseman, “A Stochastic Map for Uncertain
Spatial Relationships”, In O. Faugeras and G. Giralt (eds.), Robotics Research, The Fourth Int. Symp., pp. 467-474. The MIT Press, 1988
![Page 31: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/31.jpg)
31J. Neira, Vibot 2007-2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 32: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/32.jpg)
32J. Neira, Vibot 2007-2008
EKF-SLAM: approach• Environment information related to a set
of elements:
• represented by a stochastic map:
B
Fn
F2
RF1
xFnB
xF1B
xF2B
xRB
![Page 33: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/33.jpg)
33J. Neira, Vibot 2007-2008
Map Features in 2D
Points:
Lines:
![Page 34: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/34.jpg)
34J. Neira, Vibot 2007-2008
EKF-SLAM
![Page 35: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/35.jpg)
35J. Neira, Vibot 2007-2008
Example: SLAM in a cloister
• Red dots: environment features (columns)• Black line: robot trajetory
![Page 36: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/36.jpg)
36J. Neira, Vibot 2007-2008
Sensor measurements• In polar coordinates: • In Cartesian coordinates:
![Page 37: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/37.jpg)
37J. Neira, Vibot 2007-2008
The basic EKF SLAM Algorithms
Sensor measurements
![Page 38: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/38.jpg)
38J. Neira, Vibot 2007-2008
EKF-SLAM: add new features
Linearization:
Where:
![Page 39: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/39.jpg)
39J. Neira, Vibot 2007-2008
EKF-SLAM: add new features
![Page 40: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/40.jpg)
40J. Neira, Vibot 2007-2008
EKF-SLAM: add new features
![Page 41: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/41.jpg)
41J. Neira, Vibot 2007-2008
EKF-SLAM: compute robot motion
![Page 42: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/42.jpg)
42J. Neira, Vibot 2007-2008
EKF-SLAM: compute robot motion
Odometry model (white noise):
EKF prediction:
![Page 43: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/43.jpg)
43J. Neira, Vibot 2007-2008
EKF-SLAM: compute robot motion
![Page 44: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/44.jpg)
44J. Neira, Vibot 2007-2008
EKF-SLAM: Observations
![Page 45: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/45.jpg)
45J. Neira, Vibot 2007-2008
EKF-SLAM: Data association
Predicted map .vs. measurements
![Page 46: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/46.jpg)
46J. Neira, Vibot 2007-2008
EKF-SLAM: Observations
Sensor model (white noise):Measurement equation:
Observations at instant k:
Association Hypothesis (obs. i with map feature ji) :
![Page 47: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/47.jpg)
47J. Neira, Vibot 2007-2008
EKF-SLAM: Observations
Linearization:
![Page 48: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/48.jpg)
48J. Neira, Vibot 2007-2008
Data association
Innovation:
Mahalanobis distance:
Hypothesis test:
![Page 49: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/49.jpg)
49J. Neira, Vibot 2007-2008
EKF-SLAM: map update
State update:
Covariance update:
Filter gain:
![Page 50: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/50.jpg)
50J. Neira, Vibot 2007-2008
EKF-SLAM: map update
![Page 51: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/51.jpg)
51J. Neira, Vibot 2007-2008
Why we do SLAM
Uncertainty still grows!
![Page 52: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/52.jpg)
52J. Neira, Vibot 2007-2008
Dead-reckoning, movingforward
0 50 100 150-1
-0.5
0
0.5
1Vehicle error in x (m)
0 50 100 150-4
-2
0
2
4Vehicle error in y (m)
0 50 100 150-4
-2
0
2
4Vehicle error in theta (deg)
0 50 100 1500
0.5
1
1.5
2x 10
-3 sqrt(det (P))
![Page 53: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/53.jpg)
53J. Neira, Vibot 2007-2008
Good news!
Loop closing reduces uncertainty!
![Page 54: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/54.jpg)
54J. Neira, Vibot 2007-2008
Loop closing in EKF-SLAM
Loop closing reduces uncertainty!
0 20 40 60-1
-0.5
0
0.5
1Vehicle error in x (m)
0 20 40 60-1
-0.5
0
0.5
1Vehicle error in y (m)
0 20 40 60-10
-5
0
5
10Vehicle error in theta (deg)
0 20 40 600
2
4
6
8x 10
-4 sqrt(det(P))
![Page 55: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/55.jpg)
55J. Neira, Vibot 2007-2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 56: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/56.jpg)
56J. Neira, Vibot 2007-2008
Feature extraction: Laser
• Obtain line segments from a laser scan:
– Segmentation– Line estimation
Split and merge:1. Recursive Split:
1. Obtain the line passing by the two extreme points
2. Obtain the point more distant to the line
3. If distance > error_max, split and repeat with the left and right sub-scan
2. Merge:1. If two consecutive
segments are close enough, obtain the common line and the more distant point
2. If distance <= error_max, merge both segments
3. Prune short segments4. Estimate line equation
![Page 57: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/57.jpg)
57J. Neira, Vibot 2007-2008
Split and Merge
Split
Split
Split
No more Splits
Merge
![Page 58: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/58.jpg)
58J. Neira, Vibot 2007-2008
....
Less than 8cm
Split and Merge
![Page 59: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/59.jpg)
59J. Neira, Vibot 2007-2008
Between-segments distance < 10cm
Less than 6 pixels
Split and Merge• Elimination of small segments:
• Segment fusion:
![Page 60: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/60.jpg)
60J. Neira, Vibot 2007-2008
• Problematic case:
• Split and merge: only the extreme points are used• Alternatives:
– Linear regression– RANSAC– Hough transform
Split and Merge
![Page 61: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/61.jpg)
61J. Neira, Vibot 2007-2008
Split and Merge
Not robust to complex and/or spurious data
![Page 62: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/62.jpg)
62J. Neira, Vibot 2007-2008
RANSAC• Given a model that requires n data points to
compute a solution and a set of data points P, with #(P) > n :–Randomly select a subset S1 of n data points
and compute the model M1–Determine the consensus set S1* of points is
P compatible with M1 (within some error tolerance)
–If #(S1*) > th, use S1* to compute (maybe using least squares) a new model M1*
–If #(S1*) < th, randomly select another subset S2 and repeat
–If, after t trials there is no consensus set with th points, return with failure
![Page 63: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/63.jpg)
63J. Neira, Vibot 2007-2008
RANSAC
p no. of pointsn points to build
modelw probability that
a point is good
z acceptable probabilityof failure
w 0,5n 2z 0,05t 11
0 0.2 0.4 0.6 0.8 1
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
t tries ¿?
![Page 64: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/64.jpg)
64J. Neira, Vibot 2007-2008
RANSAC
S & MTLS RANSAC RANSAC+TLS
![Page 65: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/65.jpg)
65J. Neira, Vibot 2007-2008
RANSAC
Robust statistics deal with spuriousness
![Page 66: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/66.jpg)
66J. Neira, Vibot 2007-2008
RANSAC for 3D planes
P.M. Newman, J.J. Leonard, J. Neira and J.D. Tardós: Explore and Return: Experimental Validation of Real Time Concurrent Mapping and Localization. IEEE Int. Conf. Robotics and Automation, May, 2002
![Page 67: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/67.jpg)
67J. Neira, Vibot 2007-2008
Sonar
−4 −2 0 2 4 6 8 10−4
−2
0
2
4
6
8
10Laser scan Sonar returns
Very sparse and noisy data
![Page 68: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/68.jpg)
68J. Neira, Vibot 2007-2008
Move and build a local map
Use a good sensor modelExploit redundancy
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Sonar data
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Sonar data
Wall!
Corner!
People!
![Page 69: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/69.jpg)
69J. Neira, Vibot 2007-2008
Sj
θkSjρS
j
βρB
i
θkBi ySj
BixSj
Bi
Apparent Sonar Return
Possible Point
Bi
Hough Votingfor i in 1..n_positions
for j in 1..n_sensorsCompute xBi
Sjfor θSj
k in -β/2..β/2 step δCompute θBj
k ρBjk
Vote(θBjk, ρBj
k)end
endend
Sonar Model for Points
![Page 70: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/70.jpg)
70J. Neira, Vibot 2007-2008
0
5
10
15
20
25
30
35
40
45
50
Ro (m)
Thet
a (d
eg)
Hough Table for Points
1
2
0 1 2 3 4 5
-150
-100
-50
0
50
100
150
Hough Transform: Corners• Sonar returns vote for points• Look for local maxima
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Point Groups
1
2
![Page 71: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/71.jpg)
71J. Neira, Vibot 2007-2008
Apparent Sonar Return
Sj
θkSj
ρSj
β
ρBi
θkBi ySj
BixSj
Bi
Possible Line
Bi
Hough Votingfor i in 1..n_positions
for j in 1..n_sensorsCompute xBi
Sjfor θSj
k in -β/2..β/2 step δCompute θBj
k ρBjk
Vote(θBjk, ρBj
k)end
endend
Sonar Model for Lines
![Page 72: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/72.jpg)
72J. Neira, Vibot 2007-2008
0
5
10
15
20
25
30
35
40
45
50
Ro (m)
Thet
a (d
eg)
Hough Table for Lines
1
2
0 1 2 3 4 5
-150
-100
-50
0
50
100
150
Hough Transform: Lines
• Sonar returns vote for lines• Look for local maxima
−2 −1 0 1 2 3 4 5−3
−2
−1
0
1
2
3Line Groups
1
2
The Hough gives robust local data associations
![Page 73: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/73.jpg)
73J. Neira, Vibot 2007-2008
Hough Transform• Imaging sonar
![Page 74: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/74.jpg)
74J. Neira, Vibot 2007-2008
Hough Transform
D. Ribas, P. Ridao, J. Neira, J.D. Tardós, SLAM using an Imaging Sonar for Partially Structured Underwater Environments, The 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.
![Page 75: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/75.jpg)
1J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 76: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/76.jpg)
2J. Neira, VIBOT, 2008
EKF-SLAM
![Page 77: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/77.jpg)
3J. Neira, VIBOT, 2008
» Red dots: environment features (columns)» Black line: robot trajectory» Black semicircle: sensor range
Example: SLAM in a cloister
![Page 78: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/78.jpg)
4J. Neira, VIBOT, 2008
The Data Association Problem
• n map features:
• m sensor measurements:
• Data association should return a hypothesis that associates each observation Ei with a feature Fji
• Non matched observations:
![Page 79: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/79.jpg)
5J. Neira, VIBOT, 2008
The Correspondence Space
Interpretation tree (Grimson et al. 87):
......
...... ......
......
......
......
possible hypotheses
Green points: measurementsBlue Points: predicted features
![Page 80: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/80.jpg)
6J. Neira, VIBOT, 2008
When data association is difficult• Low sensor error • High sensor error
![Page 81: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/81.jpg)
7J. Neira, VIBOT, 2008
When data association is difficult• Low odometry error • High odometry error
![Page 82: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/82.jpg)
8J. Neira, VIBOT, 2008
When data association is difficult• Low feature density • High feature density
![Page 83: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/83.jpg)
9J. Neira, VIBOT, 2008
How important is data association?
A good algorithm A bad algorithm
![Page 84: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/84.jpg)
10J. Neira, VIBOT, 2008
Why it’s difficult?
A good algorithm A bad algorithm
![Page 85: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/85.jpg)
11J. Neira, VIBOT, 2008
Importance of Data Association• EKF update:
• If the association of Ei with feature Fj is…..
error:
covariance:
Divergence!Consistency
correct: spurious:
Values that depend on
![Page 86: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/86.jpg)
12J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 87: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/87.jpg)
13J. Neira, VIBOT, 2008
Individual Compatibility• Measurement equation for observation Ei and
feature Fj
• Ei and Fj are compatible if:
d = length(zi)
![Page 88: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/88.jpg)
14J. Neira, VIBOT, 2008
Nearest Neighbor
......
...... ......
......
......
Greedy algorithm: O(mn)
![Page 89: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/89.jpg)
15J. Neira, VIBOT, 2008
The Fallacy of the Nearest Neighbor
Unrobust
![Page 90: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/90.jpg)
16J. Neira, VIBOT, 2008
Joint Compatibility
• Given a hypothesis• Joint measurement equation
• The joint hypothesis is compatible if:
d = length(z)
![Page 91: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/91.jpg)
17J. Neira, VIBOT, 2008
Joint Compatibility Branch and Bound
• Find the largest hypothesis with jointly consistent pairings
Selects the largest set of pairingswhere there is consensus
![Page 92: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/92.jpg)
18J. Neira, VIBOT, 2008
The Fallacy of the Nearest Neighbor
A
B
J. Neira, J.D. Tardós. Data Association in Stochastic Mapping usingthe Joint Compatibility Test IEEE Trans. Robotics and Automation, Vol. 17, No. 6, Dec 2001, pp 890 –897
![Page 93: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/93.jpg)
19J. Neira, VIBOT, 2008
Nearest neighbor .vs. Joint Compatibility
NN JCBB
![Page 94: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/94.jpg)
20J. Neira, VIBOT, 2008
SLAM without odometry• No estimation of the
vehicle motion• Segments in the
environment
![Page 95: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/95.jpg)
21J. Neira, VIBOT, 2008
SLAM without odometry
x
y
x
y
x
y
x
y
x
y
x
y
x
y
x
y
Assuming small motions
![Page 96: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/96.jpg)
22J. Neira, VIBOT, 2008
SLAM without odometry
x
y
x
y
x
y
x
y
x
y
x
y
x
y
x
y
Data association using Joint Compatibility
![Page 97: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/97.jpg)
23J. Neira, VIBOT, 2008
SLAM without odometry
x
y
x
y
x
y
x
y
x
y
x
y
x
y
Vehicle motion estimation
![Page 98: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/98.jpg)
24J. Neira, VIBOT, 2008
SLAM without odometry
![Page 99: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/99.jpg)
25J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 100: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/100.jpg)
26J. Neira, VIBOT, 2008
Loop closing in mosaicinguse first
Joint work with R. García, University of Girona
![Page 101: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/101.jpg)
27J. Neira, VIBOT, 2008
Loop closing in mosaicing:use Last
Sequential mosaicing is a form of odometry
![Page 102: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/102.jpg)
28J. Neira, VIBOT, 2008
The loop closing problem• Loop beginning • Loop end
![Page 103: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/103.jpg)
29J. Neira, VIBOT, 2008
The loop closing problem
![Page 104: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/104.jpg)
30J. Neira, VIBOT, 2008
The loop closing problem
-100 -50 0 50 100 150 200 250 300 350
-150
-100
-50
0
50
100
150
200
Measurements (red) and predicted features (blue)
![Page 105: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/105.jpg)
31J. Neira, VIBOT, 2008
The loop closing problem
• Individual compatibility • Joint Compatibility
0 20 40 60 80 100 120 140 160 180
-50
0
50
100
0 20 40 60 80 100 120 140 160 180
-50
0
50
100
![Page 106: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/106.jpg)
32J. Neira, VIBOT, 2008
The loop closing problem
![Page 107: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/107.jpg)
33J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 108: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/108.jpg)
34J. Neira, VIBOT, 2008
Global Localization• Robot placed in a previously mapped environment
http://www.acfr.usyd.edu.au/homepages/academic/enebot/victoria_park.htm
![Page 109: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/109.jpg)
35J. Neira, VIBOT, 2008
Problem Definition • On-board sensor obtains m measurements:
http://www.acfr.usyd.edu.au/homepages/academic/enebot/victoria_park.htm
![Page 110: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/110.jpg)
36J. Neira, VIBOT, 2008
Problem Definition
• Twofold question:– Is the vehicle in
the map?– If so, where?
http://www.acfr.usyd.edu.au/homepages/academic/enebot/victoria_park.htm
![Page 111: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/111.jpg)
37J. Neira, VIBOT, 2008
Global Localization algorithms
• Correspondence space– Consider consistent combinations of measurement-
feature pairings.» Branch and Bound (Grimson, 1990)» Maximum Clique (Bailey et. al. 2000)» Random Sampling (Neira et. al. 2003)
• Configuration space– Consider different vehicle location hypotheses.
» Monte Carlo Localization (Fox et. al. 1999)» Markov Localization (Fox et. al. 1998)
![Page 112: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/112.jpg)
38J. Neira, VIBOT, 2008
In correspondence space
![Page 113: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/113.jpg)
39J. Neira, VIBOT, 2008
No vehicle location• Unary constraints:
depend on a single matching (size, color,…)
–Trees: trunk diameter
–Walls: length, corners: angle....
OK
61 714 354 176 000 valid hypotheses
![Page 114: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/114.jpg)
40J. Neira, VIBOT, 2008
No vehicle location• Binary constraints:
distances betweenpoints:
0 100 200 300 400 500 600
100
200
300
400
500
600
700
800
900
![Page 115: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/115.jpg)
41J. Neira, VIBOT, 2008
Locality• Features Fi and Fj may belong to the same
hypotesis iff they are ‘close enough’.
xyxyxyxyxyxy
xy
xyS1
S2S3
S4
S5
S6S7xy
xyS1
S2S3
S4
S5
S6S7
0 10 20 30 40 50 60 70
0
10
20
30
40
50
60
70
nz = 755
Covisibilitymatrix
Limit search in the map to subsets of covisible featuresLocality makes search linear with the global map size
![Page 116: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/116.jpg)
42J. Neira, VIBOT, 2008
Algorithm 1: Geometric Constraints Branch and Bound (Grimson, 1990)
......
...... ......
......
......
......
X X
XX
![Page 117: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/117.jpg)
43J. Neira, VIBOT, 2008
Algorithm 2: Maximum Clique
• All unary and binary constraints can be precomputed
• Build a compatibility graph where:– Nodes represent unary compatible pairings– Arcs represent pairs of binary compatible pairings
• Carrahan, Pardalos (1990)
13
22
32
41
![Page 118: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/118.jpg)
44J. Neira, VIBOT, 2008
Algorithm 3: Generation Verification
......
...... ......
......
......
......
X X
XX
Verification
Generation
![Page 119: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/119.jpg)
45J. Neira, VIBOT, 2008
Algorithm 4: RANSAC
......
...... ......
......
......
......
X X
X X
![Page 120: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/120.jpg)
46J. Neira, VIBOT, 2008
mean time .vs. m
Experiments1. No significant difference in effectiveness of the
considered algorithms2. All algorithms are made linear with the size of
the map3. Efficiency when the vehicle is in the map
![Page 121: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/121.jpg)
47J. Neira, VIBOT, 2008
Experiments4. When the vehicle is NOT in the map:
random measurements, 100 for each m=3..30
mean time .vs. m
J. Neira, J.D. Tardós, J.A. Castellanos, Linear time vehicle relocation in SLAM.IEEE Int. Conf. Robotics and Automation, Taipei, Taiwan, May, 2003
![Page 122: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/122.jpg)
48J. Neira, VIBOT, 2008
In configuration space
![Page 123: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/123.jpg)
49J. Neira, VIBOT, 2008
In Configuration Space:RANDOM sampling
• Consider s randomly chosen vehicle locationshypotheses:
• Monte CarloLocalization
![Page 124: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/124.jpg)
50J. Neira, VIBOT, 2008
Alternative 1: location-driven
• Consider each alternative location hypothesis in turn
![Page 125: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/125.jpg)
51J. Neira, VIBOT, 2008
In Configuration Space:GRID sampling
• Uniformly tessellate the space in s = nx ny nφ gridcells:
• MarkovLocalization
![Page 126: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/126.jpg)
52J. Neira, VIBOT, 2008
Voting in configuration space• Each measurement-feature pairing constrains the
set of possible vehicle locations:
![Page 127: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/127.jpg)
53J. Neira, VIBOT, 2008
Alternative 2: pairing-driven
• Consider each measurement-feature pairing in turn
2
![Page 128: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/128.jpg)
54J. Neira, VIBOT, 2008
Results– Resolution: 1.5m for x and y, and 1º for theta
![Page 129: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/129.jpg)
55J. Neira, VIBOT, 2008
Computational Complexity
• How do they compare?
• Pair_driven is better in proportion to the density of features in the environment.
• Victoria Park: 18321 sq. m.,sample every 1.5m, expect pair_driven to be 82 times faster.
![Page 130: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/130.jpg)
56J. Neira, VIBOT, 2008
Voting for map basedlocalization
Ribas D., Palomeras, N. Hernandez, E., Ridao, P., Carreras, M. ICTINEU AUV Wins the First SAUC-E Competition2007 IEEE International Conference on Robotics and Automation, Rome, Italy.
![Page 131: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/131.jpg)
57J. Neira, VIBOT, 2008
Conclusions
• A solution to SLAM is indeed possible• Data association: algorithms based on some form
of consensus provide the best results
»Joint Compatibility»RANSAC»Hough Transform
• What happens at a large scale?
But…
![Page 132: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/132.jpg)
1J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 133: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/133.jpg)
2J. Neira, VIBOT, 2008
The EKF SLAM algorithm
![Page 134: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/134.jpg)
3J. Neira, VIBOT, 2008
Without loss of generality...• Environment to be mapped has more or less
uniform density of features
![Page 135: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/135.jpg)
4J. Neira, VIBOT, 2008
Without loss of generality...• Enviroment to be mapped has more or less
uniform density of features
![Page 136: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/136.jpg)
5J. Neira, VIBOT, 2008
Without loss of generality...• Onboard range and bearing sensor obtains m
measurements
![Page 137: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/137.jpg)
6J. Neira, VIBOT, 2008
Without loss of generality...• Vehicle performs an exploratory trajectory, re-
observing r features, and seeing s = m – r new features.
![Page 138: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/138.jpg)
7J. Neira, VIBOT, 2008
The prediction step
![Page 139: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/139.jpg)
8J. Neira, VIBOT, 2008
EKF SLAM prediction
![Page 140: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/140.jpg)
9J. Neira, VIBOT, 2008
EKF SLAM: prediction
EKF prediction is O(n)
O(1)
O(n)
![Page 141: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/141.jpg)
10J. Neira, VIBOT, 2008
Adding new features
![Page 142: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/142.jpg)
11J. Neira, VIBOT, 2008
EKF SLAM: add new features
Linearization:
Where:
![Page 143: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/143.jpg)
12J. Neira, VIBOT, 2008
EKF SLAM: add new features
Adding new features is O(n) O(s)
O(n)
![Page 144: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/144.jpg)
13J. Neira, VIBOT, 2008
The update step
![Page 145: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/145.jpg)
14J. Neira, VIBOT, 2008
EKF SLAM: map updatem observations:
State update:Covariance update:
Filter gain:Innovation Matrix:
![Page 146: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/146.jpg)
15J. Neira, VIBOT, 2008
The innovation matrix
rxr rxn nxn nxr rxr
O(n²) ?
rn² r²n r²
rxn
rxr
rxr
( )
![Page 147: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/147.jpg)
16J. Neira, VIBOT, 2008
The innovation matrix
O(rn)
rxr rxc nxn cxr rxr( )
![Page 148: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/148.jpg)
17J. Neira, VIBOT, 2008
The Kalman gain matrix
nxnnxr rxrnxc
O(r²n)
![Page 149: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/149.jpg)
18J. Neira, VIBOT, 2008
The covariance matrix
nxnnxr cxn
…..…..
…..…..
EKF update step is O(n²)
![Page 150: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/150.jpg)
19J. Neira, VIBOT, 2008
The mixed blessing of Covariance
• Covariance provides data association• But the covariance matrix is full
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
![Page 151: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/151.jpg)
20J. Neira, VIBOT, 2008
EKF-SLAM updates are O(n2)
50 100 150 200 2500
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Steps
Tim
e (s
eg)
Mean Time by Local Maps
1 local maps
![Page 152: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/152.jpg)
21J. Neira, VIBOT, 2008
Total EKF SLAM is O(n3)
• The total cost of computing a map os size n, withm observations per step, with s=m-r new features per step:
![Page 153: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/153.jpg)
22J. Neira, VIBOT, 2008
Total cost of EKF-SLAM
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
Total SLAM is O(n3)
![Page 154: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/154.jpg)
23J. Neira, VIBOT, 2008
Continuous data association
Individual compatibility is O(nm) = O(n)
![Page 155: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/155.jpg)
24J. Neira, VIBOT, 2008
Map tessellation
Individual compatibility can be O(1)
![Page 156: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/156.jpg)
25J. Neira, VIBOT, 2008
Global localization:Computational Complexity
Global localization is O(nm) = O(n)
![Page 157: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/157.jpg)
26J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 158: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/158.jpg)
27J. Neira, VIBOT, 2008
Consistency of EKF-SLAM
• Nice “convergence” properties of(Dissanayake et al. 2001):– Landmark covariance decreases monotonically– In the limit, landmarks become fully
correlated– In the limit, landmark covariance reaches a
lower bound related to the initial vehicle covariance
• But SLAM is a non-linear problem– The inherent approximations due to
linearizations can lead to divergence (inconsistency) of the EKF » see for example (Jazwinski, 1970)
![Page 159: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/159.jpg)
28J. Neira, VIBOT, 2008
EKF-SLAM: Robot Motion
Odometry model (white noise):
EKF prediction:
Linearization
![Page 160: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/160.jpg)
29J. Neira, VIBOT, 2008
EKF-SLAM: Map Update
Feature observations:
EKF map update:
Linearization
![Page 161: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/161.jpg)
30J. Neira, VIBOT, 2008
The Consistency Problem
• An estimator is consistent if:
– Pessimistic covariance is OK (but not too pessimistic)– Optimistic covariance = Inconsistency = Filter
divergence
True map valueEKF-SLAMestimation
UnbiasedThe Mean SquareError matches the fil-ter computed Cova-riance
![Page 162: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/162.jpg)
31J. Neira, VIBOT, 2008
1. Normalized Estimation Error Squared NEES
2. Innovation test (observation i map feature j)
Consistency Testing
True map requiredSimulations
Critical whenclosing big
loops
![Page 163: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/163.jpg)
32J. Neira, VIBOT, 2008
EKF-SLAM: Real Example
![Page 164: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/164.jpg)
33J. Neira, VIBOT, 2008
EKF-SLAM: Real Example
Inconsistent! Standard data associationcannot close a 250m loop
Computational complexity was not a problem here!
![Page 165: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/165.jpg)
34J. Neira, VIBOT, 2008
EKF-SLAM: Simulation
• Simulation conditions– Perfect data association– Ideal odometry and measurement noise
» white, Gaussian, known covariance
• Advantages of simulation:– Consistency can be tested against the true
map– A simulation with noise=0 gives the
theoretical map covariance (without linearization errors)
![Page 166: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/166.jpg)
35J. Neira, VIBOT, 2008
EKF-SLAM: SimulationPerfect data associationand noise model
![Page 167: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/167.jpg)
36J. Neira, VIBOT, 2008
EKF-SLAM: Covariance
Initial uncertainty = 0 Initial uncertainty > 0
Optimistic Violates lower bound
J.A. Castellanos, J. Neira, J.D. Tardós, Limits to the Consistency of EKF-based SLAM, 5th IFAC Symposium on Intelligent Autonomous Vehicles, Lisbon, July 2004
![Page 168: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/168.jpg)
37J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 169: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/169.jpg)
38J. Neira, VIBOT, 2008
Local submaps
![Page 170: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/170.jpg)
39J. Neira, VIBOT, 2008
Local map building• Periodically, the robot
starts a new map, relative to its currentlocation:
• Given measurements:
• EKF approximates theconditional mean:
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3
t= 33.700s
x
y
x
y
x
y
x y
P1
xy P2
x
y
S3
x
y
S4
B1 R1
![Page 171: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/171.jpg)
40J. Neira, VIBOT, 2008
Local map building• Second map:
• No information isshared:
• Common reference:
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
xy
x
yx
y
x
yS1
x
y
S2
B2 R2 Maps are uncorrelated
![Page 172: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/172.jpg)
41J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 173: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/173.jpg)
42J. Neira, VIBOT, 2008
Map Joining: Example
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
xy
x
y
xy
x y
P1
xyP2
x
yS3
x
y
S4B1 R2
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3t= 33.700s
x
yx
y
x
y
x y
P1
xy P2
x
yS3
x
y
S4B1
R1
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
xy
x
y xy
x
yS1
x
y
S2
B2 R2
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3 t= 48.492s
x
y
x y
P1
xy
x
y
x
y xy
x
y
xy
x y
P1
xy P2
x
y
x
y xy
x
y
xy
x
y
x
y xy
x
y
xy
x
y
S5
x
y
S6
xy
x
y
xy
x
y
S3
x
yS4B1 R2
Joined map After matching and fusionJ.D. Tardós, J. Neira, P.M. Newman and J.J. Leonard, Robust Mapping and Localization in Indoor Environments using Sonar Data, The Int. Journal of Robotics Research, Vol. 21, No. 4, April, 2002, pp 311 –330
![Page 174: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/174.jpg)
43J. Neira, VIBOT, 2008
Map Joining
![Page 175: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/175.jpg)
44J. Neira, VIBOT, 2008
Map Joining Step• New state vector:
• New covariance matrix:
Map joining is O(n²2) on the maps
n1xn1
n2xn2
nxn
n = n1+n2
![Page 176: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/176.jpg)
45J. Neira, VIBOT, 2008
Matching and Fusion Step• Matching function:
• Joint matching function for the hypothesis:
• Map update using EKF:
![Page 177: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/177.jpg)
46J. Neira, VIBOT, 2008
The innovation matrix
O(rn)
rxr rxc nxn cxr
![Page 178: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/178.jpg)
47J. Neira, VIBOT, 2008
The gain matrix
nxr cxr rxrnxn
O(r²n)
![Page 179: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/179.jpg)
48J. Neira, VIBOT, 2008
The update
nxr rxc nxn
…..…..
…..…..
Matching and fusion is O(n2)
![Page 180: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/180.jpg)
49J. Neira, VIBOT, 2008
Cost of Map Joining per step
50 100 150 200 2500
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Tim
e (s
eg)
Mean Time by Local Maps
1 local maps
8 local maps
![Page 181: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/181.jpg)
50J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
![Page 182: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/182.jpg)
51J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
![Page 183: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/183.jpg)
52J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
![Page 184: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/184.jpg)
53J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
![Page 185: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/185.jpg)
54J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
![Page 186: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/186.jpg)
55J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
![Page 187: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/187.jpg)
56J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
![Page 188: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/188.jpg)
57J. Neira, VIBOT, 2008
Total SLAM with Map Joining
50 100 150 200 250
2
4
6
8
10
12
14
Number of Features
Tim
e (s
eg)
Mean Time by Local Maps
There is an optimal submap size
![Page 189: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/189.jpg)
58J. Neira, VIBOT, 2008
Optimal local map size
0 50 100 150 200 2500
2
4
6
8
10
12
14
16
Local Map Size p
Tim
e (s
eg)
Mean Final Time
p = 270N = 1
p = 141N = 2
p = 77N = 4
p = 44N = 8
p = 28N = 16
p = 20N = 32
p = 16N = 64
p = 14 N = 128 Local Map Size p
![Page 190: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/190.jpg)
59J. Neira, VIBOT, 2008
EKF-SLAM: remember?
Inconsistent!
![Page 191: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/191.jpg)
60J. Neira, VIBOT, 2008
Map Joining closes the loop!
-60 -40 -20 0 20 40 60 80
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
10
t= 0.000s
xyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxyxy
xy
Speedx50
![Page 192: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/192.jpg)
61J. Neira, VIBOT, 2008
Montecarlo Simulations
• Simulation conditions– Perfect data association– Ideal odometry and measurement noise
» white, Gaussian, known covariance
• Advantages of simulation:– Consistency can be tested against the true
map– A simulation with noise=0 gives the
theoretical map covariance (without linearization errors)
– Run several algorithms using the same data– Carry out a large amount of runs
![Page 193: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/193.jpg)
62J. Neira, VIBOT, 2008
1. Define the consistency index CI as:
2. When CI < 1, the estimation is consistent with ground truth
3. When CI > 1, the estimation is inconsistent
Consistency Testing
![Page 194: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/194.jpg)
63J. Neira, VIBOT, 2008
Mean map consistency
0 50 100 150 200 2500.94
0.96
0.98
1
1.02
1.04
1.06
1.08
1.1
1.12
p = 270N = 1
p = 141N = 2
p = 77N = 4 p = 44
N = 8
p = 28N = 16
p = 20N = 32
p = 16N = 64
p = 14 N = 128
Local Map Size p
Inde
x va
lue
Map final Consistency index by local maps
Map final ConsistencyBound
![Page 195: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/195.jpg)
64J. Neira, VIBOT, 2008
Mean feature location
![Page 196: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/196.jpg)
65J. Neira, VIBOT, 2008
Mean feature location
![Page 197: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/197.jpg)
66J. Neira, VIBOT, 2008
Map Joining is a ’more for less’
algorithm:
• It improves the consistency of the resulting map• It reduces the computational cost
Limitations:• Map joining is still O(n2)• Environment size must be known
Overcoming limitations:• Can we have EKF-SLAM updates in O(n)?
![Page 198: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/198.jpg)
67J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction4. Data Association
2. The Data Association Problem1. Continuous Data Association2. The Loop Closing Problem3. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 199: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/199.jpg)
68J. Neira, VIBOT, 2008
D&C: Map Hierarchy• Tree of local maps of half the size their ancestors• Leaves: m=n/P local maps of limited size P
n
n/2 n/2
n/4 n/4n/4 n/4
2p
p p
4p
2p 2p
p pp p m local maps
m/2 joins
4 joins
2 joins
1 join, 1 resulting map
![Page 200: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/200.jpg)
69J. Neira, VIBOT, 2008
Map Hierarchy
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
![Page 201: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/201.jpg)
70J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 1
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 202: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/202.jpg)
71J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 203: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/203.jpg)
72J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 204: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/204.jpg)
73J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 205: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/205.jpg)
74J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 1
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 206: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/206.jpg)
75J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 207: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/207.jpg)
76J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 208: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/208.jpg)
77J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 209: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/209.jpg)
78J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 210: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/210.jpg)
79J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 4
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 211: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/211.jpg)
80J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 3
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 212: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/212.jpg)
81J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 2
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 213: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/213.jpg)
82J. Neira, VIBOT, 2008
Map Hierarchy
Number of Maps : 1
x position(m)
y po
sitio
n(m
)
4p
2p 2p
p pp p
n
4p
2p 2p
p pp p
![Page 214: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/214.jpg)
83J. Neira, VIBOT, 2008
Divide and Conquer SLAM• m maps of size P, cost O(P3) each• joined into m/2 maps of size 2P, cost O((2P)2)• joined into m/4 maps of size 4P, cost O((4P)2)• … joined into 1 local map of size n, cost O(n2)
![Page 215: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/215.jpg)
84J. Neira, VIBOT, 2008
Divide and Conquer SLAM• This is a geometric progression of the type:
• Thus in this case:
![Page 216: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/216.jpg)
85J. Neira, VIBOT, 2008
Total cost of D&C SLAM
![Page 217: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/217.jpg)
86J. Neira, VIBOT, 2008
Computational cost per step
50 100 150 200 2500
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Step
Tim
e (s
)EKFD&C
O(n2)
O(n2)
![Page 218: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/218.jpg)
87J. Neira, VIBOT, 2008
Computational cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
22 22 22 22 22 22 22 22 …
42 42 42 42 …
82 82 …
162 …
![Page 219: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/219.jpg)
88J. Neira, VIBOT, 2008
Computational cost per step
Not needed until…
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
22 22 22 22 22 22 22 22 …
42 42 42 42 …
82 82 …
162 …
![Page 220: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/220.jpg)
89J. Neira, VIBOT, 2008
Computational cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
22 22 22 22 22 22 22 22 …
42 42 42 42 …
82 82 …
162 …
![Page 221: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/221.jpg)
90J. Neira, VIBOT, 2008
Amortized cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 …
4 4 4 4 4 4 4 4 4 4 4 4 4 …
8 8 8 8 8 8 8 8 8 …
16
• First idea:
![Page 222: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/222.jpg)
91J. Neira, VIBOT, 2008
Amortized cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 …
4 4 4 4 4 4 4 4 4 4 4 4 4 …
8 8 8 8 8 8 8 8 8 …
16
• First idea:
![Page 223: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/223.jpg)
92J. Neira, VIBOT, 2008
Amortized cost per step
Step: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 …
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 …
4 4 4 4 4 4 4 …
8 8 8 8 8 8 …
16 16 16 16 …
• Better idea:
![Page 224: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/224.jpg)
93J. Neira, VIBOT, 2008
• The full map can be recovered at any time in a single O(n2)step
• But no part of the algorithm or process requires it
50 100 150 200 2500
0.1
0.2
0.3
0.4
0.5
0.6
Step
Tim
e (s
)
Amortized cost per step
D&C SLAM is O(n) per step
EKFD&C
O(n2)
O(n)
![Page 225: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/225.jpg)
94J. Neira, VIBOT, 2008
Consistency: LOOP
![Page 226: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/226.jpg)
95J. Neira, VIBOT, 2008
Consistency: LAWN
![Page 227: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/227.jpg)
96J. Neira, VIBOT, 2008
Consistency : SNAIL
![Page 228: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/228.jpg)
97J. Neira, VIBOT, 2008
Consistency: Montecarlo runsmean consistency
D&C SLAM is always more consistent
50 100 150 200 2500
0.5
1
1.5
2
2.5
3
3.5
4
steps
Inde
x
EKFD&CBound
![Page 229: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/229.jpg)
98J. Neira, VIBOT, 2008
6DOF SLAM with stereo
L. Paz, P. Piníes, J. Neira and J.D. Tardós Large Scale 6DOF SLAM with Stereo-in-Hand. Conditionally accepted, IEE Transactions on Robotics, 2008.
![Page 230: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/230.jpg)
99J. Neira, VIBOT, 2008
6DOF SLAM with stereo
L. Paz, P. Piníes, J. Neira and J.D. Tardós Large Scale 6DOF SLAM with Stereo-in-Hand. Conditionally accepted, IEE Transactions on Robotics, 2008.
![Page 231: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/231.jpg)
100J. Neira, VIBOT, 2008
6Dof Stereo SLAM, outdoors
150 m loop
![Page 232: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/232.jpg)
101J. Neira, VIBOT, 2008
6DOF SLAM with stereo
![Page 233: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/233.jpg)
102J. Neira, VIBOT, 2008
6Dof Stereo SLAM, indoors
180 m loop
![Page 234: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/234.jpg)
103J. Neira, VIBOT, 2008
Outline
1. Basic EKF SLAM1. Introduction: the need for SLAM2. The basic EKF SLAM algorithm3. Feature extraction4. Data Association
2. The Data Association Problem1. Introduction2. Continuous Data Association3. The Loop Closing Problem4. The Global Localization Problem
3. Advanced EKF SLAM1. Computational complexity of EKF SLAM2. Consistency of the EKF SLAM3. SLAM using local maps
1. Sequential Map Joining2. Divide and Conquer SLAM3. Hierarchical SLAM
![Page 235: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/235.jpg)
104J. Neira, VIBOT, 2008
Hierarchical SLAM• Global level: adjacency graph and relative
stochastic map
• Local level: statistically independent local maps
B0
B1
B3B2X1 X2
X3
B0
B1
B2B3
X1 X2
X3
![Page 236: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/236.jpg)
105J. Neira, VIBOT, 2008
Hierarchical SLAM• Local maps:
• Global relative map:
Block diagonal
![Page 237: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/237.jpg)
106J. Neira, VIBOT, 2008
Hierarchical SLAM• Global relative map before loop closing:
• After loop closing:
![Page 238: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/238.jpg)
107J. Neira, VIBOT, 2008
-60 -40 -20 0 20 40 60-100
-80
-60
-40
-20
0
Imposing loop constraints
![Page 239: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/239.jpg)
108J. Neira, VIBOT, 2008
Nonlinear constrainedoptimization
• Minimize corrections to the global map, subject tothe loop constraint:
• Sequential Quadratic Programming (SQP) :
» Iterate until convergence
![Page 240: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/240.jpg)
109J. Neira, VIBOT, 2008
Nonlinear constrainedoptimization
• A more efficient version:
» Iterate until convergence
• Complexity:– P0 is block diagonal– Hi is sparse with nonzeros only for the maps in the loop– The iteration is linear with the number of maps in the
loop
• Convergence:– Converges in 2 or 3 iterations (for loops around 300m)– For bigger errors, may it converge to a local minimum ??
![Page 241: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/241.jpg)
110J. Neira, VIBOT, 2008
Nonlinear constrainedoptimization
• Generalization to closing several loopssimultaneously:
![Page 242: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/242.jpg)
111J. Neira, VIBOT, 2008
Iterated Extended KalmanFilter
• Jacobian of the measurement function:
• Iterated EKF equations:
• With exact loop constraint, z = 0 and Pz = 0, IEKFis equivalent to nonlinear optimization with SQP
C. Estrada, J. Neira, J.D. Tardós, Hierarchical SLAM: real-time accurate map-ping of large environments. To appear in the IEEE Transactions on Robotics
![Page 243: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/243.jpg)
112J. Neira, VIBOT, 2008
Experiment
L. Clemente, A. Davison, I. Reid, J. Neira and J.D. TardósMapping Large Loops with a Single Hand-Held Camera2007 Robotics: Science and Systems, June 27-30, Atlanta, USA
![Page 244: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/244.jpg)
113J. Neira, VIBOT, 2008
Keble College, Oxford
![Page 245: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/245.jpg)
114J. Neira, VIBOT, 2008
![Page 246: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/246.jpg)
115J. Neira, VIBOT, 2008
Sequence of local maps
![Page 247: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/247.jpg)
116J. Neira, VIBOT, 2008
Sequence of local maps
![Page 248: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/248.jpg)
117J. Neira, VIBOT, 2008
Loop closing
![Page 249: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/249.jpg)
118J. Neira, VIBOT, 2008
Keble College, Oxford (290m)
![Page 250: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/250.jpg)
119J. Neira, VIBOT, 2008
Results• Local Map building in real-time @30Hz
– 60 features per map using inverse depth– Bigger maps if converted to (x,y,z)
• Joint Compatibility search adds only 2ms in the worst case
• Map-to-map matching in 1s (in Matlab)– With a new algorithm based on graph theory
• Loop optimization takes 800ms (6 iterations)
• The scale drifts along the map
![Page 251: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/251.jpg)
120J. Neira, VIBOT, 2008
Conclusions
future workexcellentoverkillHierarchical SLAMRelocation
---weakexcellentMap JoiningJoint Compatib.
------inconsistent
very goodEKF-SLAMJoint Compatib.
------inconsistent
weakEKF-SLAMNearest Neighbor
Longer loopLoop 300mLoop 30m
![Page 252: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/252.jpg)
121J. Neira, VIBOT, 2008
Recommended Readings• J.D. Tardós, J. Neira, P. Newman, and J. Leonard. Robust Mapping and
Localization in Indoor Environments using Sonar Data, Int. J. RoboticsResearch, Vol. 21, No. 4, April 2002, pp 311 –330
• J.A. Castellanos, J. Neira, J.D. Tardós, Limits to the Consistency of EKF-basedSLAM, 5th IFAC Symposium on Intelligent Autonomous Vehicles, Lisbon, July 2004.
• C. Estrada, J. Neira, J.D. Tardós, Hierarchical SLAM: real-time accurate mappingof large environments. To appear in the IEEE Transactions on Robotics
• Grimson, W. E. L., Object Recognition by Computer: The Role of Geometric Constraints, The MIT Press, Cambridge, Mass., 1990
• José A. Castellanos and Juan D. Tardós, Mobile Robot Localization and Map Building: A Multisensor Fusion Approach, Kluwer Ac. Pub., Boston, 1999
• J. Neira and J.D. Tardós, Data Association in Stochastic Mapping Using the Joint Compatibility Test, IEEE Trans. Robotics and Automation, vol. 17, no. 6, pp. 890-897, Dec 2001.
• P.M. Newman, J.J. Leonard, J. Neira and J.D. Tardós: Explore and Return: Experimental Validation of Real Time Concurrent Mapping and Localization. IEEE Int. Conf. Robotics and Automation, May, 2002.
• J. Neira, J.D. Tardós, J.A. Castellanos, Linear time vehicle relocation in SLAM. 2003 IEEE Int. Conf. Robotics and Automation, Taipei, Taiwan, May, 2003.
• D. Ortín, J. Neira, J.M.M. Montiel, Relocation using Laser and Vision. 2004 IEEE Int. Conf. Robotics and Automation, New Orleans, USA, April, 2004.
![Page 253: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/253.jpg)
122J. Neira, VIBOT, 2008
INFORMATION
1st Summer School 2002, Stockholm
http://www.cas.kth.se/SLAM/
2nd Summer School 2004, Toulouse
http://www2.laas.fr/SLAM/
3rd Summer Schhol 2006, Oxford
http://www.robots.ox.ac.uk/~SSS06/
![Page 254: The EKF Solution to Simultaneous Localization and Mappingwebdiis.unizar.es/~neira/5007439/VIBOT 2008 - EKF SLAM.pdf · J. Neira, Vibot 2007-2008 5 From Wikipedia: • Simultaneous](https://reader030.fdocuments.us/reader030/viewer/2022040608/5ec87b8fb4138a4902462d79/html5/thumbnails/254.jpg)
123J. Neira, VIBOT, 2008
BIBLIOGRAPHY