16.412 / 6.834 Lecture, 15 March 2004 1 Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMMs Stanislav Funiak 16.412 / 6.834 Lecture, 15 March 2004 2Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models References z Hofbaur, M. W., and Williams, B. C. (2002). Mode estimation of probabilistic hybrid systems. In: Hybrid Systems: Computation and Control, HSCC 2002. z Funiak, S., and Williams, B. C. (2003). Multi-modal particle filtering for hybrid systems with autonomous mode transitions. In: DX-2003, SafeProcess 2003. z Lerner, U., R. Parr, D. Koller and G. Biswas (2000). Bayesian fault detection and diagnosis in dynamic systems. In: Proc. of the 17 th National Conference on A. I.. pp. 531-537. z V. Pavlovic. J. Rehg, T.-J. Cham, and K. Murphy. A Dynamic Bayesian Network Approach to Figure Tracking Using Learned Dynamic Models. In: Proc. ICCV, 1999. z H.A.P. Blom and Y. Bar-Shalom. The interacting multiple model algorithm for systems with Markovian switching coefficients. IEEE Transactions on Automatic Control, 33, 1988. 16.412 / 6.834 Lecture, 15 March 2004 3Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Hybrid Models Hidden Markov model on failed off p(x 0 ) p(x t | x t-1 ) p(z t | x t ) Dynamic systems ? Applications: - target tracking - localization and mapping -… Applications: - topological localization 16.412 / 6.834 Lecture, 15 March 2004 4Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Outline z Applications: fault diagnosis, visual tracking z Switching linear Gaussian models + exact filtering z Probabilistic Hybrid Automata + filtering z Approximate Gaussian filtering with hybrid HMM models 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 1 ),z(t 2 ) (x|z 1 ,z 2 ) X Z 1 x 1 x2 Z 1 Z 2 Z 1 Z 2 X σ σ σ μ Process model is x t =Ax t-1 +Bu t-1 +q t-1 Measurement model is z t =Hx t-1 +r t Image adapted from Maybeck. 16.412 / 6.834 Lecture, 15 March 2004 5Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Scenario 1: Wheel monitoring for planetary rovers Discrete variable: wheel failed (if any) Continuous variables: linear and angular velocity Normal trajectory and trajectories with fault at each wheel Courtesy NASA JPL Courtesy of Vandi Verma. Used with permission. 16.412 / 6.834 Lecture, 15 March 2004 6Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Scenario 2: Diagnosing subtle faults [H&W 2002] Discrete variables: operational mode Continuous variables: CO 2 flow, CO 2 & O 2 conc. {closed, open, stuck-closed, stuck-open} Courtesy NASA JSC 16.412 / 6.834 Lecture, 15 March 2004 7Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Scenario 3: Visual pose tracking Discrete variables: type of movement Continuous variables: head, legs, and torso position Courtesy Pavlovic. J. Rehg, T.-J. Cham, and K. Murphy 16.412 / 6.834 Lecture, 15 March 2004 8Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Scenarios 1-3: Common properties 1. Continuous dynamics 2. Finite set of behaviors, determines dynamics z Continuous state hidden z Noisy observations z Uncertainty in the model z System may switch between behaviors Need continuous statistical estimation Need to track discrete changes Need both 16.412 / 6.834 Lecture, 15 March 2004 9Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Outline z Applications: fault diagnosis, visual tracking z Switching linear Gaussian models + exact filtering z Probabilistic Hybrid Automata + filtering z Approximate Gaussian filtering with hybrid HMM models 16.412 / 6.834 Lecture, 15 March 2004 10Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Hybrid models – Desired properties z State evolution: ? Stochastic continuous evolution (uncertain model) ? Gaussian noise (for KF) ? Probabilistic discrete transitions ? Continuous observations, discrete and continuous actions z Interaction of discrete and continuous state: ? Discrete state affects continuous evolution ? Continuous state affects discrete evolution z Large systems 1 2 u c1 u d1 u d2 w c1 3 y c2 y c1 v s1 v s3 v o1 v o2 A A CA A v s2 16.412 / 6.834 Lecture, 15 March 2004 11Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Graphical models revisited a 1 b 1 Z 1 x 1 b 2 Z 2 x 2 ),|( iij xaxp Actions Beliefs Observations States Observable Hidden )|( ii xzp Model: Transition distribution Observation distribution )|( ii xzp ),|( iij xaxp 16.412 / 6.834 Lecture, 15 March 2004 12Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Review: Hidden Markov models z Discrete states, actions, and observations z Transition & observation p. written as tables a 1 b 1 Z 1 x 1 b 2 Z 2 x 2 ),|( iij xaxp Actions Beliefs Observations States Observable Hidden 117 5 9 77 Mass Ave Belief update: 16.412 / 6.834 Lecture, 15 March 2004 13Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Review: Linear models, Kalman Filter z Continuous states, actions, and observations z Linear (linearized) process and measurement model a 1 b 1 Z 1 x 1 b 2 Z 2 x 2 ),|( iij xaxp Actions Beliefs Observations States Observable Hidden 111   tttt qBuAxx ttt rHxz  Belief update: 16.412 / 6.834 Lecture, 15 March 2004 14Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Switching Linear Systems (SLDS) z Discrete and continuous state z Also known as jump Markov linear Gaussian model a 1 b 1 Z 1 x 1 b 2 Z 2 x 2 ),|( 1 tt xaxp  Actions Beliefs Observations Continuous states Observable Hidden )|( tt xzp d 1 d 2 Discrete states Discrete states (modes): 00 1 )Pr( )|Pr( S  d dd tt Continuous state: )( )( )()( 000 11 111 dvx rHxy dq udBxdAx ttt tt ttttt      )|( 1 tt ddp  16.412 / 6.834 Lecture, 15 March 2004 15Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Example: Acrobatic robot tracking ok failed 0.005 0.995 1.0 μ  μ  T 2121 ,,, TTTT  noisetf noisetf noiset noiset kkkkyesk kkkkyesk kkk kkk         ),,,,( ),,,,( ,2,2,1,1,21,2 ,2,2,1,1,11,1 ,2,21,2 ,1,11,1 GTTTTT GTTTTT GTTT GTTT     noisetf noisetf noiset noiset kkkknok kkkknok kkk kkk         ),,,,( ),,,,( ,2,2,1,1,21,2 ,2,2,1,1,11,1 ,2,21,2 ,1,11,1 GTTTTT GTTTTT GTTT GTTT     )(okA )( failedA )(okB )( failedB )(okq )( failedq )|Pr( 1 tt dd  )01.0,0(, 0 0 1 0 NrH ? ? ? ? ? o ? ? ? ? ? a 16.412 / 6.834 Lecture, 15 March 2004 16Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Example cont’d z The actuator works until t=20, then breaks ok ok ok ok failed )(okA )(okB )(okq )( failedA )( failedB )( failedq )(okA )(okB )(okq )(okA )(okB )(okq )(okA )(okB )(okq t=20t=19t=18t=17 … t=21 16.412 / 6.834 Lecture, 15 March 2004 17Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models What questions? z Mode estimation: ? Given a 1 z 1 … a t z t , estimate d t ? Application: fault diagnosis z Continuous state estimation ? Given a 1 z 1 … a t z t , estimate x t ? Application: tracking z Hybrid state estimation ? Given a 1 z 1 … a t z t , estimate x t , d t 16.412 / 6.834 Lecture, 15 March 2004 18Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Exact filtering for SLDS z Main idea: ? Do estimation for each sequence of mode assignments ? Sum up the assignments that end in the same mode ? Each Gaussian has an associated weight (probability) ok ok failed ok failed ok failed Maths: )...|...,( )...|,( 11 ... 1 11 11 tt dd tt tttt zazaddxp zazadxp t |  )...|...( )...,...|( )...|...,( 111 111 111 ttt tttt tttt zazaddp zazaddxp zazaddxp Continuous tracking Probability of a mode sequence 16.412 / 6.834 Lecture, 15 March 2004 19Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Continuous tracking Back to our example: ok ok ok ok failed )(okA )(okB )(okq )( failedA )( failedB )( failedq )(okA )(okB )(okq )(okA )(okB )(okq )(okA )(okB )(okq t=19t=18t=17 … Know: 1. Model A,B each each t 2. Observations 3. Actions ? Can do Kalman filtering as before )()( ,? i t i t Cx Sequence (i) Kalman filter: ),?()...,...|( )()( 111 i t i ttttt CxNzazaddxp )( ? i t x )(i t C )( 1 ? i t x  t=20 16.412 / 6.834 Lecture, 15 March 2004 20Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Probability of a mode sequence (1/2) Challenge: Can we update efficiently? )...|...( 111 ttt zazaddp a 1 b 1 Z 1 x 1 b 2 Z 2 x 2 ),|( 1 tt xaxp  Actions Beliefs Observations Continuous states Observable Hidde n )|( tt xzp d 1 d 2 Discrete states )|( 1 tt ddp  ok ok ok ok failed )...|......( 111111  ttt zazaokokddp )...|......( 111 ttt zazafailedokokddp 1. Prediction: )...,...|( )...|...( )...|...( 111111 111111 11111    tttt ttt ttt zazadddp zazaddp zazaddp )|()...|...( 1111111  ttttt ddpzazaddp conditional probability independence discrete transition probability 16.412 / 6.834 Lecture, 15 March 2004 21Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Probability of a mode sequence (2/2) Challenge: Can we incorporate the latest observation? ok ok ok ok failed )...|......( 111 ttt zazafailedokokddp 2. Update: Bayes rule + Kalman Filter )...|......( 11111  ttt zazafailedokokddp )...|...( 11111 ttttt zazazaddp  Observation likelihood given the mode sequence Prediction ),?()...,...|( )()( 11111   ii ttttt CxNzazaddxp 1. Sequence (i) 2. ),?()...,...|( )()( 11111 RHHCxHNzazaddzp Tii ttttt    S T rrS e S 1 5.0 2/1 |2| 1   S constzazaddpzazaddzp ttttttt )...|...()...,...|( 1111111111  16.412 / 6.834 Lecture, 15 March 2004 22Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Outline z Applications: fault diagnosis, visual tracking z Switching linear Gaussian models + exact filtering z Probabilistic Hybrid Automata + filtering z Approximate Gaussian filtering with hybrid HMM models 16.412 / 6.834 Lecture, 15 March 2004 23Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Probabilistic Hybrid Automata z SLDS + nonlinear dynamics, guarded transitions Hofbaur & Williams 2002 has-ball true false 7.0 1 T 0.1 0.9 7.0 1 !T 0.5 7.0 1 !T 0.5 7.0 1 T 1.0 0.5 0.5 2121 ,,, TTTT  Continuous dynamics: noisetf noisetf noiset noiset kkkkyesk kkkkyesk kkk kkk         ),,,,( ),,,,( ,2,2,1,1,21,2 ,2,2,1,1,11,1 ,2,21,2 ,1,11,1 GTTTTT GTTTTT GTTT GTTT     noisetf noisetf noiset noiset kkkknok kkkknok kkk kkk         ),,,,( ),,,,( ,2,2,1,1,21,2 ,2,2,1,1,11,1 ,2,21,2 ,1,11,1 GTTTTT GTTTTT GTTT GTTT     μ  μ  T ball: a 1 b 1 Z 1 x 1 b 2 Z 2 x 2 ),|( 1 tt xaxp  Actions Beliefs Observations Continuous states Observable Hidd en )|( tt xzp d 1 d 2 Discrete states Discrete transition now depends on the continuous state ),|( 1 ttt xddp  (also, the observation function g depends on discrete state) 16.412 / 6.834 Lecture, 15 March 2004 24Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Discrete prediction in PHA Main idea: compute transition probability from continuous estimate constant for all x t-1 that satisfy a given guard )( 1 )( 1 ,? i t i t Cx  true true true true false )...,...|()...|...( )...|...( 111111111111 11111   ttttttt ttt zazadddpzazaddp zazaddp )...|...( 111111  ttt zazaddp )...|...( 111111  tttt zazadddp O def ttttt X ttt Pdxzazaddxpxddp  3 1111111111 )...,...|(),|( Cannot simplify as easily as before Instead: = 6 p(transition W) p(guard for W satisfied | previous estimate) 16.412 / 6.834 Lecture, 15 March 2004 25Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Example 0.7 T 1 meanof estimated T 1 guard boundary probability of guard c 7.0 1 T 7.0 1 !T 7.0 1 !T 7.0 1 T )( 1 )( 1 ,? i t i t Cx  P(true -> false | estimate i) = 0.1* + 0.5* 16.412 / 6.834 Lecture, 15 March 2004 26Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Concurrent Probabilistic Hybrid Automata z PHA + composition 2121 ,,, TTTT  μ  μ  T ball: Gaussian filtering: z Merge the difference equations in component models ? e.g., equation solver z Track sequences of full mode assignments as before z Compute the transition probability component-wise 16.412 / 6.834 Lecture, 15 March 2004 27Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Putting it all together z Estimate the continuous state for each mode sequence z Update the posterior probability of each mode sequence 1. prediction (transition expansion) 2. observation Hybrid update equations Hofbaur & Williams 2002 P o P t prediction observation )( 1 )( 111 ,?),...( i t i tt Cxddb  Old estimate: New estimate: )()( 1 ,?),...( i t i tt Cxddb ottt PPddbddb )...()...( 111  16.412 / 6.834 Lecture, 15 March 2004 28Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Outline z Applications: fault diagnosis, visual tracking z Switching linear Gaussian models + exact filtering z Probabilistic Hybrid Automata + filtering z Approximate Gaussian filtering with hybrid HMM models 16.412 / 6.834 Lecture, 15 March 2004 29Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Gaussian filtering for hybrid HMMs z Tracks mode sequences with a bank of Kalman Filters Problem: the number of possible mode sequences increases exponentially in time (also exponentially in # components) ? Exponential computational complexity 2 strategies: 1. pruning (truncating) 2. collapsing (merging) 16.412 / 6.834 Lecture, 15 March 2004 30Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Pruning: Selecting relevant mode sequences Two methods z K-best filtering ? Looking for a set of leading sequences ? Obtained efficiently with A* search z Particle filtering ? Selecting trajectories probabilistically by sampling ? Rao-Blackwellised particle filtering x 2 x 2 x 2 x 2 x 2 x 2 x 2 x 1 x 1 x 0 x 2 x 2 (1) (2) (3) x 2 (3) (1) (2) x 1 (3) (1) (4) (5) (6) (7) (8) (9) 16.412 / 6.834 Lecture, 15 March 2004 31Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models K-best Filtering z A* search through space of possible successors [H&W 2002] z Evaluated in the order of )()()( QQQ nhngnf  Probability (cost) of trajectory so far Admissible heuristics: upperbound on probability from n v onward 16.412 / 6.834 Lecture, 15 March 2004 32Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Rao-Blackwellised Particle Filtering Principle: Decrease the computational complexity by reducing dimensionality of sampled space ? Sample variables r ? Closed-form solution for variables s r s r )|( )( :0 i t rp s s component 1component 2 P T2 n v P T1 P T1 P o h (k) h (k-1) (k-1) transition expansion estimation component l x x (k) 16.412 / 6.834 Lecture, 15 March 2004 33Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models RBPF for Hybrid HMMs m ok,0 (1) Initialization step (2) Importance sampling step (3) Selection (resampling) step m ok,0 m ok,1 m ok,1 m ok,0 m ok,1 m f,0 m f,1 (4) Exact (Kalman Filtering) step draw samples from the initial distribution over the modes initialize the corresponding continuous state estimates evolve each sample trajectory according to the transition model and previous continuous estimates determine transition & observ- ation model for each sample update continuous estimates m ok,0 m ok,0 m ok,0 m ok,1 m ok,1 m ok,1 m ok,1 m f,1 m ok,1 m ok,1 m ok,0 m ok,0 m ok,0 m ok,1 m ok,1 m ok,1 Funiak & Williams 2003 16.412 / 6.834 Lecture, 15 March 2004 34Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models Do we really need hybrid? Alternatives: ? HMM, grid-based methods: Course discretization ineffective for tracking a dynamic system! ? Kalman Filter: Unimodal distribution too weak ? Particle filter: Sample size too large 16.412 / 6.834 Lecture, 15 March 2004 35Hybrid Mode Estimation and Gaussian Filtering with Hybrid HMM Models What you should know z The form of hybrid models z The “exact” algorithm for computing the belief state over mode sequences + continuous estimates ? And how to use it for common diagnostic tasks z The strategies for pruning mode sequences (conceptually)