Foundations of state Estimation part i Topics: Hidden markov models Particle filters Additional reading L R Rabiner, "A tutorial on hidden Markor models, Proceedings of the IEEE, vol. 77, pp. 257-286, 1989 equential 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 ntelligence.128:1-2,99-141(2001). Hidden markoⅴ Models actions Beliefs Observations Observable ORIx) Hidden State Discrete states. actions and observations f(…,°,°),h(,) can now be written as tables
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 x1 x2 T(xj |ai , xi ) Z2 b Beliefs 1 Observations Z1 a Actions 1 O(zj |xi ) b2 Z2 Hidden Observable
(Somewhat) Useful for Localization in Topological Maps a p(x2x1a)=.9 p(xiX, a)=.05 X4:p(x4x1a)=05 Observations can be features such as corridor features Junction features, etc Belief Tracking Estimating p(x)is now easy After each action a, and observation zt VX∈X, update: ∑p( p This algorithm is quadratic in XI (Recall that Kalman Filter is quadratic in number of state features Continuous x means infinite number of states
(Somewhat) Useful for Localization in Topological Maps x1 x2: p(x2|x1,a)= .9 x3: p(x3|x1,a)=.05 x4: p(x4|x1,a)=.05 Observations can be features such as corridor features, junction features, etc. ● Estimating pt (x) is now easy ● After each action at and observation zt , ∀x∈X, update : ● This algorithm is quadratic in |X|. ● number of state features. Continuous X means infinite number of states.) Belief Tracking )()',|()|()( 1 ' xxp xpx t X t = t ∑ t − (Recall that Kalman Filter is quadratic in a x p z p
The Three Basic Problems for HMms 1)Given the history o=a1, Z1, a2, Z2 aT, ZT, and a model 2=(A, B, I), how do we efficiently compute P(on), the probability of the history, given the model? 2)Given the history O=a1, Z1, a2, Z2,. a, ZT and a model 2. how do we choose a corresponding state sequence X=X,X2,XT e, best""the observations/?se which is optimal in some meaningful sense 3)How do we adjust the model parameters 入=(A.B,) to maximize p(O|入)? HMM Basic Problem 1 Probability of history o given n is sum over all state sequences Q=X1, X2, X3 X, PO|)=∑POQ,PQ|) >I(x)P(=11xP(x21x, a,)P(=21x2)p(x3 1x2, a2) all 91, 92 Summing over all state sequences is 2T.XT Instead build lattice of states forward in time computing probabilities of each possible trajectory as lattice is built Forward algorithm is X2T
The Three Basic Problems for HMMs 1,z1,a2,z2,...,aT,zT, and a model λ=(A,B,π), how do we efficiently compute P(O|λ), the probability of the history, given the model? 1,z1,a2,z2,...,aT,zT and a model λ, how do we choose a corresponding state sequence X=x1,x2,...,xT 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=x1,x2,x3,...,xT,: ● 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|2T ∑ ∑ = = ,..., 22322112111 2 )...,|()|(),|()|()( )|(),|()|( Q Q all 1 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 O P Q P O P
HMM Basic problem 1 1.Initialization a1(1)=7D(二1|x) 2. Induction: Repeat for t=1:T ()=∑a(p(x X 3. Termination :水7 (O|A)=∑a() of the computation of a ()in terms of a ttice of observations t, and states j HMM Basic Problem 2 Viterbi Decoding: Same principle as forward algorithm with an extra term iNitialization tErmination 6()=xp(=1|x) P=maxi(i) 1≤isX v1()=0 arg ma 6(O)] 2)Induction 4)Back tracking 6(0)=max-(1p(x1|x,a)p(|x) x=v1+1(x+1) v, (i)=arg max8_()p(x; Ixj, a
HMM Basic Problem 1 )|()( 1 i 1 i α i = π )|(),|()()( 1 || 1 1 it X j t t tij i j x + = + ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ α = ∑α ∑= = || 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 1 1 = = i i i i ψ δ π [ ] )( [ ),|()( ] )|(),|()(max)( 1 ||1 1 ||1 t jji Xj t t itjji Xj t i j i j − − = = ψ δ δ δ [ ] [ ])( )(max ||1 * ||1 x i P i T T 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 State 2 3 T N at
HMM Basic problem 3 Given labelled data sequence D=X,, a1, Z1,X2, a2, Z2 XT, aT, ZT3 estimating p(z, Xi) and p(xi X, a) is just counting Given unlabelled data sequence D={a1,z1a2,z2,…,a,Z1} estimating p(Zi xi) and p(xi X; ak is equivalent to simultaneous localization and mapping (next lecture) Particle Filters
HMM Basic Problem 3 ● D={x1,a1,z1,x2,a2,z2,...,xT,aT,zT}, estimating p(zi ,xi ) and p(xj |xi ,ak) is just counting ● Given unlabelled data sequence, D={a1,z1,a2, z2, ...,aT,zT}, estimating p(zi ,xi ) and p(xj |xi ,ak) is equivalent to simultaneous localization and mapping (next lecture) Particle Filters Given labelled data sequence
Monte Carlo Localization The Particle Filter State space 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 How to sample Want to sample from arbitrary p(x) Dont know po Do know p(x) for any specific X Do know how to sample from g(x) Sample x from q Compare q(x) to p(x) Adjust samples accordingly
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 State space Sample from an easy function Compute rejection ratio: a(x)=p(x) /cq(x) State space Keep particles with probability a(x), reject with probability 1-a(x) Sample Importance resampling State space Sample from an easy function Compute importance weights State space Resample particles from particle set, according to importance weights
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
Robot Localization using sir Sample x', from(x, y, 8) Iterate 1) Sample from motion model according to action a to get proposal distribution q(x) 2) Compute importance weights Meaw p(x) 9(ent 3) Resample from x)according to w] Sampling from Motion Model a common motion model: Decompose motion into rotation translation rotation Rotation u=△61,02△,=a1△d+a2△ Translation:u=△d,o2ad=a3△d+a4(△1+△2) Rotat u=△010a2=a,△d+a2△02 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
Prediction Measurement I. Sample {xi t } from (x, y, θ) II. Iterate: 1) Sample from motion model according to action at , to get proposal distribution q(x) 2) Compute importance weights 3) Resample from {xi } according to {wi } 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 0.125 Approximated Measured Expected distance 0.075 005 0.025 Measured distance y [cm] Sensor model eastre dstanneIF叫 aureddslance len ctet stome响 Laser model built from collected data Laser model fitted to measured data using approximate geometric distribution
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 bility 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(, y,6) Ray-tra Cached expected distances for all(x, y, 8) Approximation Assume a symmetric sensor model depending only on Ad absolute difference between expected and measured ranges Compute expected distance only for(x, y) Much faster to compute this sensor model 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=(r, Compute end-point (X (x+rcos(e), y+rsin(e)) Retrieve d(x, y), error in measurement Compute probability of error, p(d), from Gaussian sensor model of specific g2
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 zi =(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
