Foundations of State Estimation Topics: Hidden Markov Models ? Additional reading: ? ? J. J. Leonard and H. F. Durrant-Whyte. “ IEEE Trans. ? Issues ? Statement: – world? ? – time t? ? Inputs: – Model – ? Outputs from different algorithms: – Most likely current state x i – i ) – Most likely sequence of states over time: x 1 , x 2 , x 3 , x 4 , ... x t – 1 ), p(x 2 ), p(x 3 ) ... p(x t ) ? Choices: – How to compute the estimate x i or p(x i ) – i ) Bayes Filters Kalman Filters G. Welch and G. Bishop. “An Introduction to the Kalman Filter”. University of North Carolina at Chapel Hill, Department of Computer Science. TR 95-041. http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html Mobile robot localization by tracking geometric beacons.” Robotics and Automation, 7(3):376-382, June 1991 L.R. Rabiner, “A tutorial on hidden Markov models," Proceedings of the IEEE, vol. 77, pp. 257-286, 1989. Given a set of observations of the world, what is the current state of the Alternatives: Given a set of observations of the world, what is the state of the world at Sequence of observations, or perhaps actions and observations Probability distribution over possible current states p(x Sequence of probability distributions over time p(x How to represent p(x Graphical Models, Bayes’ Rule and the Markov Assumption 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 Hidden Observable )( )()|( )|( yp xpxyp yxp :ruleBayes ),|(),,,,,,,|( 1111001 ttttttt axxpzzazaaxxp  :Markov The Bayes’ Filter )()',|()|( ),,...,,,,|'()',|()|( ),...,,,,,,|()|( ),...,,,,,,|(),...,,,,,,,|( ),,...,,,,,,|()( 1 ' 111100 ' 221100 221100221100 221100 xpxaxpxzp zazazaxpxaxpxzp azazazaxpxzp azazazaxpazazazaxzp zazazazaxpxp t X tt tt X tt tt ttt ttt   3 3 :recursionhwitAnd :variableauxiliaryaningIntroduc :assumptionMarkovthetongAccordi :ruleBayestongAccordi Posterior belief after an action An action is taken Posterior belief after sensing State Space Initial belief ? Linear process and measurement models ? Gaussian noise ? Gaussian state estimate Kalman, 1960 f x(t 1 )|z(t 1 ) (x|z 1 ) f x(t 2 )|z(t 2 ) (x|z 2 ) f x(t 2 )|z(t 12 ) |z 12 ) X Z 1 x 1 Z 1 Z 2 Z 1 Z 2 x σ σ σ μ x t z tt . Bayes Filter The Kalman Filter ),z(t (x ,z x2 Process model is = Ax t-1 + Bu t-1 + q t-1 Measurement model is = Hx t-1 + r Image adapted from Maybeck ? Only need first two moments: ? Process model: ? Measurement model: ]) ? )( ? [( )( ? T ttttt tt xxxxEC xEx  QAACC Bux T tt ttt       1 11 ??        ttt ttttt T t T tt CHKIC zKxx RHHCHCK )( ) ? ( ?? )( 1 ? Process model is now ? Measurement model is ? Linearising, we get ? where A, H, W and V are the Jacobians: ),,( 111  tttt qux ),( ttt rz ttttt ttttt ttt VrxxHz WqxxAxx ux | |   ) ~ () ~ ( )?( ~ ),( ~ 111 11 ? ? ? ? ? ? o ? ? ? ? ? ? a w w w w w w w w n nn n x f x f x f x f A    1 1 1 1 ? ? ? ? ? ? o ? ? ? ? ? ? a w w w w w w w w n mm n x h x h x h x h H    1 1 1 1 ? ? ? ? ? ? o ? ? ? ? ? ? a w w w w w w w w n nn n q f q f q f q f W    1 1 1 1 ? ? ? ? ? ? o ? ? ? ? ? ? a w w w w w w w w m mm m v h v h v h v h V    1 1 1 1 Linear Kalman Filter x A xH The Extended Kalman Filter x f x h x h x f 0, 0, Updates for the EKF ? Still using only first two moments. ? Process model: ? Measurement model: T ttt T tttt ttt AC uxfx      1 11 ), ? ( ?        tttt ttttt T ttt T ttt T ttt CIC zKxx HHCK )( ? (( ?? )( 1 Robot Navigation Initial belief Action Measurement ? ? ? ? o ? ? ? ? a T [ y x ? ? ? ? o ? ? ? ? a ' ' ' TTT TT TT [ q tqty tqtx f sinsin coscos ),,( ? ? o ? ? a ' ' T t u ? ? o ? ? a b r z ? ? ? ? o ? ? ? ? a  ? ? 1 · ¨ ¨ ? §    ? ? o ? ? a  T T O O OO [ v x y vyx vh x y ryx 1 22 tan ),( bearing range 1t [  t [ t [ ? ? o ? ? a y x O O O W Q W C A 0, H K x h V R V C H ))0 , ' ' ' q u Leonard and Durrant-White, 1991 The EKF for Robot Localization ? ? ? ? o ? ? ? ? a ' ' ' TTT TT TT [ q ty tx f sinsin coscos ),,( ? ? ? ? o ? ? ? ? a ' 100 cos10 sin01 T T t t A >@ T ttW TTT ''' sincos ? ? ? ? ? o ? ? ? ? ? a      1 0 22 r x r y r y r x H x y y x O O O O ? ? ? ? o ? ? ? ? a  ? ? 1 · ¨ ¨ ? §     T T O O OO [ v x y vyx vh x y ryx 1 22 tan ),( ? ? o ? ? a 10 01 V Prediction step Measurement step ? ? Gaussian sensor and motion error models – Particularly painful for range data than can have occlusions ? Quadratic in number of state features Data Association ? ' ' ' t q t q q u ' Problems with Kalman Filters Unimodal probability distribution 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 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 | The Three Basic Problems for HMMs 1 ,z 1 ,a 2 ,z 2 ,...,a T ,z T , and a model O=(A,B,S), how do we efficiently compute P(O|O), the probability of the history, given the model? 1 ,z 1 ,a 2 ,z 2 ,...,a T ,z T and a model O, 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)? O=(A,B,S) to maximize P(O|O)? (Recall that Kalman Filter is quadratic in a x p z p 1) Given the history O=a 2) Given the history O=a 3) How do we adjust the model parameters HMM Basic Problem 1 ? Probability of history O given O 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 axxxx Q 1 all all S OOO HMM Basic Problem 1 )|()( 11 ii xi SD )|()|()()( 1 || 1 1 it X j ijtt xxjia   ? ? o ? ? a | D | || 1 )()|( X i t iDO (j) j. 1 1 2 2 3 T N α t q q x p z p a x x p z p Q P O P O P 1. Initialization 2. Induction: Repeat for t=1:T 3. Termination: z p z p x p O p Implementation of the computation of in terms of a lattice of observations t, and states Observation, t State 3) Termination 4) Back tracking HMM Basic Problem 2 ? algorithm, with an extra term 1) Initialization 2) Induction 0)( )|()( 1 11 i xi ii SG > @ >@),|()()( )|(),|()(max)( 1 ||1 1 ||1 jjit Xj t itjjit Xj t axji xaxji   GGG >@ >@)( )(max ||1 * ||1 ix iP T Xi T T Xi G G )( * 11 *  ttt xx What you should know ? Form of Bayes’ Filter ? ? How to take a state estimation problem and represent ? What terms are needed and how to find them ? What a Hidden Markov Model is ? The Forward algorithm ? Viterbi Decoding: Same principle as forward z p max arg x p z p x p dd dd max arg dd dd Kalman filter representation it as a Kalman (or Extended Kalman) filter The Viterbi algorithm