Lecture 09: SLAM

2,116 views
1,918 views

Published on

0 Comments
1 Like
Statistics
Notes
  • Be the first to comment

No Downloads
Views
Total views
2,116
On SlideShare
0
From Embeds
0
Number of Embeds
120
Actions
Shares
0
Downloads
63
Comments
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

×