1. Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal
Dpt. of System Engineering
and Automation
University of Málaga
(Spain)
A Pure Probabilistic Approach
to Range-Only SLAM
Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal
May 19-23
Pasadena, CA (USA)

2. Outline of the talk 1. RO-SLAM: the RBPF approach
2. The observation model
3. Map update
4. Experiments
5. Conclusions

3. Outline of the talk 1. RO-SLAM: the RBPF approach
2. The observation model
3. Map update
4. Experiments
5. Conclusions

4. 1. RO-SLAM: the RBPF approach
Range Only (RO) SLAM:
Localization & Mapping with range-only devices.
Our purpose:
To enable a vehicle to localize itself using RO devices, without any
previous information about the location of the beacons.
Typical technologies:
Radio, sonars.

5. 1. RO-SLAM: the RBPF approach
Advantages of RO-SLAM (depending on technologies):
No need for line-of-sight between vehicle-beacons.
Artificial beacons, can identify themselves: no data-association problem.
Drawback of RO-SLAM (always):
The high ambiguity of all measurements.
Robot poses
Two likely positions

6. 1. RO-SLAM: the RBPF approach
Why is it difficult to integrate RO-SLAM in a probabilistic framework?
Multi-modality: With RO sensors, everything is multimodal by nature:
- In global localization  vehicle location hypotheses [not in this paper]
- In SLAM  beacon location hypotheses [addressed here].

7. 1. RO-SLAM: the RBPF approach
Why is it difficult to integrate RO-SLAM in a probabilistic framework?
Multi-modality: With RO sensors, everything is multimodal by nature:
- In global localization  vehicle location hypotheses [not in this paper]
- In SLAM  beacon location hypotheses.
Strongly non-linear problem, with non-Gaussian densities.
- Classic approach to SLAM (EKF) is inappropriate to RO-SLAM:
a covariance matrix is incapable of capturing the relations between
all the variables.
Alternative implementation in this work:
Rao-Blackwellized Particle Filter (RBPF)

8. 1. RO-SLAM: the RBPF approach
The Rao-Blackwellized Particle Filter (RBPF) approach
The full SLAM posterior can be separated into:
- Robot path: estimated by a set of particles.
- The map: only conditional distributions, for each path hypothesis.
The covariances are represented implicitly by the particles,
rather than explicitly  easier!

9. 1. RO-SLAM: the RBPF approach
Taking advantage of conditional independences
Instead of keeping the joint map posterior, we can estimate each beacon
independently:
Robot path
Beacon 1
Beacon 2
Beacon 3
Robot path
Beacon 1
Robot path
Beacon 2
Robot path
Beacon 3

10. 1. RO-SLAM: the RBPF approach
The key insight of our approach:
Each beacon, at each particle, can be represented by the probability
distribution that best fits the current uncertainty.
 The first time a beacon is observed, it is inserted as a second
particle filter.
 Eventually, it can be approximated by a Gaussian, then it becomes
a Gaussian and it is updated through an EKF.
Robot path
Beacon 1
Robot path
Beacon 2
Robot path
Beacon 3

11. 1. RO-SLAM: the RBPF approach
The benefits of our approach:
New beacons can be inserted into the map at any time: they are immediately used to improve robot localization.
Computational complexity dynamically adapts to the uncertainty.
Unified Bayesian framework: it’s not a two-stage algorithm:
[Kantor, Singh ICRA02], [Kurth, et al. 2003]: EKF, assuming initial gross estimate of beacons.
[Singh, et al. ICRA03]: Delayed initialization of beacons.
[Newman & Leonard ICRA03]: Least square, batch optimization.
[Olson et al. 2004], [Djugash et al. ICRA06]: Two steps, first probability grid for beacons,
then converge to EKF.

12. Outline of the talk 1. RO-SLAM: the RBPF approach
2. The observation model
3. Map update
4. Experiments
5. Conclusions

13. 2. The observation model
Sensor model: (optional) bias + additive Gaussian noise
p(z)
Bias
z (sensed range)
Actual range

14. 2. The observation model Sensor model:
Implementation depends on representation of beacons “m”.
In general, it is the integral over all the potential beacon positions:
Robot path
Robot path

15. 2. The observation model Comparison of both methods: Small uncertainty
Observation likelihood
(Monte Carlo)
Observation likelihood
(Gaussian)
Beacon position uncertainty
-0.5
0.5 1
1.5 -1
Real robot
location
Beacon position samples
and Gaussian fit. -1 1
Real robot
location -1 1
Real robot
location
Small uncertainty
Gaussian is acceptable
-0.5
0.5 1
1.5 -1
Real robot
location
-0.5
0.5 1
1.5 -1
Real robot
location
Large uncertainty
Must use samples

16. Outline of the talk 1. RO-SLAM: the RBPF approach
2. The observation model
3. Map update
4. Experiments
5. Conclusions

17. 3. Map update
With each iteration, new measurements are integrated into the map:
We can find three situations to implement this:
- The beacon is inserted into the map for the first time.
- The beacon is represented by samples.
- The beacon is represented by a Gaussian.

18. 3. Map update Case 1: First insertion into the map
Equally weighted particles are generated along a “thick ring” centered
at the sensor:
Radius:
sensed range
Sigma: sensor noise

19. 3. Map update Case 2: Update of a beacon represented by samples
Only the importance weights of samples are modified:
One sample of the
beacon density

20. 3. Map update Case 2: Update of a beacon represented by samples
Only the importance weights of samples are modified:
Keys for efficiency:
One sample of the
beacon density
 Remove samples with very small weights.
 Eventually, a Gaussian becomes a good approximation.
Simple & fast criterion: check for maximum size of the
Gaussian fit of the samples.

21. The Gaussian represents
3. Map update
Case 3: Update of a beacon represented by a Gaussian
In this case, updating each beacon is done through a standard EKF.
The Gaussian represents
the beacon density

22. Outline of the talk 1. RO-SLAM: the RBPF approach
2. The observation model
3. Map update
4. Experiments
4.1. UWB radio beacons
4.2. Synthetic data
5. Conclusions

23. 4.1. Experiments: UWB radio beacons
Ultra Wide Band (UWB) technology:
Measure time-of-flight of short radio pulses.
Spread spectrum for robustness against multi-path.
It does not require line-of-sight.
We have used 3 beacons + 1 mobile transceiver on the robot.
[Timedomain – PulsOn]

24. 4.1. Experiments: UWB radio beacons
The experimental setup:
Beacon #3
Beacon #2
Beacon #1
Onboard
UWB device

25. 4.1. Experiments: UWB radio beacons
Evolution of the estimated map:
Beac. #2
Beac. #3
Beac. #1
1 m
Time step 0
Beac. #2
Beac. #3
Beac. #1
Time step 15
Robot path
Beac. #2
Beac. #3
Beac. #1
Time step 45

26. 4.1. Experiments: UWB radio beacons

27. Outline of the talk 1. RO-SLAM: the RBPF approach
2. The observation model
3. Map update
4. Experiments
4.1. UWB radio beacons
4.2. Synthetic data
5. Conclusions

28. 4.2. Experiments: synthetic data
Simulation of a circular path while observing 15 beacons.
Evolution of the estimated map
Time step #0
2 m
Time step #10 #8 #7 #6
#14 #3 #5 #4
#13 #1 #9
#10
Time step #30
Robot path #4 #8 #9 #1 #7 #6
#13
#14 #3 #5
#11
#12
#10
Time step #100
Robot path #4 #8 #9 #1 #7 #6
#13
#14 #3 #5
#11
#12
#10 #0

29. 4.2. Experiments: synthetic data
Simulation of a circular path while observing 15 beacons.
Beacon errors (m) 6
Added #11
Added #12
Added #0
Added #2 5 4 3 2 1 20 40 60 80
100
120
Time steps
160
Computation time (sec)
100 10
Average: 0.69 sec 1
0.1
0.01 20 40 60 80
100
120
Time steps
160

30. 4.2. Experiments: synthetic data

31. Outline of the talk 1. RO-SLAM: the RBPF approach
2. The observation model
3. Map update
4. Experiments
5. Conclusions

32. 5. Conclusions We have presented a consistent probabilistic
framework for Bayesian RO-SLAM.
New beacons can be added to the map at any time.
The density representations adapt dynamically.
Tested with real UWB sensors.
Future work:
Explore more efficient representations of pdfs.

33. Final remark… http://mrpt.sourceforge.net/
Source code (MRPT C++ libs), datasets, slides and instructions to reproduce the experiments available online:
papers
ICRA 08

34. Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal
Dpt. of System Engineering
and Automation
University of Málaga
(Spain)
A Pure Probabilistic Approach
to Range-Only SLAM
Jose-Luis Blanco, Javier González, Juan-Antonio Fernández-Madrigal
Thanks for your attention!