Create Presentation
Download Presentation

Download Presentation
## Markov Localization & Bayes Filtering

- - - - - - - - - - - - - - - - - - - - - - - - - - - E N D - - - - - - - - - - - - - - - - - - - - - - - - - - -

**Markov Localization & Bayes Filtering**with Kalman Filters Discrete Filters Particle Filters Slides adapted from Thrun et al., Probabilistic Robotics**Markov Localization**The robot doesn’t know where it is. Thus, a reasonable initial believe of it’s position is a uniform distribution.**Markov Localization**A sensor reading is made (USE SENSOR MODEL) indicating a door at certain locations (USE MAP). This sensor reading should be integrated with prior believe to update our believe (USE BAYES).**Markov Localization**The robot is moving (USE MOTION MODEL) which adds noise.**Markov Localization**A new sensor reading (USE SENSOR MODEL) indicates a door at certain locations (USE MAP). This sensor reading should be integrated with prior believe to update our believe (USE BAYES).**Markov Localization**The robot is moving (USE MOTION MODEL) which adds noise. …**Normalization**Algorithm:**Recursive Bayesian Updating**Markov assumption: znis independent of z1,...,zn-1if we know x.**Putting oberservations and actions together: Bayes Filters**• Given: • Stream of observations z and action data u: • Sensor modelP(z|x). • Action modelP(x|u,x’). • Prior probability of the system state P(x). • Wanted: • Estimate of the state X of a dynamical system. • The posterior of the state is also called Belief:**Graphical Representation and Markov Assumption**Underlying Assumptions • Static world • Independent noise • Perfect model, no approximation errors**Bayes**Markov Total prob. Markov Markov z = observation u = action x = state Bayes Filters**Prediction**• Correction**Bayes Filter Algorithm**• Algorithm Bayes_filter( Bel(x),d ): • h=0 • Ifd is a perceptual data item z then • For all x do • For all x do • Else ifd is an action data item uthen • For all x do • ReturnBel’(x)**Bayes Filters are Familiar!**• Kalman filters • Particle filters • Hidden Markov models • Dynamic Bayesian networks • Partially Observable Markov Decision Processes (POMDPs)**Probabilistic Robotics**Bayes Filter Implementations Gaussian filters**m**Univariate -s s Gaussians Linear transform of Gaussians**Multivariate Gaussians**• We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations.**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**Linear Gaussian Systems: Initialization**• Initial belief is normally distributed:**Linear Gaussian Systems: Dynamics**• Dynamics are linear function of state and control plus additive noise:**Linear Gaussian Systems: Observations**• Observations are linear function of state plus additive noise:**Kalman Filter Algorithm**• Algorithm Kalman_filter( mt-1,St-1, ut, zt): • Prediction: • Correction: • Returnmt,St**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!**Nonlinear Dynamic Systems**• Most realistic robotic problems involve nonlinear functions**EKF Linearization: First Order Taylor Series Expansion**• Prediction: • Correction:**EKF Algorithm**• Extended_Kalman_filter( mt-1,St-1, ut, zt): • Prediction: • Correction: • Returnmt,St**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)**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!**Kalman Filter-based System**• [Arras et al. 98]: • Laser range-finder and vision • High precision (<1cm accuracy) [Courtesy of Kai Arras]**Localization With MHT**• Belief is represented by multiple hypotheses • Each hypothesis is tracked by a Kalman filter • Additional problems: • Data association: Which observation corresponds to which hypothesis? • Hypothesis management: When to add / delete hypotheses? • Huge body of literature on target tracking, motion correspondence etc.**MHT: Implemented System (2)**Courtesy of P. Jensfelt and S. Kristensen**Probabilistic Robotics**Bayes Filter Implementations Discrete filters**Discrete Bayes Filter Algorithm**• Algorithm Discrete_Bayes_filter( Bel(x),d ): • h=0 • Ifd is a perceptual data item z then • For all x do • For all x do • Else ifd is an action data item uthen • For all x do • ReturnBel’(x)**Probabilistic Robotics**Bayes Filter Implementations Particle filters**Particle Filters**• Represent belief by random samples • Estimation of non-Gaussian, nonlinear processes • Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter • Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96] • Computer vision: [Isard and Blake 96, 98] • Dynamic Bayesian Networks: [Kanazawa et al., 95]d**Importance Sampling**Weight samples: w = f / g