Foundations of State Estimation Part II Topics: Hidden Markov Models Particle Filters ● Additional reading: ● L.R. Rabiner, “A tutorial on hidden Markov models," Proceedings of the IEEE, vol. 77, pp. 257-286, 1989. ● Sequential Monte Carlo Methods in Practice. A. Doucet, N. de Freitas, N. Gordon (eds.) Springer-Verlag, 2001. ● Radford M. Neal, 1993. Probabilistic Inference Using Markov Chain Monte Carlo Methods. University of Toronto CS Tech Report. ● Robust Monte Carlo Localization for Mobile Robots. S. Thrun, D. Fox, W. Burgard and F. Dellaert. Artificial Intelligence. 128:1-2, 99-141 (2001). Hidden Markov Models ● Discrete states, actions and observations – f(?,?,?), h(?,?) can now be written as tables States x 1 x 2 T(x j |a i , x i ) Z 2 b 1 Beliefs Z 1 Observations a 1 Actions O(z j |x i ) b 2 2 Hidden Observable (Somewhat) Useful for Localization in Topological Maps x 1 x 2 : p(x 2 |x 1 ,a)= .9 x 3 : p(x 3 |x 1 ,a)=.05 x 4 : p(x 4 |x 1 ,a)=.05 Observations can be features such as corridor features, junction features, etc. ● Estimating p t (x) is now easy ● After each action a t and observation z t , ?x∈X, update : ● This algorithm is quadratic in |X|. ● number of state features. Continuous X means infinite number of states.) Belief Tracking )()',|()|()( 1 ' xpxxxp t X ttt ?∑ = (Recall that Kalman Filter is quadratic in a x p z p The Three Basic Problems for HMMs 1 ,z 1 ,a 2 ,z 2 ,...,a T ,z T , and a model λ=(A,B,π), how do we efficiently compute P(O|λ), the probability of the history, given the model? 1 ,z 1 ,a 2 ,z 2 ,...,a T ,z T and a model λ, how do we choose a corresponding state sequence X=x 1 ,x 2 ,...,x T which is optimal in some meaningful sense (i.e., best “explains” the observations)? λ=(A,B,π) to maximize P(O|λ)? HMM Basic Problem 1 ● Probability of history O given λ is sum over all state sequences Q=x 1 ,x 2 ,x 3 ,...,x T ,: ● Summing over all state sequences is 2T?|X| T ● Instead, build lattice of states forward in time, computing probabilities of each possible trajectory as lattice is built ● Forward algorithm is |X| 2 T ∑ ∑ = = ,..., 22322112111 2 )...,|()|(),|()|()( )|(),|()|( Q Q 1 all all π λλλ 1) Given the history O=a 2) Given the history O=a 3) How do we adjust the model parameters q q a x x p x z p a x x p x z p x Q P O P O P HMM Basic Problem 1 )|()( 11 ii i πα = )|(),|()()( 1 || 1 1 it X j tijtt xji + = + ? ? ? ? ? ? = ∑ αα ∑ = = || 1 )()|( X i t iαλ 3) Termination 4) Back tracking HMM Basic Problem 2 ● algorithm, with an extra term 1) Initialization 2) Induction 0)( )|()( 1 11 = = i i ii ψ πδ [ ] [ ]),|()()( )|(),|()(max)( 1 ||1 1 ||1 jjit Xj t itjjit Xj t ji ji ? ? = = δψ δδ [ ] [ ])( )(max ||1 * ||1 ix iP TT T δ δ ? = = )( * 11 * ++ = ttt xx ψ 1. Initialization 2. Induction: Repeat for t=1:T 3. Termination: x z p z p a x x p O p Viterbi Decoding: Same principle as forward x z p max arg a x x p x z p a x x p ≤≤ ≤≤ max arg X i X i ≤≤ ≤≤ Implementation of the computation of (j) in terms of a lattice of observations t, and states j. Observation, t 1 1 2 S t a t e 2 3 T N a t HMM Basic Problem 3 ● D={x 1 ,a 1 ,z 1 ,x 2 ,a 2 ,z 2 ,...,x T ,a T ,z T }, estimating p(z i ,x i ) and p(x j |x i ,a k ) is just counting ● Given unlabelled data sequence, D={a 1 ,z 1 ,a 2 , z 2 , ...,a T ,z T }, estimating p(z i ,x i ) and p(x j |x i ,a k ) is equivalent to simultaneous localization and mapping (next lecture) Particle Filters Given labelled data sequence, Monte Carlo Localization: The Particle Filter ● Sample particles randomly from distribution ● Carry around particles, rather than full distribution ● Sampling from uniform distributions is easy ● Sampling from Gaussians (and other parameteric distributions) is a little harder ● What about arbitrary distributions? ● Many algorithms ● Rejection sampling ● Importance sampling ● Gibbs sampling ● Metropolis sampling ● …. State Space How to sample ● Want to sample from arbitrary p(x) ● Don’t know p() ● Do know p(x) for any specific x ● Do know how to sample from q(x) ● Sample x from q ● Compare q(x) to p(x) ● Adjust samples accordingly Rejection Sampling ● Sample from an easy function State Space ● Compute rejection ratio: α(x) = p(x)/cq(x) State Space ● Keep particles with probability α(x), reject with probability 1-α(x) State Space Sample Importance Resampling ● Sample from an easy function State Space ● Compute importance weights State Space ● Resample particles from particle set, according to importance weights State Space Prediction Measurement I. Sample {x i t } from (x, y, θ) II. Iterate: 1) Sample from motion model according to action a t , to get proposal distribution q(x) 2) Compute importance weights 3) Resample from {x i } according to {w i } Robot Localization using SIR )( )( w i = Sampling from Motion Model ● A common motion model: ● ● Rotation: ?θ 1 , σ 2 ?θ 1 = α 1 ?d+ α 2 ?θ 1 ● Translation: ?d, σ 2 ?d = α 3 ?d+ α 4 (?θ 1 + ?θ 2 ) ● Rotation: ?θ 1 ,σ 2 ?θ 2 = α 1 ?d+ α 2 ?θ 2 ● Compute rotation, translation, rotation from odometry ● For each particle, sample a new motion triplet by from Gaussians described above ● Use geometry to generate posterior particle position x q x p Decompose motion into rotation, translation, rotation μ = μ = μ = Sensor Model Laser model built from collected data Laser model fitted to measured data, using approximate geometric distribution Sensor Model 100 200 300 Expected distance Measured distance y [cm] P r o b a b i l i t y p ( y , x ) Approximated Measured 400 0 0.025 0.05 500 0.075 0.1 0.125 Problem ● How to compute expected distance for any given (x, y, θ)? ● Ray-tracing ● Cached expected distances for all (x, y, θ). ● Approximation: ● ?d: ● Compute expected distance only for (x, y) ● ● Only useful for highly-accurate range sensors (e.g., laser range sensors, but not sonar) Computing Importance Weights (Approximate Method) ● Off-line, for each empty grid-cell (x, y) ● Compute d(x, y) the distance to nearest filled cell from (x, y) ● Store this “expected distance” map ● At run-time, for a particle (x, y) and observation z i =(r, θ) ● Compute end-point (x’, y’) = (x+rcos(θ),y+rsin(θ)) ● Retrieve d(x’, y’) ● Compute probability of error, p(d), from Gaussian sensor model of σ 2 Assume a symmetric sensor model depending only on absolute difference between expected and measured ranges Much faster to compute this sensor model , error in measurement specific Bayes’ Filters ● ● ● Linear-Gaussian motion and sensor models ● Data association problem ● Quadratic in number of state features ● HMM ● Discrete multimodal distribution ● Arbitrary motion and sensor models ● Quadratic in number of states ● Particle Filters ● Arbitrary distributions ● Arbitrary motion and sensor models ● Exponential in number of state features Kalman filter Unimodal Gaussian What you should know Kalman Multihypothesis tracking Grid HMM Topology Particle Belief Unimodal Multimodal Discrete Discrete Non-parametric or discrete Accuracy + + 0 - + Robustness 0 + + + + Sensor variety - - + 0 + Efficiency + 0 - 0 0 Implementation 0 - 0 0 + What you should know ● What a Hidden Markov Model is ● The Forward algorithm ● ● How to implement particle filtering ● Pros and cons of particle filters ● How to implement robot localization using particle filters The Viterbi algorithm