Upcoming SlideShare
×

# Lecture 09: SLAM

2,229 views
1,976 views

Published on

1 Like
Statistics
Notes
• Full Name
Comment goes here.

Are you sure you want to Yes No
• Be the first to comment

Views
Total views
2,229
On SlideShare
0
From Embeds
0
Number of Embeds
123
Actions
Shares
0
63
0
Likes
1
Embeds 0
No embeds

No notes for slide

### Lecture 09: SLAM

1. 1. Probabilistic State Estimation With Application To Vehicle Navigation Matthew Kirchner Naval Air Warfare Center – Weapons Division Department of ECEE – University of Colorado at Boulder November 1, 2010
2. 2. Topics • Why? • Review • Background • Kalman Filter • Particle Filter • SLAM
3. 3. Why Probability? • Real sensors have uncertainty • May have multiple sensors • Some states are not directly observable • Ambiguous sensor observations
4. 4. Importance of Bayes Rule Bayes Rule Recursive Bayesian Estimation Linear Kalman Filter Unscented Kalman Filter Extended Kalman Filter EKF SLAM FastSLAMParticle Filter FastSLAM
5. 5. Bayes Rule )( )()|( )|( BP APABP BAP  Prior Likelihood Posterior Normalizing Constant
6. 6. Equivalent Bayes Rule )()|()|( APABPBAP  )()|()|( APABPBAP 
7. 7. Bayes Rule Example • School: 60% boys and 40% girls • All boys wear pants • Half of girls wear skirts, half wear pants • You see a random student and can only tell they are wearing pants. • Based on your observation, what is the probability the student you saw is a girl?
8. 8. Bayes Rule Example • School: 60% boys and 40% girls • All boys wear pants • Half of girls wear skirts, half wear pants • You see a random student and can only tell they are wearing pants. • Based on your observation, what is the probability the student you saw is a girl? • We want to find: – P(Student=Girl | Clothes=Pants) • Prior? – P(Student=Girl) = 0.4 • Likelihood? – P(Clothes=Pants | Student=Girl) = 0.5 • Normalizing Constant? – P(Clothes=Pants) = 0.8 • Bayes Rule! – P(Student=Girl | Clothes=Pants) = (0.5)*(0.4)/(0.8) = 0.25
9. 9. Gaussian 1D 2D
10. 10. Gaussian 1D 2D
11. 11. Gaussian • 2D Probability density function described by mean vector and covariance matrix • 1D Probability density function described by mean and variance ),(~ 2 Nx                       2 221 12 2 1 2 1 2 1 ),(~          NX x x X
12. 12. Functions of Random Variables
13. 13. Functions of Random Variables • Linear function • Mean: • Covariance: • Linear functions only! xFy  T xy FF  xFy 
14. 14. Kalman Filter • Introduced in 1960 by Rudolf Kalman • Many applications: – Vehicle guidance systems – Control systems – Radar tracking – Object tracking in video – Atmospheric models
15. 15. Kalman Filter ),|( ttt uzxPWe want to find:
16. 16. Kalman Filter Process Model Process Model Observation Model Observation Model Observation Model
17. 17. Kalman Filter • Process Model: – Deterministic: – Probabilistic: • Used to calculate prior distribution • Observation Model: – Deterministic: – Probabilistic: • Used to calculate likelihood distribution ),( 1 ttt uxfx  )( tt xfz  ),,(),|( 11 tttttt wuxfuxxP   ),()|( tttt vxfxzP 
18. 18. Kalman Filter: Assumptions • Underlying system is modeled as Markov – • All beliefs are Gaussian distributions – Additive zero mean Gaussian noise • Linear process and observation models – Process Model: • – Observation Model: • ),,,|()|( 3211   tttttt xxxxPxxP ttt vHxz  tttt wBuFxx  1 ),0(~ QNwt ),0(~ RNvt
19. 19. Kalman Filter Steps 1. Using process model, previous state, and controls, find prior – Sometimes called ‘predict’ step, or ‘a priori’ 2. Using prior and observation model, find sensor likelihood – If I knew state, what should sensors read? 3. Find observation residual – Difference between actual sensor values and what was calculated in step 2. Also sometimes called ‘innovation’.
20. 20. Kalman Filter Steps 4. Compute Kalman gain matrix 5. Using Kalman gain and prior, calculate posterior – Sometimes called ‘correct’ step or ‘a posteriori’
21. 21. Kalman Filter Steps • Algorithm Kalman_Filter( ) – 1a: – 1b: – 2: – 3: – 4: – 5a: – 5b: • Return( ) ttttt uBxFx  1 ˆ t T tttt QFF  1 ˆ ttt xHz ˆˆ  ttt zzz ˆ 1 )ˆ(ˆ   t T ttt T ttt RHHHK tttt zKxx  ˆ tttt HKI  ˆ)( tttt zux ,,, 11   ttx ,
22. 22. Kalman Filter
23. 23. Kalman Filter Example 1txtx
24. 24. Kalman Filter Example Root Mean Squared Error (RMSE) KF 1.0030m GPS Only 3.6840m
25. 25. Kalman Filter Example GPS Lost GPS Reacquired
26. 26. Kalman Filter Example
27. 27. Kalman Filter • O(k^2.4+n^2) • Many real systems are non linear – Extended Kalman filter – Unscented Kalman filter – Particle filter • Some systems are non-Gaussian – Particle filter
28. 28. Extended Kalman Filter • Linearize process and observation models – By finding Jacobian matrices – Analytically or numerically • Then use regular Kalman filter algorithm • Sub-optimal
29. 29. 29 EKF Linearization
30. 30. EKF Linearization
31. 31. Unscented Kalman Filter
32. 32. Unscented Kalman Filter EKF UKF
33. 33. Particle Filter
34. 34. Particle Filter • Represent distribution as set of randomly generated samples, called ‘particles’. • Functions can be nonlinear and non-gaussian • Multi-hypothesis belief propagation
35. 35. Particle Filter • Sample the prior • Compute likelihood of particles given measurement – Also called particle ‘weights’ • Sample posterior: Sample from particles proportional to particle weights – Also called ‘resampling’ or ‘importance sampling’
36. 36. Simultaneous Localization and Mapping • SLAM Problem: – Need map to localize – Need location to make map • Brainstorming: How can we solve this problem? – Map could be locations of landmarks or occupancy grid • Kalman filter based: landmark positions part of state variables • Particle filter based: landmark positions or occupancy map included in each particle ),|,( ttt uzmxP
37. 37. Feature-based SLAM
38. 38. FastSLAM - Example
39. 39. Other Probabilistic Applications
40. 40. Other Probabilistic Applications