1 / 62

Probabilistic Robotics

Probabilistic Robotics. Bayes Filter Implementations Gaussian filters. Bayes Filter Reminder. Prediction Correction. m. Univariate. - s. s. m. Multivariate. Gaussians. Properties of Gaussians. Multivariate Gaussians.

nathaniel-gates
Download Presentation

Probabilistic Robotics

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Probabilistic Robotics Bayes Filter Implementations Gaussian filters

  2. Bayes Filter Reminder • Prediction • Correction

  3. m Univariate -s s m Multivariate Gaussians

  4. Properties of Gaussians

  5. Multivariate Gaussians • We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.

  6. Discrete Kalman Filter Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation with a measurement

  7. Components of a Kalman Filter Matrix (nxn) that describes how the state evolves from t to t-1 without controls or noise. Matrix (nxl) that describes how the control ut changes the state from t to t-1. Matrix (kxn) that describes how to map the state xt to an observation zt. Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Rt and Qt respectively.

  8. Representing Belief Column vector (nx1) that describes the mean of the Gaussian of the state Matrix (nxn) that is the covariant matrix of the Gaussian • Kalman Filter uses Gaussian to represent beliefe.

  9. Kalman Filter Updates in 1D

  10. Kalman Filter Updates in 1D

  11. Kalman Filter Updates in 1D

  12. Kalman Filter Updates

  13. Linear Gaussian Systems: Initialization • Initial belief is normally distributed:

  14. Linear Gaussian Systems: Dynamics • Dynamics are linear function of state and control plus additive noise:

  15. Linear Gaussian Systems: Dynamics

  16. Linear Gaussian Systems: Observations • Observations are linear function of state plus additive noise:

  17. Linear Gaussian Systems: Observations

  18. Kalman Filter Algorithm • Algorithm Kalman_filter( mt-1,St-1, ut, zt): • Prediction: • Correction: • Returnmt,St

  19. Prediction The Prediction-Correction-Cycle

  20. Correction The Prediction-Correction-Cycle

  21. Prediction Correction The Prediction-Correction-Cycle

  22. Kalman Filter Summary • Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2) • Optimal for linear Gaussian systems! • Most robotics systems are nonlinear!

  23. Nonlinear Dynamic Systems • Most realistic robotic problems involve nonlinear functions

  24. Linearity Assumption Revisited

  25. Non-linear Function

  26. EKF Linearization (1)

  27. EKF Linearization (2)

  28. EKF Linearization (3)

  29. EKF Linearization: First Order Taylor Series Expansion • Prediction: • Correction:

  30. EKF Algorithm • Extended_Kalman_filter( mt-1,St-1, ut, zt): • Prediction: • Correction: • Returnmt,St

  31. Localization “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91] • Given • Map of the environment. • Sequence of sensor measurements. • Wanted • Estimate of the robot’s position. • Problem classes • Position tracking • Global localization • Kidnapped robot problem (recovery)

  32. Landmark-based Localization

  33. EKF_localization ( mt-1,St-1, ut, zt,m):Prediction: Jacobian of g w.r.t location Jacobian of g w.r.t control Motion noise Predicted mean Predicted covariance

  34. EKF_localization ( mt-1,St-1, ut, zt,m):Correction: Predicted measurement mean Jacobian of h w.r.t location Pred. measurement covariance Kalman gain Updated mean Updated covariance

  35. EKF Prediction Step

  36. EKF Observation Prediction Step

  37. EKF Correction Step

  38. Estimation Sequence (1)

  39. Estimation Sequence (2)

  40. Comparison to GroundTruth

  41. EKF Summary • Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2) • Not optimal! • Can diverge if nonlinearities are large! • Works surprisingly well even when all assumptions are violated!

  42. Linearization via Unscented Transform EKF UKF

  43. UKF Sigma-Point Estimate (2) EKF UKF

  44. UKF Sigma-Point Estimate (3) EKF UKF

  45. Unscented Transform Sigma points Weights Pass sigma points through nonlinear function Recover mean and covariance

  46. Motion noise UKF_localization ( mt-1,St-1, ut, zt,m): Prediction: Measurement noise Augmented state mean Augmented covariance Sigma points Prediction of sigma points Predicted mean Predicted covariance

  47. Measurement sigma points UKF_localization ( mt-1,St-1, ut, zt,m): Correction: Predicted measurement mean Pred. measurement covariance Cross-covariance Kalman gain Updated mean Updated covariance

  48. EKF_localization ( mt-1,St-1, ut, zt,m):Correction: Predicted measurement mean Jacobian of h w.r.t location Pred. measurement covariance Kalman gain Updated mean Updated covariance

  49. UKF Prediction Step

  50. UKF Observation Prediction Step

More Related