MULTI-MODAL PARTICLE FILTERING FOR HYBRID SYSTEMS WITH AUTONOMOUS MODE TRANSITIONS

Transcription

1 MULTI-MODAL PARTICLE FILTERING FOR HYBRID SYSTEMS WITH AUTONOMOUS MODE TRANSITIONS Sanislav Funiak,1 Brian C. Williams MIT Space Sysems and AI Laboraories, 77 Mass. Ave., Rm , Cambridge, MA USA, MIT Space Sysems and AI Laboraories, 77 Mass. Ave., Rm , Cambridge, MA USA, Absrac: Model-based diagnosis of embedded sysems relies on he abiliy o esimae heir hybrid sae from noisy observaions. This ask is especially challenging for sysems wih many sae variables and auonomous ransiions. We propose a fair sampling algorihm ha combines Rao-Blackwellised paricle filers wih a muli-modal Gaussian represenaion. In order o handle auonomous ransiions, we le he coninuous sae esimaes conribue o he proposal disribuion in he paricle filer. The algorihm ouperforms purely simulaional paricle filers and provides unificaion of paricle filers wih hybrid hidden Markov model (HMM) observers. Keywords: sae esimaion, hybrid modes, Mone Carlo mehod, analyic approximaions, Kalman filers, filering echniques, diagnosic inference 1. INTRODUCTION Embedded sysems abound in many real-world applicaions, ranging from space probes (Musceola e al., 1998) and life suppor chambers (Hofbaur and Williams, 2002) o walking robos (Pra e al., 1997). These sysems exhibi boh coninous and discree behavior and ighly inerac wih heir surrounding environmen hrough coninuous dynamics. Therefore, hey are well fi for hybrid modeling. Wihin he model-based diagnosis communiy, i is ofen desirable o esimae he sae of hybrid sysems from a sequence of noisy observaions. This ask is crucial for diagnosing suble fauls ha exhibi hemselves only over a 1 Suppored by NASA under conrac NAG lenghy period of ime. A challenge is ha realworld sysems, such as he BIO-Plex Tes Complex a NASA Johnson Space Cener (Hofbaur and Williams, 2002) have as many as 10,000,000 modes and 20 coninuous variables. A he same ime, he sysems exhibi non-linear dynamics and auonomous ransiions riggered by he coninuous dynamics. The sheer size and complexiy of hese sysems make he hybrid sae esimaion problem very challenging. In recen years, paricle filering mehods have been on he rise (Verma e al., 2001; Dearden and Clancy, 2002; Kousoukos e al., 2002). Paricle filers approximae he poserior disribuion wih a se of samples ha simulae he probabilisic model of he sysem. Thus, hey are applicable o a range of general, non-gaussian, non-linear models. However, wih a few excepions, hese

2 mehods are purely simulaional in he sense ha hey sample he complee sae space. Hence, for large sysems, he sample size is oo large o be pracical. On he oher side of he specrum sand he mulimodal filering mehods, which represen he belief sae as a mixure of Gaussians, e.g. (Li and Bar-Shalom, 1996; Hanlon and Maybeck, 2000; Lerner e al., 2000; Hofbaur and Williams, 2002). Their Gaussian represenaion and focused search provide an efficien soluion o high-dimensional problems. A he same ime, non-lineariies and merging can inroduce significan bias in he esimae. Clearly, here has been a gap beween hese wo mehods: on one end are saisically robus sampling algorihms and on he oher, analyical represenaion algorihms ha scale. Freias (2002) explored his gap in a Rao-Blackwellised paricle filering algorihm. By disallowing auonomous ransiions, he was able o decouple he discree and coninuous sae and only sample he modes. Ye, auonomous ransiions are pervasive in many areas, including rocke propulsion (Kousoukos e al., 2002) and biological sysems (Hofbaur and Williams, 2002). In our previous work, we have addressed auonomous ransiions by inerpreing muliple-model filering and AIbased search mehods in erms of hybrid HMMsyle predicion and updae equaions. However, we have no addressed he issues of sampling. The key conribuion of his paper is a mulimodal sampling algorihm for hybrid esimaion in he presence of auonomous mode ransiions. The algorihm samples mode rajecories and, for each rajecory, esimaes he coninuous sae wih a Kalman Filer. Hence, i can also be viewed as a specializaion of Rao-Blackwellised paricle filering (Murphy and Russell, 2001) wih furher approximaions. The key insigh o handling auonomous ransiions is ha coninuous esimaes are reused in he imporance sampling sep of he paricle filer. The algorihm is hus subsanially more efficien han purely simulaional paricle filers. I provides an elegan unificaion of paricle filering wih muliple-model filering and hybrid Markov observers. 2. HYBRID SYSTEM MODELING 2.1 Example: Acrobaic Robo Consider he following model of an acrobaic robo wih wo degrees of freedom, swinging on a high bar (see Figure 1). The robo has wo links he orso and he legs wih poin masses m 1 and m 2 a heir ends. The dynamic model for his sysem has four coninuous variables θ 1, θ 1, θ 2, θ 2 and Fig. 1. Acrobaic robo wih 2 degrees of freedom. a discree mode x d. If we le x c = [θ 1, θ 1, θ 2, θ 2 ], is evoluion can be expressed as ẋ c = [ θ 1, f 1 (θ 1, θ 1, θ 2, θ 2, T 1 ; x d ), θ 2, f 2 (θ 1, θ 1, θ 2, θ 2, T 1 ; x d )] + v x (1) where f 1 and f 2 are non-linear funcions (Paul, 1982), T 1 is he desired orque, and v x is he model uncerainy. The sysem is underacuaed: in order o move, he robo can only apply orque around is cener mass. The sysem can be in four modes, m 0,ok, m 1,ok, m 0,failed, and m 1,failed, represening wheher or no he robo holds a ball, which increases m 2, and wheher or no he acuaor has failed. The acuaor can fail a all imes when orque is exered wih low probabiliy. Furhermore, if he robo is far o he righ (θ 1 > 0.7), i capures a ball wih probabiliy 0.01 in each ime sep. If he robo holds a ball and θ 1 0.7, i will lose he ball wih probabiliy 0.01 in each ime sep. Clearly, capuring a ball is an example of an auonomous mode ransiion: he ransiion probabiliies depend on he coninuous sae (see Figure 2). 2.2 Probabilisic Hybrid Auomaa Formally, he sysem can be described as a Probabilisic Hybrid Auomaon (PHA), a formalism merging hidden Markov models (HMM) wih coninuous dynamical sysem models (Hofbaur and Williams, 2002). I is a uple x, w, F, T, X d, U d, T s : x denoes he hybrid sae of he auomaon, composed of variables {x d } x c. 2 The discree variable x d wih finie domain X d represens he operaional mode of he sysem, while he coninuous vecor x c R nx denoes he coninuous sae. The iniial sae probabiliy p(x 0 ) is assumed o be known and he condiional sae disribuion for each mode p(x c,0 x d,0 ) is Gaussian. 2 When clear from he conex, we use lowercase bold symbols, such as v, o denoe a se of variables {v 1,..., v l }, as well as a vecor [v 1,..., v l ] T wih componens v i.

3 For example, hey define heir beam search algorihm in erms of predicion and updae equaions h [ˆx i ] = P T (m i ˆx j, 1, u d, 1 )h 1 [ˆx j ] (4) h [x i ] = h [ˆx i, ]P O (y c, ˆx i, ) j h [ˆx j ]P O (y c, x j, ). (5) Fig. 2. Condiional dependencies among he sae variables x c, x d and he oupu y c expressed as a Dynamic Bayesian Nework. The edge from x c, 1 o x d, represens he dependence of x d, on x c, 1, i.e., auonomous ransiions. w = u d u c y c denoes he se of I/O variables, consising of disjoin ses of discree inpu variables u d U d, coninuous inpu variables u c R nu, and coninuous oupu variables y c R ny. The discree ransiion funcion T : U d R nu X d R nx T specifies, for each possible assignmen of variables u d, u c, x d, and x c a ime sep 1, a disribuion τ T over he modes a he nex ime sep. T is described by a finie se of pairs { τ, c } of ransiion disribuions τ : X d [0; 1] and guard condiions c ha cover disinc regions of he U d R nu X d R nx space. F : X d { f, g, v x, v y } specifies he coninuous evoluion of he auomaon for each mode x d X d in erms of he ransiion funcion f, observaion funcion g, and modedependen zero-mean whie Gaussian noise v x (x d ) and v y (x d ): x c, = f(x c, 1, u c, 1 ; x d, ) + v x (x d, ) (2) y c, = g(x c,, u c, ; x d, ) + v y (x d, ) (3) Larger sysems can be modeled as a composiion of muliple PHAs (Hofbaur and Williams, 2002). 2.3 Hybrid Markov Observer Muli-modal Gaussian filering encompasses a wide family of mehods for hybrid esimaion, including Muliple-Model (MM) esimaion, adapive MM esimaion, and more recen AI-based mehods. The common premise of hese mehods is ha hey represen he belief sae as a mixure of Gaussians and hey use a bank of Kalman Filers o evolve he coninuous sae. The algorihms vary in how hey choose which mode rajecories o expand (qualiaively or quaniaively) and how hey merge coninuous sae esimaes. The conribuion of Hofbaur and Williams (2002) was o inerpre hese algorihms in erms of HMM-syle hybrid belief-sae updae equaions. In hese equaions, h [ˆx i ] denoes an inermediae hybrid belief-sae ha is based on ransiion probabiliies only. Hybrid esimaion deermines he possible ransiions for each ˆx j, 1 a he previous ime sep, hus specifying candidae rajecories o be racked by he filer bank. Kalman filering hen provides he new hybrid sae ˆx i,. Finally, he inermediae belief sae is adjused using he observaion funcion P O. 3. PARTICLE FILTERING: REVIEW Paricle filers belong o he family of Mone Carlo simulaion-based mehods and are applicable o a range of domains. Given a discree-ime hybrid sysem (e.g. a PHA), paricle filers approximae he poserior of he hybrid sae x wih a se of sample rajecories {x (i) 0: }. The rajecories are raced sequenially and drawn from he poserior disribuion p(x 0: y 1: ). 3 They approximae he poserior in he sense ha he poserior probabiliy over a sufficienly large region A X d R nx is approximaed by he number of samples in i: P (x A y 1: ) 1 {i : x(i) A} (6) N Paricle filering algorihms ypically involve hree seps, as shown in Figure 3. The algorihm sars by drawing samples from he iniial disribuion p(x 0 ); hus, effecively approximaing he poserior a = 0. Then, in each ieraion, we evolve he hybrid rajecories by aking one random sample x for each rajecory x (i) 0: 1 according o a so-called proposal disribuion and compue he imporance weighs (sep 2). In is simples form, he proposal disribuion is jus he ransiion disribuion p(x x 1 ). This sep, called imporance sampling, is hen enirely analogous o he predicion-updae sequence in oher filering mehods: firs, we predic he hybrid sae x using he esimae a he previous ime sep and hen we adjus he predicion using he newes observaion. The final selecion sep simply muliplies he good paricles and removes he bad ones, so ha in he nex ime sep, he high-likelihood paricles conribue more o he sampling process. Oherwise, mos paricles would have zero weigh afer a few ieraions due o he accumulaed 3 We use he noaion v k:l o denoe he uple v k, v k+1,..., v l.

4 Fig. 3. The hree seps of a paricle filer: Iniializaion (1), Imporance sampling (2), and Selecion (3). The illusraed sysem has wo modes and one coninuous variable. errors in heir predicions, and he filer would no converge o he poserior. Paricle filers are concepually simple, ye offer ineresing generalizaions of he ideas presened above. Firs, he paricles approximae no only he poserior over he presen sae x, bu also over he space of complee rajecories raced by he sae ransiions, albei wih less precision. Furhermore, in principle, he proposal disribuion can be an arbirary funcion q(x ; x (i) 0: 1, y 1:), no only he ransiion disribuion p(x x (i) 1 ). For example, he proposal disribuion can direcly incorporae presen observaions and hus, make more precise predicions. I has been shown ha as long as he suppor of q includes he suppor of p(x x (i) 0: 1, y 1:) and we le he weighs w (i) = p(y x (i) 0:, y 1: 1)p( x (i) x (i) q( x ; x (i) 0: 1, y 1:) 0: 1, y 1: 1), (7) he paricle filer converges o he rue poserior in he limi. See (Douce e al., 2001) for a comprehensive review of paricle filers and derivaion of he resuls above. 4. MULTI-MODAL PARTICLE FILTERING Our algorihm combines paricle filering wih a muli-modal Gaussian represenaion. I sill performs paricle filering, bu reduces he dimensionaliy of he sampled space by applying he mehod of Rao-Blackwellised paricle filers (Murphy and Russell, 2001): If we divide he sae variables ino wo ses, he roo (sampled) variables r and leaf variables s, we can express he poserior disribuion of x as Fig. 4. Represenaion in he muli-modal paricle filer. Noe ha he rajecory m ok m f was duplicaed in he selecion sep a = 1. (1) Iniializaion sample x (i) d,0 p(x d,0), i = 1,..., N KF iniializaion: for i = 1,..., N, le ˆx (i) c,0 = E[x c,0 x (i) (i) d,0 ] and P 0 = Λ xc,0 x (i) d,0 (2) Imporance sampling sep for i = 1,..., N, compue he proposal disribuion p(x d, x (i) d,0: 1, y c,1: 1) sample x (i) d, p(x d, x (i) d,0: 1, y c,1: 1) and se x (i) d,0: (x(i) d,0: 1, x(i) d, ) evaluae he imporance weighs w (i) normalize he imporance weighs (3) Selecion sep resample (wih replacemen) N paricles from { x (i) d,0: } according o w(i) o obain samples {x (i) d,0: } disribued approximaely according o p(x (i) d,0: y c,1:). (4) Exac sep for i = 1,..., N, updae ˆx (i) c, and P (i) wih an EKF using f(, u c, 1 ; x (i) d, ), g(, u c, ; x (i) d, ), Λ v x(x (i) d, ), and Λ v y(x (i) d, ) le + 1 and go o sep 2 Fig. 5. Muli-modal paricle filer for PHA sae esimaion. p(x y 1: ) = p(s, r y 1: ) = p(s, r 0: y 1: )dr 0: 1 = p(s r 0:, y 1: )p(r 0: y 1: )dr 0: 1 (8) Thus, we expand he poserior in erms of he roo rajecory r 0: and leaf s condiioned on he rajecory (ypically, he leaf variables s would be descendans of he roos r in a DBN srucure, hence he roo-leaf analogy). The key o his formulaion is ha if we can compue p(s r 0:, y 1: ) analyically, we only need o sample he roo variables r. In his manner, fewer paricles are needed o cover he poserior. In he spiri of he beam search algorihm of Hofbaur and Williams (2002), we sample he mode rajecories and, condiioned on hese rajecories, compue he poserior of he coninuous variables analyically (see Figure 4). This pariion reveals

5 significan srucure: given a sampled rajecory x (i) d,0:, he ransiion and observaion disribuions are known. The disribuion p(x c, x (i) d,0:, y 1:) is hen approximaely Gaussian 4 and can be efficienly updaed wih a Kalman Filer. The resuling muli-modal paricle filering algorihm is shown in Figure 5. Each paricle now holds no only a sample rajecory x (i) d,0: drawn approximaely from he poserior disribuion of x d,0: bu also he esimaed mean ˆx (i) c, and covariance marix P (i) of p(x c, x (i) d,0:, y c,1:). There is also an addiional Kalman Filering sep ha updaes ˆx (i) c, and P (i) for each paricle i based on he ransiion and observaion equaions associaed wih he laes mode x (i) d,. As before, he algorihm sars by iniializing he paricles, albei only he modes are sampled. The algorihm hen proceeds o expand each rajecory i probabilisically by aking one random sample from he corresponding proposal disribuion. Afer we compue he imporance weighs, we resample he rajecories according o heir weighs, so as o direc heir fuure expansion ino relevan regions of he rajecory space. The proposal disribuion p(x d, x (i) d,0: 1, y 1: 1) is similar in is form o he ransiion disribuion p(x x 1 ) in Markov processes. However, i is condiioned on a complee rajecory and all previous observaions, raher han simply on he previous sae. This is because {x d, } alone is no an HMM process: due o he auonomous ransiions, knowing x d, 1 alone does no ell us wha he disribuion of x d, is. The disribuion of x d, is known only when condiioned on he mode and coninuous sae in he previous ime sep (see Figure 2). This observaion suggess ha in order o compue he proposal disribuion, we need o expand i in erms of he previous esimaes: p(x d, x (i) d,0: 1, y c,1: 1) = p(x d, x (i) d,0: 1, y c,1: 1, x c, 1 ) x c, 1 = = p(x c, 1 x (i) d,0: 1, y c,1: 1)dx c, 1 x c, 1 p(x d, x (i) d, 1, x c, 1) τ,c T p(x c, 1 x (i) d,0: 1, y c,1: 1)dx c, 1 τ(x d, )P (c(x c, 1 ) x (i) d,0: 1, y c,1: 1)(9) 4 More precisely, p(x c, x (i) d,0:, y 1:) is Gaussian if and only if all f and g along he rajecory are linear and here are no auonomous ransiions. Here, he firs equaliy follows from he oal probabiliy heorem. The second equaliy comes from he independence assumpion made in he model (Figure 2). The hird, final equaliy holds because he mode disribuion τ is he same for all values of x c, 1 ha saisfy is associaed guard condiion c. Only he guard condiions ha apply o inpus u d, 1 and u c, 1 are considered. The final erm in equaion 9 is he probabiliy ha a guard condiion is saisfied, given he rajecory and observaions o ime 1. Bu hen, x 1 is disribued (approximaely) as N (ˆx (i) c, 1, P (i) 1 ), so he final erm is simply an inegral over a Gaussian mulivariae disribuion wih mean ˆx (i) c, 1 and covariance P (i) 1. Depending on he form of he guard condiion, his probabiliy can be evaluaed more or less efficienly. For (hyper)recangular regions, efficien approximaions exis (Joe, 1995), and convex (possibly unbounded) regions can be reduced o recangular ones wih a linear ransform. For oher cases, one can always fall back o Mone Carlo simulaions. Given our choice of he proposal disribuion, he weigh w (i) in equaion 7 simplifies o w (i) = p(y c, y c,1: 1, x (i) d,0: ) (10) Unforunaely, equaion 10 is raher hard o evaluae efficienly. Even if we expand he weigh in erms of he coninuous sae as x c, p(y c, x c,, x (i) d, )p(x c, x (i) d,0:, y c,1: 1)dx c,, (11) he second inegrand erm is sill non-sandard in he presence of auonomous ransiions. We address his issue by ignoring he auonomous ransiions for he purpose of compuing imporance weighs. In his case, he inegrand is a produc of wo Gaussians (assuming linearized ransiions and observaions), and he weigh can be compued using he measuremen residual r = y c, g(f(ˆx (i) c, 1, u c, 1; x (i) d, ), u c,; x (i) d, ) (12) from he predicion sep of an EKF: w (i) N (r (i), S (i) ) (13) An open research problem is o approximae w (i) beer in he presence of auonomous ransiions. Noe ha he expressions derived for he proposal disribuion and he imporance weigh (equaions 9 and 13, respecively) are precisely he probabilisic hybrid ransiion funcion P T and he hybrid observaion funcion P O in equaions 4-5. By leveraging Rao-Blackwellisaion, he algorihm eleganly unifies wih HMM-based hybrid observers and muliple-model esimaion. In paricular, advances in one mehod, e.g. a beer approximaion

6 No. of paricles no ball, acuaor o.k. ball, acuaor o.k. no ball, acuaor failed ball, acuaor failed ime [s] Fig. 6. The disribuion of paricles across he four modes for he robo ball capure scenario. o w (i) in he paricle filer or a more accurae represenaion of he belief sae in search-based algorihms, may lead o improvemens in he oher mehods. 5. DISCUSSION In our experimens, we simulaed he moion of he acrobaic robo wih a differenial equaion solver for hree scenarios and generaed noisy observaions seen by he filer. Our implemenaion only mainains he laes mode for each rajecory, since he laes mode, along wih he coninuous esimae, provide sufficien saisics for he sampling and Kalman filering seps. Figure 6 shows a ypical execuion of he algorihm wih 100 paricles when T s = 0.01s. Iniially, he robo is sraigh a 45 degrees from he verical. Afer swinging for one round, he robo receives a ball and keeps i, which is correcly deeced by he filer. For comparison, we also implemened a simple boosrap paricle filer. However, he filer did no converge even wih 5000 paricles. In he near fuure, we hope o conduc a more rigorous comparison wih sae-of-he-ar paricle filers and he Gaussian beam filer used wihin hybrid mode esimaion (Hofbaur and Williams, 2002). This paper demonsraed an efficien sampling algorihm for hybrid models wih auonomous ransiions. By sampling from a proposal disribuion ha can be efficienly expressed using he previous coninuous sae esimaes, he algorihm overcomes coupling beween he discree and coninuous sae. I provides naural unificaion of Rao-Blackwellised paricle filering wih muliplemodel filering and mehods based on HMM-syle hybrid predicion and updae equaions. Acknowledgemens To A. Hofmann of he MIT LEG Laboraory. REFERENCES Dearden, R. and D. Clancy (2002). Paricle filers for real-ime faul deecion in planeary rovers. In: Proceedings of he 13h Inernaional Workshop on Principles of Diagnosis (DX02). pp Douce, A., N. Freias and N. J. Gordon (2001). Sequenial Mone Carlo Mehods in Pracice. Springer-Verlag. Freias, N. (2002). Rao-Blackwellised paricle filering for faul diagnosis. IEEE Aerospace. Hanlon, P. and P. Maybeck (2000). Muliplemodel adapive esimaion using a residual correlaion kalman filer bank. IEEE Transacions on Aerospace and Elecronic Sysems 36(2), Hofbaur, M. W. and B. C. Williams (2002). Mode esimaion of probabilisic hybrid sysems. In: HSCC 2002 (C.J. Tomlin and M.R. Greensree, Eds.). Vol of Lecure Noes in C. S.. pp Springer Verlag. Joe, H. (1995). Approximaions o mulivariae normal recangle probabiliies based on condiional expecaions. Journal of he American Saisical Associaion 90(431), Kousoukos, X., J. Kurien and F. Zhao (2002). Monioring and diagnosis of hybrid sysems using paricle filering mehods. In: MTNS Lerner, U., R. Parr, D. Koller and G. Biswas (2000). Bayesian faul deecion and diagnosis in dynamic sysems. In: Proc. of he 17h Naional Conference on A. I.. pp Li, X.R. and Y. Bar-Shalom (1996). Muliplemodel esimaion wih variable srucure. IEEE Transacions on Auomaic Conrol 41, Murphy, K. and S. Russell (2001). Rao- Blackwellised paricle filering for dynamic Bayesian neworks. In: Sequenial Mone Carlo Mehods in Pracice (A. Douce, N. Freias and N. Gordon, Eds.). Chap. 24, pp Springer-Verlag. Musceola, N., P. Nayak, B. Pell and B. C. Williams (1998). The new millennium remoe agen: To boldly go where no AI sysem has gone before. Arificial Inelligence 103(1-2), Paul, R. P. (1982). Robo Manipulaors. MIT Press. Pra, J., P. Dilworh and G. Pra (1997). Virual model conrol of a bipedal walking robo. In: Proc. of he IEEE Inernaional Conference on Roboics and Auomaions (ICRA 97). Verma, V., J. Langford and R. Simmons (2001). Non-parameric faul idenificaion for space rovers. In: Inernaional Symposium on Arificial Inelligence and Roboics in Space (isairas).