1. Simultaneous Localization and Mapping Presented by Lihan He Apr. 21, 2006

2. Outline  Introduction  SLAM using Kalman filter  SLAM using particle filter Particle filter SLAM by particle filter  My work : searching problem

3. Introduction: SLAM SLAM: Simultaneous Localization and Mapping A robot is exploring an unknown, static environment. Given:  The robot’s controls  Observations of nearby features Estimate:  Location of the robot -- localization where I am ?  Detail map of the environment – mapping What does the world look like? The controls and observations are both noisy.

4. Introduction: SLAM Markov assumption x0x0 x1x1 x2x2 x t-1 xtxt … o1o1 o t-1 … a1a1 a t-1 … o2o2 otot a2a2 atat m State transition: Observation function:

5. Introduction: SLAM Method: Sequentially estimate the probability distribution p(x t ) and update the map. Prior: p(x 0 ) p(x 1 ) p(m 1 ) or m 1 … p(x t-1 ) p(m t-1 ) or m t-1 p(x t ) p(m t ) or m t Prior distribution on x t after taking action a t

6. Introduction: SLAM Map representations 1. Landmark-based map representation 2. Grid-based map representation Track the positions of a fixed number of predetermined sparse landmarks. Observation: estimated distance from each landmark. Map is represented by a fine spatial grid, each grid square is either occupied or empty. Observation: estimated distance from an obstacle using a laser range finder.

7. Introduction: SLAM Methods: 1. Parametric method – Kalman filter 2. Sample-based method – particle filter The robot’s trajectory estimate is a tracking problem Represent the distribution of robot location x t (and map m t ) by a Normal distribution Sequentially update μ t and Σ t Represent the distribution of robot location x t (and map m t ) by a large amount of simulated samples. Resample x t (and m t ) at each time step

8. Introduction: SLAM Why is SLAM a hard problem? Robot location and map are both unknown. Location error Map error  The small error will quickly accumulated over time steps.  The errors come from inaccurate measurement of actual robot motion (noisy action) and the distance from obstacle/landmark (noisy observation). When the robot closes a physical loop in the environment, serious misalignment errors could happen.

9. SLAM: Kalman filter Update equation: Assume: Prior p(x 0 ) is a normal distribution Observation function p(o|x) is a normal distribution Then: Posterior p(x 1 ), …, p(x t ) are all normally distributed. Mean μ t and covariance matrix Σ t can be derived analytically. Sequentially update μ t and Σ t for each time step t

10. SLAM: Kalman filter Assume: State transition Observation function Kalman filter: Propagation (motion model): Update (sensor model):

11. SLAM: Kalman filter The hidden state for landmark-based SLAM: localizationmapping Map with N landmarks: (3+2N)-dimentional Gaussian State vector x t can be grown as new landmarks are discovered.

12. SLAM: particle filter Update equation: Idea:  Normal distribution assumption in Kalman filter is not necessary  A set of samples approximates the posterior distribution and will be used at next iteration.  Each sample maintains its own map; or all samples maintain a single map.  The map(s) is updated upon observation, assuming that the robot location is given correctly.

13. SLAM: particle filter Assume it is difficult to sample directly from But get samples from another distribution is easy. We sample from, with normalized weight for each x i t as The set of (particles) is an approximation of Particle filter: Resample x t from,with replacement, to get a sample set with uniform weights

14. SLAM: particle filter Particle filter (cont’d): 0.2 0.3 0.4 0.1

15. SLAM: particle filter Choose appropriate Choose Then Transition probability Observation function

16. SLAM: particle filter Algorithm: Let state x t represent the robot’s location, 1. Propagate each state through the state transition probability. This samples a new state given the previous state. 2. Weight each new state according to the observation function 3. Normalize the weights, get. 4. Resampling: sample N s new states from are the updated robot location from

17. SLAM: particle filter State transition probability: are the expected robot moving distance (angle) by taking action a t. Observation probability: Measured distance (observation) for sensor k Map distance from location x t to the obstacle

18. SLAM: particle filter Lots of work on SLAM using particle filter are focused on:  Reducing the cumulative error  Fast SLAM (online)  Way to organize the data structure (saving robot path and map). Maintain single map: cumulative error Multiple maps: memory and computation time In Parr’s paper:  Use ancestry tree to record particle history  Each particle has its own map (multiple maps)  Use observation tree for each grid square (cell) to record the map corresponding to each particle.  Update ancestry tree and observation tree at each iteration.  Cell occupancy is represented by a probabilistic approach.

19. SLAM: particle filter

20. Searching problem  The agent doesn’t have map, doesn’t know the underlying model, doesn’t know where the target is.  Agent has 2 sensors: Camera: tell agent “occupied” or “empty” cells in 4 orientations, noisy sensor. Acoustic sensor: find the orientation of the target, effective only within certain distance.  Noisy observation, noisy action. Assumption:

21. Searching problem Differences from SLAM  Rough map is enough; an accurate map is not necessary.  Objective is to find the target. Robot need to actively select actions to find the target as soon as possible. Similar to SLAM  To find the target, agent need build map and estimate its location.

22. Searching problem Model:  Environment is represented by a rough grid;  Each grid square (state) is either occupied or empty.  The agent moves between the empty grid squares.  Actions: walk to any one of the 4 directions, or “stay”. Could fail in walking with certain probability.  Observations: observe 4 orientations of its neighbor grid squares: “occupied” or “empty”. Could make a wrong observation with certain probability.  State, action and observation are all discrete.

23. Searching problem In each step, the agent updates its location and map:  Belief state: the agent believes which state it is currently in. It is a distribution over all the states in the current map.  The map: The agent thinks what the environment is. For each state (grid square), a 2-dimentional Dirichlet distribution is used to represent the probability of “empty” and “occupied”. The hyperparameters of Dirichlet distribution are updated based on current observation and belief state.

24. Searching problem Belief state update: where Probability of successful moving from s j to s when taking action a From map representation and with Neighbor of s in orientation j the set of neighbor states of s

25. Searching problem Map update (Dirichlet distribution): Assume at step t-1, the hyperparameter for state S is At step t, the hyperparameter for state S is updated as and are the posterior after observing o given that the agent is in the neighbor of state s. If the probability of wrong observation for any orientation is p 0, then priorp 0 if o is “occupied” 1-p 0 if o is “empty” can be computed similarly.

26. Searching problem Belief state update: 0 010 0 0.95 0.05 0.95 1010 0.05 0.95 Map representation update: 0 00.0010 0 0.0790.9180 00.0010 0 0.000 0.002 0.000 1.026 0.054 0.873 0.046 0.001 0.000 0.126 0.954 1.953 0.047 1.943 0.054 0.872 0.046 0.001 0.055 1.025 0.046 0.872 0.000 0.001 a=“right” a=“up” 0 00.0010 00.0000.2110.7450 00.0000.0010.0020.0330.0090 00.000 0 0 0 0 0.001 0.000 0.202 0.011 0.708 0.037 0.000 0.203 0.011 1.947 0.091 1.849 0.058 0.716 0.038 0.000 0.002 0.000 0.128 0.954 2.186 0.059 2.693 0.092 0.912 0.048 0.009 0.001 0.000 0.002 0.001 0.057 1.025 0.078 0.874 0.009 0.001 0.000 0.001 0.000 a=“right” a=“up”

27. Searching problem Choose actions: Each state is assigned a reward R(s) according to following rules: Less explored grids have higher reward. Try to walk to the “empty” grid square. Consider neighbor of s with priority. xx