1. Reliable Range based Localization and SLAM Joseph Djugash Masters Student Presenting work done by: Sanjiv Singh, George Kantor, Peter Corke and Derek Kurth

2. Motivation

4. Introduction  Much research has been done to perform localization under normal/ideal conditions  Classical sensors fail to provide reliable results under non-ideal scenarios  Alternative methods such as range- only sensors have not received enough attention in the research field

5. Outline  Introduction  Range-Only Sensor  Localization  SLAM  Future Work

6. Range & Bearing Sensors  Errors in estimation of robot location and landmark locations are represented as ellipses.  Each landmark ellipse contains the error of both the robot’s current error and the error within the sensors. Robot Landmark

7. Range-Only Sensors  We are provided with an annulus instead of an ellipse.  Extending classical approaches to localization requires additional considerations. Robot Landmark

8. Range-Only Sensors r1r1 r2r2

9. Outline  Introduction  Range-Only Sensor  Localization Kalman Filter Particle Filter Results  SLAM  Future Work

10. Predictor  Corrector  Iterative Process Predict the new state (and its uncertainty) based on current state and process model Correct state estimate with new measurement CorrectorPredictor

11. Outline  Introduction  Range-Only Sensor  Localization Kalman Filter Particle Filter Results  SLAM  Future Work

12. Kalman Filter  Belief Representation Error Function – Gaussian Mean and Covariance  Process Model (State q k = [x r, y r, θ r ] T ) q k = A·q k-1 + B·u k-1 + w k-1 P k = A·P k-1 ·A T + B·U k-1 ·B T + Q k-1  Measurement Model q k = q k-1 + K k ·(z k – H·q k-1 ) K k = P k ·H T ·(H·P k ·H T + R k ) -1 P k = (I – K k ·H)·P k ^ –^ – – + ^ +^ + ^ +^ + ^ –^ – ^ –^ – – – + –

13. Kalman Filter  Belief Representation Error Function – Gaussian Mean and Covariance  Process Model (State q k = [x r, y r, θ r ] T ) q k = A·q k-1 + B·u k-1 + w k-1 P k = A·P k-1 ·A T + B·U k-1 ·B T + Q k-1  Measurement Model q k = q k-1 + K k ·(z k – H·q k-1 ) K k = P k ·H T ·(H·P k ·H T + R k ) -1 P k = (I – K k ·H)·P k ^ –^ – – + ^ +^ + ^ +^ + ^ –^ – ^ –^ – – – + – Last Relative Measurement. (∆d, ∆θ)

14. Kalman Filter  Belief Representation Error Function – Gaussian Mean and Covariance  Process Model (State q k = [x r, y r, θ r ] T ) q k = A·q k-1 + B·u k-1 + w k-1 P k = A·P k-1 ·A T + B·U k-1 ·B T + Q k-1  Measurement Model q k = q k-1 + K k ·(z k – H·q k-1 ) K k = P k ·H T ·(H·P k ·H T + R k ) -1 P k = (I – K k ·H)·P k ^ –^ – – + ^ +^ + ^ +^ + ^ –^ – ^ –^ – – – + – Measurement range to beacon Estimated range to beacon Measurement variance

15. Kalman Filter  Advantages: Computationally Efficient Able to handle high dimensionality with limited or no extra computational cost Handles short periods of sensor silence  Disadvantages: Able to represent only Gaussian distributions Assumptions are too restrictive

16. Outline  Introduction  Range-Only Sensor  Localization Kalman Filter Particle Filter Results  SLAM  Future Work

17. Particle Filter  Representing belief by sets of samples or particles  Each particle is represented as (x p, y p ), (orientation is not maintained)  Updating procedure is a sequential importance sampling approach with re- sampling  Sampling – Standard Gaussian Formula: P(x|r m ) = e ( ) Where r m is the measured range and r is the range estimate from the particle to beacon 1. σ√2 π -(r – r m ) 2 2σ ^ ^

18. Particle Filter  Advantages: Able to represent arbitrary density Converging to true posterior even for non-Gaussian and nonlinear system Efficient in the sense that particles tend to focus on regions with high probability  Disadvantages: Worst-case complexity grows exponentially in the dimensions

19. Outline  Introduction  Range-Only Sensor  Localization Kalman Filter Particle Filter Results  SLAM  Future Work

20. The Experiments

21. Dead Reckoning – Results

22. Kalman Filter – Results XTEATE Mean Abs. Error 0.5539 m0.3976 m Max. Error 1.9033 m2.0447 m Std (σ) 0.4173 m0.3558 m

23. Particle Filter – Results XTEATE Mean Abs. Error 2.8200 m2.0898 m Max. Error 8.6526 m7.7012 m Std (σ) 1.0345 m1.1943 m

24. Outline  Introduction  Range-Only Sensor  Localization  SLAM Batch Slam Kalman Filter Results  Future Work

25. SLAM  Beacon Locations are unknown  Measurements are used to predict beacon locations  Due to errors in measurements, not all measurements can be weighed equally  Consistency between inliers help provide a reliable estimates

26. Outline  Introduction  Range-Only Sensor  Localization  SLAM Batch Slam Kalman Filter Results  Future Work

27. Batch Slam  Approaches the SLAM problem by solving two non-linear optimization problems: One to generate the initial estimate of the beacon locations One to simultaneously refine the vehicle and beacon estimates  Estimated Beacon locations are feed to the Kalman filter localization algorithm

28. Batch Slam  Initializing the Beacons Assumes robot’s odometry is perfect Using the range measurements predicts the most likely beacon estimates Estimates are acquired by minimizing the cost function: and,  Refining estimates Assumes error distributions of each measurement is independent Most likely beacon positions and vehicle relative motion can be found by minimizing the cost function:

29. Batch Slam  Advantages: Produces accurate estimates of beacon locations Requires very little data to acquire good results  Disadvantages: Computationally Expensive Requires fairly accurate dead reckoning data to acquire its initial beacon estimate

30. Outline  Introduction  Range-Only Sensor  Localization  SLAM Batch Slam Kalman Filter Results  Future Work

31. Beacon Initialization  Find all pair wise intersections of a set of range measurements  Create a histogram grid with the circle intersections  Find the first two peaks on the grid  When the ratio between the peaks reaches a threshold (set to ‘2’), declare the higher of the peaks as the beacon location

32. Beacon Initialization

33. Kalman Filter SLAM  Kalman filter localization algorithm can be easily extended for SLAM  The state vector becomes: q k = [x r, y r, θ k, x b1, y b1, …, x bn, y bn ] T  As new beacons are initialized, expand the state vector and covariance matrix to the correct dimension q ~ 2n+3 P ~ 2n+3 square where n is the number of initialized beacons

34. Kalman Filter SLAM  Advantages: Similar to Kalman Filter Localization Settles to locally accurate solution  Disadvantages: Wrong Beacon Initialization could lead to diverged solution

35. Outline  Introduction  Range-Only Sensor  Localization  SLAM Batch Slam Kalman Filter Results  Future Work

36. Kalman Filter SLAM – Results RawXTEATE Mean Abs. Error 8.5544 m5.1776 m Max. Error 18.0817 m19.2575 m Std (σ) 4.8216 m4.5486 m Aff. Trans.XTEATE Mean Abs. Error 0.7297 m0.6872 m Max. Error 2.6787 m2.7621 m Std (σ) 0.6004 m0.5745 m

37. Kalman Filter & Batch Slam – Results (Another Example) Batch SlamXTEATE Mean Abs. Error 1.5038 m2.0871 m Max. Error 4.9149 m5.8212 m Std (σ) 1.0527 m1.4968 m Batch Slam using only 5% of data set

38. Outline  Introduction  Range-Only Sensor  Localization  SLAM  Future Work

39. Future Work  Develop robust algorithms that produce reliable results with poor sensor data  Develop an approach that relies on multiple algorithms at various points during the data set to produce better results

40. Questions…