Rao-Blackwellised Particle Filtering for Dynamic...

95
Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering Dept. Cambridge University [email protected] Nando de Freitas Kevin Murphy Stuart Russell Computer Science Dept. UC Berkeley jfgf,murphyk,russell @cs.berkeley.edu Abstract Particle filters (PFs) are powerful sampling- based inference/learning algorithms for dynamic Bayesian networks (DBNs). They allow us to treat, in a principled way, any type of probabil- ity distribution, nonlinearity and non-stationarity. They have appeared in several fields under such names as “condensation”, “sequential Monte Carlo” and “survival of the fittest”. In this pa- per, we show how we can exploit the structure of the DBN to increase the efficiency of parti- cle filtering, using a technique known as Rao- Blackwellisation. Essentially, this samples some of the variables, and marginalizes out the rest exactly, using the Kalman filter, HMM filter, junction tree algorithm, or any other finite di- mensional optimal filter. We show that Rao- Blackwellised particle filters (RBPFs) lead to more accurate estimates than standard PFs. We demonstrate RBPFs on two problems, namely non-stationary online regression with radial ba- sis function networks and robot localization and map building. We also discuss other potential ap- plication areas and provide references to some fi- nite dimensional optimal filters. 1 INTRODUCTION State estimation (online inference) in state-space models is widely used in a variety of computer science and engineer- ing applications. However, the two most famous algorithms for this problem, the Kalman filter and the HMM filter, are only applicable to linear-Gaussian models and models with finite state spaces, respectively. Even when the state space is finite, it can be so large that the HMM or junction tree algorithms become too computationally expensive. This is typically the case for large discrete dynamic Bayesian net- works (DBNs) (Dean and Kanazawa 1989): inference re- quires at each time space and time that is exponential in the number of hidden nodes. To handle these problems, sequential Monte Carlo meth- ods, also known as particle filters (PFs), have been in- troduced (Handschin and Mayne 1969, Akashi and Ku- mamoto 1977). In the mid 1990s, several PF algorithms were proposed independently under the names of Monte Carlo filters (Kitagawa 1996), sequential importance sam- pling (SIS) with resampling (SIR) (Doucet 1998), bootstrap filters (Gordon, Salmond and Smith 1993), condensation trackers (Isard and Blake 1996), dynamic mixture models (West 1993), survival of the fittest (Kanazawa, Koller and Russell 1995), etc. One of the major innovations during the 1990s was the inclusion of a resampling step to avoid de- generacy problems inherent to the earlier algorithms (Gor- don et al. 1993). In the late nineties, several statistical im- provements for PFs were proposed, and some important theoretical properties were established. In addition, these algorithms were applied and tested in many domains: see (Doucet, de Freitas and Gordon 2000) for an up-to-date sur- vey of the field. One of the major drawbacks of PF is that sampling in high-dimensional spaces can be inefficient. In some cases, however, the model has “tractable substructure”, which can be analytically marginalized out, conditional on cer- tain other nodes being imputed, c.f., cutset conditioning in static Bayes nets (Pearl 1988). The analytical marginal- ization can be carried out using standard algorithms, such as the Kalman filter, the HMM filter, the junction tree al- gorithm for general DBNs (Cowell, Dawid, Lauritzen and Spiegelhalter 1999), or, any other finite-dimensional opti- mal filters. The advantage of this strategy is that it can drastically reduce the size of the space over which we need to sample. Marginalizing out some of the variables is an example of the technique called Rao-Blackwellisation, because it is related to the Rao-Blackwell formula: see (Casella and Robert 1996) for a general discussion. Rao-Blackwellised particle filters (RBPF) have been applied in specific con- texts such as mixtures of Gaussians (Akashi and Ku- mamoto 1977, Doucet 1998, Doucet, Godsill and Andrieu

Transcript of Rao-Blackwellised Particle Filtering for Dynamic...

Page 1: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks

Arnaud Doucet�

�Engineering Dept.

Cambridge [email protected]

Nando de Freitas�

Kevin Murphy�

Stuart Russell�

�Computer Science Dept.

UC Berkeley�jfgf,murphyk,russell � @cs.berkeley.edu

Abstract

Particle filters (PFs) are powerful sampling-based inference/learning algorithms for dynamicBayesian networks (DBNs). They allow us totreat, in a principled way, any type of probabil-ity distribution, nonlinearity and non-stationarity.They have appeared in several fields under suchnames as “condensation”, “sequential MonteCarlo” and “survival of the fittest”. In this pa-per, we show how we can exploit the structureof the DBN to increase the efficiency of parti-cle filtering, using a technique known as Rao-Blackwellisation. Essentially, this samples someof the variables, and marginalizes out the restexactly, using the Kalman filter, HMM filter,junction tree algorithm, or any other finite di-mensional optimal filter. We show that Rao-Blackwellised particle filters (RBPFs) lead tomore accurate estimates than standard PFs. Wedemonstrate RBPFs on two problems, namelynon-stationary online regression with radial ba-sis function networks and robot localization andmap building. We also discuss other potential ap-plication areas and provide references to some fi-nite dimensional optimal filters.

1 INTRODUCTION

State estimation (online inference) in state-space models iswidely used in a variety of computer science and engineer-ing applications. However, the two most famous algorithmsfor this problem, the Kalman filter and the HMM filter, areonly applicable to linear-Gaussian models and models withfinite state spaces, respectively. Even when the state spaceis finite, it can be so large that the HMM or junction treealgorithms become too computationally expensive. This istypically the case for large discrete dynamic Bayesian net-works (DBNs) (Dean and Kanazawa 1989): inference re-quires at each time space and time that is exponential in the

number of hidden nodes.

To handle these problems, sequential Monte Carlo meth-ods, also known as particle filters (PFs), have been in-troduced (Handschin and Mayne 1969, Akashi and Ku-mamoto 1977). In the mid 1990s, several PF algorithmswere proposed independently under the names of MonteCarlo filters (Kitagawa 1996), sequential importance sam-pling (SIS) with resampling (SIR) (Doucet 1998), bootstrapfilters (Gordon, Salmond and Smith 1993), condensationtrackers (Isard and Blake 1996), dynamic mixture models(West 1993), survival of the fittest (Kanazawa, Koller andRussell 1995), etc. One of the major innovations during the1990s was the inclusion of a resampling step to avoid de-generacy problems inherent to the earlier algorithms (Gor-don et al. 1993). In the late nineties, several statistical im-provements for PFs were proposed, and some importanttheoretical properties were established. In addition, thesealgorithms were applied and tested in many domains: see(Doucet, de Freitas and Gordon 2000) for an up-to-date sur-vey of the field.

One of the major drawbacks of PF is that sampling inhigh-dimensional spaces can be inefficient. In some cases,however, the model has “tractable substructure”, whichcan be analytically marginalized out, conditional on cer-tain other nodes being imputed, c.f., cutset conditioning instatic Bayes nets (Pearl 1988). The analytical marginal-ization can be carried out using standard algorithms, suchas the Kalman filter, the HMM filter, the junction tree al-gorithm for general DBNs (Cowell, Dawid, Lauritzen andSpiegelhalter 1999), or, any other finite-dimensional opti-mal filters. The advantage of this strategy is that it candrastically reduce the size of the space over which we needto sample.

Marginalizing out some of the variables is an example ofthe technique called Rao-Blackwellisation, because it isrelated to the Rao-Blackwell formula: see (Casella andRobert 1996) for a general discussion. Rao-Blackwellisedparticle filters (RBPF) have been applied in specific con-texts such as mixtures of Gaussians (Akashi and Ku-mamoto 1977, Doucet 1998, Doucet, Godsill and Andrieu

Page 2: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

2000), fixed parameter estimation (Kong, Liu and Wong1994), HMMs (Doucet 1998, Doucet, Godsill and Andrieu2000) and Dirichlet process models (MacEachern, Clydeand Liu 1999). In this paper, we develop the general theoryof RBPFs, and apply it to several novel types of DBNs. Weomit the proofs of the theorems for lack of space: pleaserefer to the technical report (Doucet, Gordon and Krishna-murthy 1999).

2 PROBLEM FORMULATION

Let us consider the following general state spacemodel/DBN with hidden variables ��� and observed vari-ables � � . We assume that ��� is a Markov process of ini-tial distribution ��� ��� and transition equation ��� ��� � �������� .The observations � ��� ��� � � �� ��� ��������� � � � are assumedto be conditionally independent given the process ��� ofmarginal distribution ����� � � � � . Given these observations,the inference of any subset or property of the states � � � � �� � � ��� � ������� ��� � � relies on the joint posterior distribution�!� � �"� � � � ��� � . Our objective is, therefore, to estimate thisdistribution, or some of its characteristics such as the filter-ing density �!� � � � � ��� � or the minimum mean square error(MMSE) estimate #%$ � � � � ��� ��& . The posterior satisfies thefollowing recursion

�!� ��� � ��� � ��� �')( �!� ��� � ��� ��� � ��� ��� �� �!��� � � � � �!� � � � � ��� � �!�*� � � � ��� ��� � (1)

If one attempts to solve this problem analytically, one ob-tains integrals that are not tractable. One, therefore, has toresort to some form of numerical approximation scheme. Inthis paper, we focus on sampling-based methods. Advan-tages and disadvantages of other approaches are discussedat length in (de Freitas 1999).

The above description assumes that there is no structurewithin the hidden variables. But suppose we can di-vide the hidden variables ��� into two groups, + � and , � ,such that ��� ����� �������"-( ����, ��� + ��� ��� ��� , ��� �� ���*+ ��� + ��� �� and,conditional on + �"� � , the conditional posterior distribution�!��, � � �"� � ��� � � + � � �. is analytically tractable.

�Then we can

easily marginalize out , �"� � from the posterior, and onlyneed to focus on estimating �!�/+ � � � � � ��� � , which lies in aspace of reduced dimension. Formally, we are making useof the following decomposition of the posterior, which fol-lows from the chain rule

�!��+ � � �0� , �"� ��� � ��� �')( �!�", �"� �"� � ��� � � + � � �. �!��+ �"� �"� � ��� �'The marginal posterior distribution �!�*+ �"� � � � ��� � satisfies1

The problem of how to automatically identify which vari-ables should be sampled, and which can be handled analytically,is one we are currently working on. We anticipate that algorithmssimilar to cutset conditioning (Becker, Bar-Yehuda and Geiger1999) might prove useful.

the alternative recursion

243/5�6�7 8�9 : 1 7 8<;�= 2>3/: 8 9 : 1 7 8@? 10A 5 6�7 8 ; 243B5 8 9 5 8*? 1 ;�2>3/5 6�7 8*? 1 9 : 1 7 8@? 1 ;2>3/: 8 9 : 1 7 8*? 1 ;(2)

If eq. (1) does not admit a closed-form expression, then eq.(2) does not admit one either and sampling-based methodsare also required. Since the dimension of �!��+ �"� ��� � ��� �C issmaller than the one of �!�"+ �"� � � , � � � � � ��� � , we should expectto obtain better results.

In the following section, we review the importance sam-pling (IS) method, which is the core of PF, and quantify theimprovements one can expect by marginalizing out , �"� � �i.e. using the so-called Rao-Blackwellised estimate. Sub-sequently, in Section 4, we describe a general RBPF algo-rithm and detail the implementation issues.

3 IMPORTANCE SAMPLING ANDRAO-BLACKWELLISATION

If we were able to sample D i.i.d. random sam-

ples (particles), EGF�+�HJILK� � � � ,MHLIJK�"� �CNPO�Q (SR����L�L�J� DUT , according to�!��+ �"� �0� , �"� ��� � ��� �. , then an empirical estimate of this distri-bution would be given by

�WVU�"+ � � �0� , �"� ��� � ��� �C)( RD

VXILY ��Z\[C]�^`_Jab'c d'e f ^g_Lab.c d*h �@i�+

� � � i�, � � �'where Zj[C] ^`_Jab'c d e f ^g_Jab.c d h �@i�+ � � � i�, �"� �' denotes the Dirac delta

function located at F +�HLIJK�"� � � ,MHLILK�"� � N . As a corollary, an

estimate of the filtering distribution �!��+ � � , � � � ��� � is� V �"+ � � , � � � ��� � k( �Vml VILY � Z [C] ^g_Lad e f ^n_Jad h �*i�+ � i�, � . Hence

one can easily estimate the expected value of any functiono � of the hidden variables w.r.t. this distribution, pq� o �. , us-ing

p"V�� o �.U(sr o � ��+ �"� ��� , � � �' �qVt��+ �"� �0� , �"� ��� � ��� �. i�+ � � � i�, � � �( R

DVXILY �

o � F�+�HJIJK� � � � ,MHLIJK�"� �CNThis estimate is unbiased and, from the strong law oflarge numbers (SLLN), p�V�� o �. converges almost surely(a.s.) towards p4� o �. as D u vxw . If y �z d �var{ H ] b'c d e f b.c d'| }�~�c d K $ o � �*+ �"� � � , �"� � &S� vxw , then a centrallimit theorem (CLT) holds� D�� p"Vt� o �'�� p4� o �'���(��VG���G���@� � y �z d��where � ��� denotes convergence in distribution. Typi-cally, it is impossible to sample efficiently from the “tar-get” posterior distribution �P�"+ �"� � � , � � � � � ��� � at any time � .So we focus on alternative methods.

Page 3: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

One way to estimate �!� + � � � � , � � � � � ��� � and p4� o � con-sists of using the well-known importance sampling method(Bernardo and Smith 1994). This method is based on thefollowing observation. Let us introduce an arbitrary impor-tance distribution � ��+ �"� ��� , � � ��� � ��� �. , from which it is easyto get samples, and such that �!� + �"� � � , �"� �"� � ��� �'�� � implies� �"+ �"� � � , � � �"� � ��� �.�� � . Then

p � o � ( #�� H ] b'c d e f b.c d | } ~Cc d K � o � �*+ �"� � � , �"� � �� ��+ �"� � � , � � � '#�� H ] b'c d e f b.c d'| }�~Cc d K � � ��+ �"� � � , � � �.'where the importance weight is equal to

� �*+ � � � � , �"� � ( �!��+ � � �0� , �"� ��� � ��� �.� ��+ �"� � � , �"� � � � ��� � Given D i.i.d. samples EGF +�HJILK�"� � � ,MHJIJK� � � N T distributed accord-

ing to � ��+ � � � � , �"� � � � ��� � , a Monte Carlo estimate of p4� o � is given by

p �V � o � )(� �V � o �'� �V � o �. ( l VILY � o � F + HBIJK�"� � � , HJILK�"� �CN � F + HJILK� � � � , HBIJK�"� �.N

l VILY � � F +�HLIJK�"� � � ,MHJILK�"� � N( VX

ILY � � HJILK� � � o � F + HJILK�"� � � , HJILK� � � N

where the normalized importance weights � HBIJK��� � are equal to

� HBIJK�"� � ( � F + HLILK� � � � , HLIJK�"� � Nl V� Y � � F +�H � K� � � � ,MH � K�"� � N

This method is equivalent to the following point mass ap-proximation of �!� + � � � � , �"� � � � ��� � �WVU�"+ �"� �0� , � � ��� � ��� �.)( VX

ILY � � HJILK� � � Z [C] ^n_Lab.c d.e f ^n_Jab'c d@h �@i�+ �"� � i�, �"� �'

For “perfect” simulation, that is � ��+ � � � � , �"� � � � ��� � �(�!��+ �"� � � , �"� � � � ��� � , we would have

� HBIJK�"� � ( D � �for any Q .

In practice, we will try to select the importance distribu-tion as close as possible to the target distribution in a given

sense. For D finite,p �V � o � is biased (since it is a ratio of

estimates), but according to the SLLN,p V � o � converges

asymptotically a.s. towards p4� o � . Under additional as-sumptions, a CLT also holds.

Now consider the case where one can marginalize out , � � �analytically, then we can propose an alternative estimatefor p � o �' with a reduced variance. As �!��+ �"� � � , � � �"� � ��� �' (�!��+ �"� �"� � ��� �' �!�", �"� � � � ��� � � + �"� �. , where �!�", � � �"� � ��� � � + �"� �. isa distribution that can be computed exactly, then anapproximation of �!��+ �"� ��� � ��� �' yields straightforwardlyan approximation of �!��+ �"� � � , �"� � � � ��� � . Moreover, if#q{ H f b'c d�| }�~Cc d e ] b'c d K � o � �@+ � � � � , �"� � � can be evaluated in a

closed-form expression, then the following alternative im-portance sampling estimate of p � o � can be used

p �V � o � "(� �V � o �.� �V � o �.

( l VILY � # { [ f b'c d | } ~�c d e ] ^n_Jab'c d h F o � F�+ HJIJK� � � � , �"� � NqN � F�+ HJILK�"� � Nl VILY � � F +�HJILK�"� � N

where

� �*+ �"� � U( �!��+ � � �"� � ��� �'� ��+ �"� � � � ��� � � ��+ �"� � � � ��� �. ( r�� �"+ �"� � � , � � �"� � ��� �' i�, � � �Intuitively, to reach a given precision,

p �V � o �. will require

a reduced number D of samples overp �V � o � as we only

need to sample from a lower-dimensional distribution. Thisis proven in the following propositions.

Proposition 1 The variances of the importance weights,the numerators and the denominators satisfy for any D

var ����� b'c d�� �~�c d�� 3�� 3/5 6�7 8 ;C;�� var ����� b'c d�� �b'c d!� �~Cc d"� 3�� 3/5 6�7 8 A"# 6�7 8 ;C;var ����� b'c d�� �~�c d�� $�%&�'( 3�)<8<;�* � var ����� b'c d�� �b'c d!� �~Cc d"� $+%& 1( 3�)@8�;�*var ����� b'c d�� �~�c d�� $ %,-'( 3�) 8 ;�* � var ����� b'c d�� �b'c d!� �~Cc d"� $ %, 1( 3�) 8 ;.*A sufficient condition for

p �V � o � tosatisfy a CLT is var{ H ] b'c d e f b.c d | } ~Cc d K � o � ��+ �"� � � , � � � � � vxwand � ��+ �"� � � , �"� � � vxw for any ��+ �"� � � , �"� � (Bernardo and

Smith 1994). This trivially implies thatp �V � o � also satis-

fies a CLT. More precisely, we get the following result.

Proposition 2 Under

the assumptions given above,p �V � o � and

p �V � o � satisfya CLT � D F p �V � o � M� p>� o � N (��VG��� � �@� � y �� �� D F p �V � o � M� p>� o � N (��VG����� �@� � y �� �where y ��0/ y �� , y �� and y �� being given by

1 '1 =32 ���4� b'c d�� �b'c d�� � ~�c d��65 3C3�) 8 3/5 6�7 8 A!# 6�7 8 ;87:9 3�) 8 ;C;;� 3/5 6�7 8 A!# 6�7 8 ;C; '�<1 '' =32 ���4� b'c d�� �~Cc d"� 5 3C3=2?> � �b'c d�� �~Cc d�� � b'c d"� 3�)<8 3/5�6�7 8 A�# 6�7 8�;C;7@9 3�) 8 ;C;�� 8 3/5 6�7 8 ;C; ' <The Rao-Blackwellised estimate

p �V � o � is usually compu-

tationally more extensive to compute thanp �V � o �. so it is

of interest to know when, for a fixed computational com-plexity, one can expect to achieve variance reduction. One

Page 4: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

has

y �� � y �� ( #�� H ] b'c d | } ~Cc d K � ������� ^ � b.c d�� "~�c d� ��b'c d a � � o � ��+ � � � � , �"� � � p4� o � '�� ��+ � � � � , �"� � � �so that, accordingly to the intuition, it will be worth gen-erally performing Rao-Blackwellisation when the averageconditional variance of the variable , �"� � is high.

4 RAO-BLACKWELLISED PARTICLEFILTERS

Given D particles (samples)� + HJILK�"� ��� � � ,MHJILK� � ����� � at time � �R , approximately distributed according to the distribution���*+ HJIJK� � ��� � � , HJILK�"� ��� � � � ��� ��� �" , RBPFs allow us to compute D

particles � +�HJILK� � � � ,MHJIJK� � � � approximately distributed according

to the posterior ���*+�HJILK�"� � � ,MHJILK� � � � � ��� � , at time � . This is ac-complished with the algorithm shown below, the details ofwhich will now be explained.

Generic RBPF

1. Sequential importance sampling step For � =�� A������WA�� , sample:$�� 5 ��� �8 *���� 3/5�8�9 5 ��� �6�7 8@? 1 A : 1 7 8�;and set: $ � 5 ��� �6�7 8 *��! � 5 ��� �8 A 5 ��� �6�7 8@? 1�"

For � = � A������jA#� , evaluate the importanceweights up to a normalizing constant:

� ��� �8 = 2 3 � 5 ��� �6�7 8 9 : 1 7 8�;� 3 � 5 ��� �8 9 5 ��� �6�7 8@? 1 A : 1 7 8<;L2 3 � 5 ��� �6�7 8@? 1 9 : 1 7 8@? 1 ; For � =$� A������WA#� , normalize the importanceweights: %� �&� �8 = � ��� �8(' ()*,+ 1 � � * �8.- ? 1

2. Selection step Multiply/ suppress samples 3 � 5 ��� �6�7 8 ; with high/lowimportance weights

%� ��� �8 , respectively, to obtain �random samples 3 % 5 �&� �6�7 8 ; approximately distributedaccording to 2 3 % 5 ��� �6�7 8 9 : 1 7 8<; .

3. MCMC step Apply a Markov transition kernel with invariantdistribution given by 2 3/5 ��� �6�7 8 9 : 1 7 8 ; to obtain 3/5 �&� �6�7 8 ; ./

4.1 IMPLEMENTATION ISSUES

4.1.1 Sequential importance sampling

If we restrict ourselves to importance functions of the fol-lowing form

� �"+ �"� ��� � ��� �.)( � ��+ ��01 Y � � ��+ 1 � � ��� 1 � + ��� 1 � �� (3)

we can obtain recursive formulas to evaluate � ��+ �"� � (� �*+ �"� ��� � � � and thus � ��� � . The “incremental weight” � �

is given by

� �32 �!�"� � � � ��� ��� � � + �"� � �P�"+ � � + ��� � � �"+ ��� � ��� � � + ��� �����" � � denotes the normalized version of � � , i.e. � HLILK� (4 l V� Y � � H

� K�!5 ��� � HBIJK� . Hence we can perform importance

sampling online.

Choice of the Importance Distribution

There are infinitely many possible choices for � �"+ �"� � � � ��� � ,the only condition being that its supports must include thatof �!�"+ �"� � � � ��� � . The simplest choice is to just sample fromthe prior, �!�"+ � � + ��� � , in which case the importance weightis equal to the likelihood, �!�"� � � � ��� ����� � + �"� � . This is themost widely used distribution, since it is simple to compute,but it can be inefficient, since it ignores the most recentevidence, � � . Intuitively, many of our samples may end upin a region of the space that has low likelihood, and hencereceive low weight; these particles are effectively wasted.

We can show that the “optimal” proposal distribution, inthe sense of minimizing the variance of the importanceweights, takes the most recent evidence into account:

Proposition 3 The distribution that minimizes the vari-ance of the importance weights conditional upon + � � �����and � ��� � is

�!�"+ � � + �"� ��� � � � ��� � ( �!�"� ��� � ��� ��� � � + �"� �' �!�"+ �"� + ��� ���P�"� � � � ��� ��� � � + � � ����� and the associated importance weight � � is

�!��� � � � ��� ����� � + �"� ��� � 0( r �!� � � � � ��� ��� � � + � � � �P�"+ � � + ��� � i�+ �Unfortunately, computing the optimal importance samplingdistribution is often too expensive. Several deterministicapproximations to the optimal distribution have been pro-posed, see for example (de Freitas 1999, Doucet 1998).

Degeneracy of SIS

The following proposition shows that, for importance func-tions of the form (3), the variance of � �*+ �"� �. can only in-crease (stochastically) over time. The proof of this propo-sition is an extension of a Kong-Liu-Wong theorem (Kong

Page 5: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

et al. 1994, p. 285) to the case of an importance function ofthe form (3).

Proposition 4 The unconditional variance (i.e. with theobservations � ��� � being interpreted as random variables)of the importance weights � �@+ � � �. increases over time.

In practice, the degeneracy caused by the variance increasecan be observed by monitoring the importance weights.Typically, what we observe is that, after a few iterations,one of the normalized importance weights tends to 1, whilethe remaining weights tend to zero.

4.1.2 Selection step

To avoid the degeneracy of the sequential importance sam-pling simulation method, a selection (resampling) stagemay be used to eliminate samples with low importance ra-tios and multiply samples with high importance ratios. Aselection scheme associates to each particle + HJIJK� � � a num-ber of offsprings, say D I ��� , such that l VIJY � D I ( D .Several selection schemes have been proposed in the lit-erature. These schemes satisfy # � D I � ( D � HJILK� , buttheir performance varies in terms of the variance of theparticles, var � D I � . Recent theoretical results in (Crisan,Del Moral and Lyons 1999) indicate that the restriction# � D I � ( D � HBIJK� is unnecessary to obtain convergence re-sults (Doucet et al. 1999). Examples of these selectionschemes include multinomial sampling (Doucet 1998, Gor-don et al. 1993, Pitt and Shephard 1999), residual resam-pling (Kitagawa 1996, Liu and Chen 1998) and stratifiedsampling (Kitagawa 1996). Their computational complex-ity is � �@D .4.1.3 MCMC step

After the selection scheme at time � , we obtain D par-ticles distributed marginally approximately according to���*+ � � � � � ��� � . As discussed earlier, the discrete nature of theapproximation can lead to a skewed importance weightsdistribution. That is, many particles have no offspring( D I ( � ), whereas others have a large number of off-spring, the extreme case being D I ( D for a particularvalue Q . In this case, there is a severe reduction in the di-versity of the samples. A strategy for improving the re-sults involves introducing MCMC steps of invariant distri-bution ���*+ �"� �"� � ��� �' on each particle (Andrieu, de Freitas andDoucet 1999b, Gilks and Berzuini 1998, MacEachern et al.1999). The basic idea is that, by applying a Markov tran-sition kernel, the total variation of the current distributionwith respect to the invariant distribution can only decrease.Note, however, that we do not require this kernel to be er-godic.

4.2 CONVERGENCE RESULTS

Let� ����� be the space of bounded, Borel measurable

functions on ��� . We denote ��� � ������������ � -��� �� . The fol-

lowing theorem is a straightforward consequence of Theo-rem 1 in (Crisan and Doucet 2000) which is an extensionof previous results in (Crisan et al. 1999).

Theorem 5 If the importance weights �G� are upperbounded and if one uses one of the selection schemes de-scribed previously, then, for all � / � , there exists � � inde-

pendent of D such that for any � � � F�������� ����� N2 !#" �� () � + 1%$ 8 $ 5 ��� �6�7 8 *�7�& $ 8 3/5 6�7 8 ;"24305 6�7 8 9<: 1 7 8 ;�' 5 6�7 8�(

'*)+��, 8�- $ 8 - '�

where the expectation is taken w.r.t. to the randomness in-troduced by the PF algorithm. This results shows that, un-der very lose assumptions, convergence of this general par-ticle filtering method is ensured and that the convergencerate of the method is independent of the dimension of thestate-space. However, � � usually increases exponentiallywith time. If additional assumptions on the dynamic sys-tem under study are made (e.g. discrete state spaces), itis possible to get uniform convergence results ( � � ( � forany � ) for the filtering distribution �!� , � � � ��� � . We do notpursue this here.

5 EXAMPLES

We now illustrate the theory by briefly describing two ap-plications we have worked on.

5.1 ON-LINE REGRESSION AND MODELSELECTION WITH NEURAL NETWORKS

Consider a function approximation scheme consisting ofa mixture of . radial basis functions (RBFs) and a linearregression term. The number of basis functions, . � , theircenters, / � , the coefficients (weights of the RBF centersplus regression terms), 0 � , and the variance of the Gaussiannoise on the output, y �� , can all vary with time, so we treatthem as latent random variables: see Figure 1. For details,see (Andrieu, de Freitas and Doucet 1999a).

In (Andrieu et al. 1999a), we show that it is possible tosimulate / � , . � and 1 � with a particle filter and to com-pute the coefficients 0 � analytically using Kalman filters.This is possible because the output of the neural networkis linear in 0 � , and hence the system is a conditionally lin-ear Gaussian state-space model (CLGSSM), that is it is alinear Gaussian state-space model conditional upon the lo-cation of the bases and the hyper-parameters. This leads toan efficient RBPF that can be combined with a reversiblejump MCMC algorithm (Green 1995) to select the number

Page 6: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

y

x

y

x

σ 2

µ

α

k

2

2

2

2

2

2

y

x

σ 2

µ

α

k

y

x

σ 2

µ

α

k

σ 2

µ

α

k

σ 2

µ

α

k1 3 4

0

0

0

0

1

1

1

1

1

3

3

3

3

3

4

4

4

4

4

Figure 1: DBN representation of the RBF model. Thehyper-parameters have been omitted for clarity.

230 240 250 260 270 280 290 300 310

−2

−1

0

1

2

Pre

dict

ion

0 50 100 150 200 250 300 350 400 450 5000

2

4

6

k

0 50 100 150 200 250 300 350 400 450

0

0.2

0.4

σ2

Time

Figure 2: The top plot shows the one-step-ahead outputpredictions [—] and the true outputs [ ����� ] for the RBFmodel. The middle and bottom plots show the true val-ues and estimates of the model order and noise variancerespectively.

of basis functions online. For example, we generated somedata from a mixture of 2 RBFs for � ( R�������� � � ��� , andthen from a single RBF for � ( � � R�������� ��R ����� ; the methodwas able to track this change, as shown in Figure 2. Furtherexperiments on real data sets are described in (Andrieu etal. 1999a).

5.2 ROBOT LOCALIZATION AND MAPBUILDING

Consider a robot that can move on a discrete, two-dimensional grid. Suppose the goal is to learn a map ofthe environment, which, for simplicity, we can think of asa matrix which stores the color of each grid cell, whichcan be either black or white. The difficulty is that the color

L1 L2 L3

Y2 Y3Y1

M1(1) M2(1) M3(1)

M2(2)M1(2) M3(2)

Figure 3: A Factorial HMM with 3 hidden chains. � � � Q represents the color of grid cell Q at time � , � � representsthe robot’s location, and � � the current observation.

sensors are not perfect (they may accidentally flip bits), norare the motors (the robot may fail to move in the desired di-rection with some probability due e.g., to wheel slippage).Consequently, it is easy for the robot to get lost. And whenthe robot is lost, it does not know what part of the matrix toupdate. So we are faced with a chicken-and-egg situation:the robot needs to know where it is to learn the map, butneeds to know the map to figure out where it is.

The problem of concurrent localization and map learn-ing for mobile robots has been widely studied. In (Mur-phy 2000), we adopt a Bayesian approach, in which wemaintain a belief state over both the location of the robot,� � � � R�������� � D�� � , and the color of each grid cell, � � � Q �� R�������� � D � , Q ( R�������� � D � , where D � is the numberof cells, and D is the number of colors. The DBN weare using is shown in Figure 3. The state space has size� �*D V �

. Note that we can easily handle changing envi-ronments, since the map is represented as a random vari-able, unlike the more common approach, which treats themap as a fixed parameter.

The observation model is � � ( M��� � ��� �C� , where M� � isa function that flips its binary argument with some fixedprobability. In other words, the robot gets to see the colorof the cell it is currently at, corrupted by noise: � � is anoisy multiplexer with � � acting as a “gate” node. Notethat this conditional independence is not obvious from thegraph structure in Figure 3(a), which suggests that all thenodes in each slice should be correlated by virtue of sharinga common observed child, as in a factorial HMM (Ghahra-mani and Jordan 1997). The extra independence informa-tion is encoded in � � ’s distribution, c.f., (Boutilier, Fried-man, Goldszmidt and Koller 1996).

The basic idea of the algorithm is to sample � ��� � with a PF,and marginalize out the � � � Q nodes exactly, which can bedone efficiently since they are conditionally independentgiven � ��� � :� 3�� 8.3 �"; A������jA � 8'3 ��� ;09 � 1 7 8 A�� 1 7 8<;�=��

(�� + 1 � 3�� 8'3 � ;09 � 1 7 8 A�� 1 7 8<;

Some results on a simple one-dimensional grid world are

Page 7: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

time t

grid

ce

ll i

Prob. location, i.e., P(L(t)=i | y(1:t))

2 4 6 8 10 12 14 16

1

2

3

4

5

6

7

80

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

time t

grid

ce

ll i

Prob. location, i.e., P(L(t)=i | y(1:t)), 50 particles, seed 1

2 4 6 8 10 12 14 16

1

2

3

4

5

6

7

80

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

time t

grid

ce

ll i

BK Prob. location, i.e., P(L(t)=i | y(1:t))

2 4 6 8 10 12 14 16

1

2

3

4

5

6

7

80

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

a b c

Figure 4: Estimated position as the robot moves from cell1 to 8 and back. The robot “gets stuck” in cell 4 for twosteps in a row on the outgoing leg of the journey (hence thedouble diagonal), but the robot does not realize this untilit reaches the end of the “corridor” at step 9, where it isable to relocalise. (a) Exact inference. (b) RBPF with 50particles. (c) Fully-factorised BK.

shown in Figure 4. We compared exact Bayesian infer-ence with the RBPF method, and with the fully-factorisedversion of the Boyen-Koller (BK) algorithm (Boyen andKoller 1998), which represents the belief state as a productof marginals:

� 3 � 8 A � 8 3 �"; A������WA � 8 3 � � ;09 : 1 7 8 ;<= � 3 � 8 9 : 1 7 8 ; ( ��� + 1 � 3�� 8 3 � ;09 : 1 7 8 ;We see that the RBPF results are very similar to the ex-

act results, even with only 50 particles, but that BK getsconfused because it ignores correlations between the mapcells. We have obtained good results learning a R ��� R �map (so the state space has size

� ��� �.��� ) using only 100particles (the observation model in the 2D case is that therobot observes the colors of all the cells in a � � � neighbor-hood centered on its current location). For a more detaileddiscussion of these results, please see (Murphy 2000).

5.3 CONCLUSIONS AND EXTENSIONS

RBPFs have been applied to many problems, mostly inthe framework of conditionally linear Gaussian state-spacemodels and conditionally finite state-space HMMs. That is,they have been applied to models that, conditionally upona set of variables (imputed by the PF algorithm), admit aclosed-form filtering distribution (Kalman filter in the con-tinuous case and HMM filter in the discrete case). One canalso make use of the special structure of the dynamic modelunder study to perform the calculations efficiently using thejunction tree algorithm. For example, if one had evolv-ing trees, one could sample the root nodes with the PF andcompute the leaves using the junction tree algorithm. Thiswould result in a substantial computational gain as one onlyhas to sample the root nodes and apply the juction tree tolower dimensional sub-networks.

Although the previoulsy mentioned models are the most

famous ones, there exist numerous other dynamic systemsadmitting finite dimensional filters. That is, the filteringdistribution can be estimated in closed-form at any time �using a fixed number of sufficient statistics. These include Dynamic models for counting observations (Smith

and Miller 1986). Dynamic models with a time-varying unknow covari-ance matrix for the dynamic noise (West and Harrison1996, Uhlig 1997). Classes of the exponential family state space models(Vidoni 1999).

This list is by no means exhaustive. It, however, shows thatRBPFs apply to very wide class of dynamic models. Con-sequently, they have a big role to play in computer vision(where mixtures of Gaussians arise commonly), robotics,speech and dynamic factor analysis.

References

Akashi, H. and Kumamoto, H. (1977). Random samplingapproach to state estimation in switching environ-ments, Automatica 13: 429–434.

Andrieu, C., de Freitas, J. F. G. and Doucet, A. (1999a). Se-quential Bayesian estimation and model selection ap-plied to neural networks, Technical Report CUED/F-INFENG/TR 341, Cambridge University EngineeringDepartment.

Andrieu, C., de Freitas, J. F. G. and Doucet, A. (1999b). Se-quential MCMC for Bayesian model selection, IEEEHigher Order Statistics Workshop, Ceasarea, Israel,pp. 130–134.

Becker, A., Bar-Yehuda, R. and Geiger, D. (1999). Randomalgorithms for the loop cutset problem.

Bernardo, J. M. and Smith, A. F. M. (1994). Bayesian The-ory, Wiley Series in Applied Probability and Statis-tics.

Boutilier, C., Friedman, N., Goldszmidt, M. and Koller,D. (1996). Context-specific independence in bayesiannetworks, Proc. Conf. Uncertainty in AI.

Boyen, X. and Koller, D. (1998). Tractable inferencefor complex stochastic processes, Proc. Conf. Uncer-tainty in AI.

Casella, G. and Robert, C. P. (1996). Rao-Blackwellisationof sampling schemes, Biometrika 83(1): 81–94.

Cowell, R. G., Dawid, A. P., Lauritzen, S. L. and Spiegel-halter, D. J. (1999). Probabilistic Networks and Ex-pert Systems, Springer-Verlag, New York.

Page 8: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Crisan, D. and Doucet, A. (2000). Convergence of gen-eralized particle filters, Technical Report CUED/F-INFENG/TR 381, Cambridge University EngineeringDepartment.

Crisan, D., Del Moral, P. and Lyons, T. (1999). Dis-crete filtering using branching and interacting parti-cle systems, Markov Processes and Related Fields5(3): 293–318.

de Freitas, J. F. G. (1999). Bayesian Methods for Neu-ral Networks, PhD thesis, Department of Engineer-ing, Cambridge University, Cambridge, UK.

Dean, T. and Kanazawa, K. (1989). A model for reason-ing about persistence and causation, Artificial Intelli-gence 93(1–2): 1–27.

Doucet, A. (1998). On sequential simulation-based meth-ods for Bayesian filtering, Technical Report CUED/F-INFENG/TR 310, Department of Engineering, Cam-bridge University.

Doucet, A., de Freitas, J. F. G. and Gordon, N. J.(2000). Sequential Monte Carlo Methods in Practice,Springer-Verlag.

Doucet, A., Godsill, S. and Andrieu, C. (2000). On se-quential Monte Carlo sampling methods for Bayesianfiltering, Statistics and Computing 10(3): 197–208.

Doucet, A., Gordon, N. J. and Krishnamurthy, V.(1999). Particle filters for state estimation of jumpMarkov linear systems, Technical Report CUED/F-INFENG/TR 359, Cambridge University EngineeringDepartment.

Ghahramani, Z. and Jordan, M. (1997). Factorial HiddenMarkov Models, Machine Learning 29: 245–273.

Gilks, W. R. and Berzuini, C. (1998). Monte Carlo in-ference for dynamic Bayesian models, Unpublished.Medical Research Council, Cambridge, UK.

Gordon, N. J., Salmond, D. J. and Smith, A. F. M. (1993).Novel approach to nonlinear/non-Gaussian Bayesianstate estimation, IEE Proceedings-F 140(2): 107–113.

Green, P. J. (1995). Reversible jump Markov chain MonteCarlo computation and Bayesian model determina-tion, Biometrika 82: 711–732.

Handschin, J. E. and Mayne, D. Q. (1969). Monte Carlotechniques to estimate the conditional expectation inmulti-stage non-linear filtering, International Journalof Control 9(5): 547–559.

Isard, M. and Blake, A. (1996). Contour tracking bystochastic propagation of conditional density, Euro-pean Conference on Computer Vision, Cambridge,UK, pp. 343–356.

Kanazawa, K., Koller, D. and Russell, S. (1995). Stochasticsimulation algorithms for dynamic probabilistic net-works, Proceedings of the Eleventh Conference onUncertainty in Artificial Intelligence, Morgan Kauf-mann, pp. 346–351.

Kitagawa, G. (1996). Monte Carlo filter and smoother fornon-Gaussian nonlinear state space models, Journalof Computational and Graphical Statistics 5: 1–25.

Kong, A., Liu, J. S. and Wong, W. H. (1994). Se-quential imputations and Bayesian missing data prob-lems, Journal of the American Statistical Association89(425): 278–288.

Liu, J. S. and Chen, R. (1998). Sequential Monte Carlomethods for dynamic systems, Journal of the Ameri-can Statistical Association 93: 1032–1044.

MacEachern, S. N., Clyde, M. and Liu, J. S. (1999).Sequential importance sampling for nonparametricBayes models: the next generation, Canadian Jour-nal of Statistics 27: 251–267.

Murphy, K. P. (2000). Bayesian map learning in dynamicenvironments, in S. Solla, T. Leen and K.-R. Muller(eds), Advances in Neural Information ProcessingSystems 12, MIT Press, pp. 1015–1021.

Pearl, J. (1988). Probabilistic Reasoning in Intelligent Sys-tems: Networks of Plausible Inference, Morgan Kauf-mann.

Pitt, M. K. and Shephard, N. (1999). Filtering via simula-tion: Auxiliary particle filters, Journal of the Ameri-can Statistical Association 94(446): 590–599.

Smith, R. L. and Miller, J. E. (1986). Predictive records,Journal of the Royal Statistical Society B 36: 79–88.

Uhlig, H. (1997). Bayesian vector-autoregressions withstochastic volatility, Econometrica.

Vidoni, P. (1999). Exponential family state space modelsbased on a conjugate latent process, Journal of theRoyal Statistical Society B 61: 213–221.

West, M. (1993). Mixture models, Monte Carlo, Bayesianupdating and dynamic models, Computing Scienceand Statistics 24: 325–333.

West, M. and Harrison, J. (1996). Bayesian Forecastingand Dynamic Linear Models, Springer-Verlag.

Page 9: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

On Sequential Monte Carlo Sampling Methods for Bayesian

Filtering

Arnaud Doucet (corresponding author) - Simon Godsill - Christophe Andrieu

Signal Processing Group, Department of Engineering

University of Cambridge

Trumpington Street, CB2 1PZ Cambridge, UK

Email: [email protected]

ABSTRACT

In this article, we present an overview of methods for sequential simulation

from posterior distributions. These methods are of particular interest in Bayesian

filtering for discrete time dynamic models that are typically nonlinear and non-

Gaussian. A general importance sampling framework is developed that unifies

many of the methods which have been proposed over the last few decades in

several different scientific disciplines. Novel extensions to the existing methods

are also proposed. We show in particular how to incorporate local linearisation

methods similar to those which have previously been employed in the determin-

istic filtering literature; these lead to very effective importance distributions.

Furthermore we describe a method which uses Rao-Blackwellisation in order

to take advantage of the analytic structure present in some important classes

of state-space models. In a final section we develop algorithms for prediction,

smoothing and evaluation of the likelihood in dynamic models.

1

Page 10: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Keywords: Bayesian filtering, nonlinear non-Gaussian state space models, sequential

Monte Carlo methods, importance sampling, Rao-Blackwellised estimates

I. Introduction

Many problems in applied statistics, statistical signal processing, time series analysis and

econometrics can be stated in a state space form as follows. A transition equation describes

the prior distribution of a hidden Markov process {xk; k ∈� }, the so-called hidden state

process, and an observation equation describes the likelihood of the observations {yk; k ∈� },

k being a discrete time index. Within a Bayesian framework, all relevant information about

{x0,x1, . . . ,xk} given observations up to and including time k can be obtained from the

posterior distribution p (x0,x1, . . . ,xk|y0,y1, . . . ,yk). In many applications we are interested

in estimating recursively in time this distribution and particularly one of its marginals, the so-

called filtering distribution p (xk|y0,y1, . . . ,yk). Given the filtering distribution one can then

routinely proceed to filtered point estimates such as the posterior mode or mean of the state.

This problem is known as the Bayesian filtering problem or the optimal filtering problem.

Practical applications include target tracking (Gordon et al., 1993), blind deconvolution of

digital communications channels (Clapp et al., 1999)(Liu et al., 1995), estimation of stochastic

volatility (Pitt et al., 1999) and digital enhancement of speech and audio signals (Godsill et

al., 1998).

Except in a few special cases, including linear Gaussian state space models (Kalman

filter) and hidden finite-state space Markov chains, it is impossible to evaluate these dis-

tributions analytically. From the mid 1960’s, a great deal of attention has been devoted

to approximating these filtering distributions, see for example (Jazwinski, 1970). The most

popular algorithms, the extended Kalman filter and the Gaussian sum filter, rely on analyt-

ical approximations (Anderson et al., 1979). Interesting work in the automatic control field

was carried out during the 1960’s and 70’s using sequential Monte Carlo (MC) integration

2

Page 11: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

methods, see (Akashi et al., 1975)(Handschin et. al, 1969)(Handschin 1970)(Zaritskii et al.,

1975). Possibly owing to the severe computational limitations of the time these Monte Carlo

algorithms have been largely neglected until recently. In the late 80’s, massive increases

in computational power allowed the rebirth of numerical integration methods for Bayesian

filtering (Kitagawa 1987). Current research has now focused on MC integration methods,

which have the great advantage of not being subject to the assumption of linearity or Gaus-

sianity in the model, and relevant work includes (Muller 1992)(West, 1993)(Gordon et al.,

1993)(Kong et al., 1994)(Liu et al., 1998).

The main objective of this article is to include in a unified framework many old and

more recent algorithms proposed independently in a number of applied science areas. Both

(Liu et al., 1998) and (Doucet, 1997) (Doucet, 1998) underline the central role of sequential

importance sampling in Bayesian filtering. However, contrary to (Liu et al., 1998) which em-

phasizes the use of hybrid schemes combining elements of importance sampling with Markov

Chain Monte Carlo (MCMC), we focus here on computationally cheaper alternatives. We

describe also how it is possible to improve current existing methods via Rao-Blackwellisation

for a useful class of dynamic models. Finally, we show how to extend these methods to

compute the prediction and fixed-interval smoothing distributions as well as the likelihood.

The paper is organised as follows. In section 2, we briefly review the Bayesian filtering

problem and classical Bayesian importance sampling is proposed for its solution. We then

present a sequential version of this method which allows us to obtain a general recursive

MC filter: the sequential importance sampling (SIS) filter. Under a criterion of minimum

conditional variance of the importance weights, we obtain the optimal importance function for

this method. Unfortunately, for numerous models of applied interest the optimal importance

function leads to non-analytic importance weights, and hence we propose several suboptimal

distributions and show how to obtain as special cases many of the algorithms presented in

the literature. Firstly we consider local linearisation methods of either the state space model

3

Page 12: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

or the optimal importance function, giving some important examples. These linearisation

methods seem to be a very promising way to proceed in problems of this type. Secondly we

consider some simple importance functions which lead to algorithms currently known in the

literature. In Section 3, a resampling scheme is used to limit practically the degeneracy of

the algorithm. In Section 4, we apply the Rao-Blackwellisation method to SIS and obtain

efficient hybrid analytical/MC filters. In Section 5, we show how to use the MC filter to

compute the prediction and fixed-interval smoothing distributions as well as the likelihood.

Finally, simulations are presented in Section 6.

II. Filtering Via Sequential Importance Sampling

A. Preliminaries: Filtering for the State Space Model

The state sequence {xk; k ∈� }, xk ∈

� nx , is assumed to be an unobserved (hidden) Markov

process with initial distribution p (x0) (which we subsequently denote as p (x0|x−1) for no-

tational convenience) and transition distribution p (xk|xk−1), where nx is the dimension of

the state vector. The observations {yk; k ∈� }, yk ∈

� ny , are conditionally independent

given the process {xk; k ∈� } with distribution p (yk|xk) and ny is the dimension of the

observation vector. To sum up, the model is a hidden Markov (or state space) model (HMM)

described by

p (xk|xk−1) for k ≥ 0 (1)

p (yk|xk) for k ≥ 0 (2)

We denote by x0:n

�{x0, ...,xn} and y0:n

�{y0, ...,yn}, respectively, the state sequence and

the observations up to time n. Our aim is to estimate recursively in time the distribution

p (x0:n|y0:n) and its associated features including p (xn|y0:n) and expectations of the form

I (fn) =

∫fn (x0:n) p (x0:n|y0:n) dx0:n (3)

4

Page 13: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

for any p (x0:n|y0:n)-integrable fn :� (n+1)×nx → �

. A recursive formula for p (x0:n|y0:n) is

given by:

p (x0:n+1|y0:n+1) = p (x0:n|y0:n)p (yn+1|xn+1) p (xn+1|xn)

p (yn+1|y0:n)(4)

The denominator of this expression cannot typically be computed analytically, thus rendering

an analytic approach infeasible except in the special cases mentioned above. It will later

be assumed that samples can easily be drawn from p (xk|xk−1) and that we can evaluate

p (xk|xk−1) and p (yk|xk) pointwise.

B. Bayesian Sequential Importance Sampling (SIS)

Since it is generally impossible to sample from the state posterior p (x0:n|y0:n) directly, we

adopt an importance sampling (IS) approach. Suppose that samples{x

(i)0:n; i = 1, ..., N

}are

drawn independently from a normalised importance function π (x0:n|y0:n) which has the

same support as the state posterior. Then an estimate IN (fn) of the posterior expectation

I (fn) is obtained using Bayesian IS (Geweke, 1989):

IN (fn) =

N∑

i=1

fn

(x

(i)0:n

)w(i)

n , w(i)n =

w∗(i)n∑N

j=1 w∗(j)n

(5)

where w∗(i)n = p (y0:n|x0:n) p (x0:n) /π (x0:n|y0:n) is the unnormalised importance weight.

Under weak assumptions IN (fn) converges to I (fn), see for example (Geweke, 1989). How-

ever, this method is not recursive. We now show how to obtain a sequential MC filter using

Bayesian IS.

Suppose one chooses an importance function of the form

π (x0:n|y0:n) = π (x0|y0)n∏

k=1

π (xk|x0:k−1,y0:k) (6)

Such an importance function allows recursive evaluation in time of the importance weights as

successive observations yk become available. We obtain directly the sequential importance

sampling filter.

5

Page 14: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Sequential Importance Sampling (SIS)

For times k = 0, 1, 2, ...

• For i = 1, ..., N , sample x(i)k ∼ π

(xk|x(i)

0:k−1,y0:k

)and x

(i)0:k

� (x

(i)0:k−1,x

(i)k

).

• For i = 1, ..., N , evaluate the importance weights up to a normalising constant:

w∗(i)k = w

∗(i)k−1

p(yk|x(i)

k

)p(x

(i)k

∣∣∣x(i)k−1

)

π(x(i)k

∣∣∣x(i)0:k−1,y0:k)

(7)

• For i = 1, ..., N , normalise the importance weights:

w(i)k =

w∗(i)k∑N

j=1 w∗(j)k

(8)

A special case of this algorithm was introduced in 1969 by (Handschin et. al, 1969)(Hand-

schin 1970). Many of the other algorithms proposed in the literature are later shown also to

be special cases of this general (and simple) algorithm. Choice of importance function is of

course crucial and one obtains poor performance when the importance function is not well

chosen. This issue forms the topic of the following subsection.

C. Degeneracy of the algorithm

If Bayesian IS is interpreted as a Monte Carlo sampling method rather than as a Monte Carlo

integration method, the best possible choice of importance function is of course the posterior

distribution itself, p (x0:k|y0:k). We would ideally like to be close to this case. However,

for importance functions of the form (6), the variance of the importance weights can only

increase (stochastically) over time.

Proposition 1 The unconditional variance of the importance weights, i.e. with the obser-

vations y0:k being interpreted as random variables, increases over time.

6

Page 15: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

The proof of this proposition is a straightforward extension of a Kong-Liu-Wong theorem

(Kong et al., 1994) to the case of an importance function of the form (6). Thus, it is impossible

to avoid a degeneracy phenomenon. In practice, after a few iterations of the algorithm, all

but one of the normalised importance weights are very close to zero and a large computational

effort is devoted to updating trajectories whose contribution to the final estimate is almost

zero.

D. Selection of the importance function

To limit degeneracy of the algorithm, a natural strategy consists of selecting the impor-

tance function which minimises the variance of the importance weights conditional upon the

simulated trajectory x(i)0:k−1 and the observations y0:k.

Proposition 2 π(xk|x0:k−1,y0:k) = p(xk|x(i)k−1,yk) is the importance function which min-

imises the variance of the importance weight w∗(i)k conditional upon x

(i)0:k−1 and y0:k.

Proof. Straightforward calculations yield

varπ

�xk|x

(i)0:k−1,y0:k �

[w∗(i)k

]=

(w∗(i)k−1

)2

p2 (yk|y0:k−1)

∫(p (yk|xk) p

(xk|x(i)

k−1

))2

π(xk|y0:k,x

(i)0:k−1

) dxk − p2(yk|x(i)

k−1

)

This variance is zero for π(xk|y0:k,x

(i)0:k−1

)= p

(xk|yk,x

(i)k−1

).

1. Optimal importance function

The optimal importance function p(xk|x(i)

k−1,yk

)was introduced by (Zaritskii et al., 1975)

then by (Akashi et al., 1977) for a particular case. More recently, this importance function has

been used in (Chen et al., 1996)(Kong et al., 1994)(Liu et al., 1995). For this distribution,

we obtain using (7) for the importance weight w∗(i)k = w

∗(i)k−1p

(yk|x(i)

k−1

). The optimal

importance function suffers from two major drawbacks. It requires the ability to sample

from p(xk|x(i)

k−1,yk

)and to evaluate, up to a proportionality constant, p

(yk|x(i)

k−1

)=

7

Page 16: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

∫p (yk|xk) p

(xk|x(i)

k−1

)dxk. This integral will have no analytic form in the general case.

Nevertheless, analytic evaluation is possible for the important class of models presented

below, the Gaussian state space model with non-linear transition equation.

Example 3 Nonlinear Gaussian State Space Models. Let us consider the following model:

xk = f (xk−1) + vk, vk ∼ N (0,Σv) (9)

yk = Cxk + wk, wk ∼ N (0,Σw) (10)

where f :� nx → � nx is a real-valued non-linear function, C ∈ � ny×nx is an observation

matrix, and vk and wk are mutually independent i.i.d.Gaussian sequences with Σv > 0 and

Σw > 0, Σv and Σw being assumed known. Defining

Σ−1 = Σ−1v

+ CtΣ−1w

C (11)

mk = Σ(Σ−1

vf (xk−1) + CtΣ−1

wyk

)(12)

one obtains

xk|xk−1,yk ∼ N (mk,Σ) (13)

and

p (yk|xk−1) ∝ exp

(−1

2(yk −Cf (xk−1))

t (Σv + CΣwCt)−1

(yk −Cf (xk−1))

)(14)

For many other models, such evaluations are impossible. We now present suboptimal

methods which allow approximation of the optimal importance function. Several Monte

Carlo methods have been proposed to approximate the importance function and the asso-

ciated importance weight based on importance sampling (Doucet, 1997)(Doucet, 1998) and

Markov chain Monte Carlo methods (Berzuini et al., 1998)(Liu et al., 1998). These itera-

tive algorithms are computationally intensive and there is a lack of theoretical convergence

results. However, these methods may be useful when non-iterative schemes fail. In fact, the

8

Page 17: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

general framework of SIS allows us to consider other importance functions built so as to ap-

proximate analytically the optimal importance function. The advantages of this alternative

approach are that it is computationally less expensive than Monte Carlo methods and that

the standard convergence results for Bayesian importance sampling are still valid. There is no

general method to build suboptimal importance functions and it is necessary to build these

on a case by case basis, dependent on the model studied. To this end, it is possible to base

these developments on previous work in suboptimal filtering (Anderson et al., 1979)(West et

al., 1997), and this is considered in the next subsection.

2. Importance distribution obtained by local linearisation

A simple choice selects as the importance function π (xk|xk−1,yk) a parametric distribution

π (xk|θ (xk−1,yk)), with finite-dimensional parameter θ (θ ∈ Θ ⊂ � n �

) determined by xk−1

and yk, θ :� nx× � ny → Θ being a deterministic mapping. Many strategies are possible based

upon this idea. To illustrate such methods, we present here two novel schemes that result in a

Gaussian importance function whose parameters are evaluated using local linearisations, i.e.

which are dependent on the simulated trajectory i = 1, ..., N . Such an approach seems to be

a very promising way of proceeding with many models, where linearisations are readily and

cheaply available. In the auxiliary variables framework of (Pitt and Shephard, 1999), related

‘suboptimal’ importance distributions are proposed to sample efficiently from a finite mixture

distribution approximating the filtering distribution. We follow here a different approach in

which the filtering distribution is approximated directly without resort to auxiliary indicator

variables.

Local linearisation of the state space model We propose to linearise the model locally in

a similar way to the Extended Kalman Filter. However, in our case, this linearisation is

performed with the aim of obtaining an importance function and the algorithm obtained

9

Page 18: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

still converges asymptotically towards the required filtering distribution under the usual

assumptions for importance functions.

Example 4 Let us consider the following model

xk = f (xk−1) + vk, vk ∼ N (0nv×1,Σv) (15)

yk = g (xk) + wk, wk ∼ N (0nw×1,Σw) (16)

where f :� nx → � nx , g :

� nx → � ny are differentiable, vk and wk are two mutually

independent i.i.d. sequences with Σv > 0 and Σw > 0. Performing an approximation up to

first order of the observation equation (Anderson et al., 1979), we get

yk = g (xk) + wk

' g (f (xk−1)) +∂g (xk)

∂xk

∣∣∣∣xk=f(xk−1)

(xk − f (xk−1)) + wk (17)

We have now defined a new model with a similar evolution equation to (15) but with a linear

Gaussian observation equation (17), obtained by linearising g (xk) in f (xk−1). This model

is not Markovian as (17) depends on xk−1. However, it is of the form (9)-(10) and one can

perform similar calculations to obtain a Gaussian importance function π (xk|xk−1,yk) ∼

N (mk,Σk) with mean mk and covariance Σk evaluated for each trajectory i = 1, ..., N using

the following formula:

Σ−1k = Σ−1

v+

[∂g (xk)

∂xk

∣∣∣∣xk=f(xk−1)

]t

Σ−1w

∂g (xk)

∂xk

∣∣∣∣xk=f(xk−1)

(18)

mk = Σk

(Σ−1

vf (xk−1) +

[∂g (xk)

∂xk

∣∣∣∣xk=f(xk−1)

]t

Σ−1w× (19)

×(

yk − g (f (xk−1)) +∂g (xk)

∂xk

∣∣∣∣xk=f(xk−1)

f (xk−1)

))(20)

The associated importance weight is evaluated using (7).

10

Page 19: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Local linearisation of the optimal importance function We assume here that l (xk)�

log p (xk|xk−1,yk) is twice differentiable wrt xk on� nx . We define:

l′ (x)� ∂l (xk)

∂xk

∣∣∣∣xk=x

(21)

l′′ (x)� ∂2l (xk)

∂xk∂xtk

∣∣∣∣xk=x

(22)

Using a second order Taylor expansion in x, we get :

l (xk) ' l (x) +[l′ (x)

]t(xk − x) +

1

2(xk − x)t l′′ (x) (xk − x) (23)

The point x where we perform the expansion is arbitrary (but determined by a deterministic

mapping of xk−1 and yk). Under the additional assumption that l′′ (x) is negative definite,

which is true if l (xk) is concave, then setting

Σ (x) = −l′′ (x)−1 (24)

m (x) = Σ (x)l′ (x) (25)

yields

[l′ (x)

]t(xk − x) +

1

2(xk − x)t l′′ (x) (xk − x)

= C − 1

2(xk − x−m (x))t Σ−1(x) (xk − x−m (x)) (26)

This suggests adoption of the following importance function:

π (xk|xk−1,yk) = N (m (x) + x,Σ (x)) (27)

If p (xk|xk−1,yk) is unimodal, it is judicious to adopt x as the mode of p (xk|xk−1,yk), thus

m (x) = 0nx×1. The associated importance weight is evaluated using (7).

Example 5 Linear Gaussian Dynamic/Observations according to a distribution from the

exponential family. We assume that the evolution equation satisfies:

xk = Axk−1 + vk where vk ∼ N (0nv×1,Σv) (28)

11

Page 20: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

where Σv > 0 and the observations are distributed according to a distribution from the

exponential family, i.e.

p (yk|xk) = exp(yt

kCxk − b (Cxk) + c (yk))

(29)

where C is a real ny × nx matrix, b :� ny → �

and c :� ny → �

. These models have

numerous applications and allow consideration of Poisson or binomial observations, see for

example (West et al., 1997). We have

l (xk) = C + ytkCxk − b (Cxk)−

1

2(xk −Axk−1)

t Σ−1v

(xk −Axk−1) (30)

This yields

l′′ (x) = − ∂2b (Cxk)

∂xk∂xtk

∣∣∣∣xk=x

−Σ−1v

= −b′′ (x)−Σ−1v

(31)

but b′′ (x) is the covariance matrix of yk for xk = x, thus l′′ (x) is definite negative. One

can determine the mode x = x∗ of this distribution by applying an iterative Newton-Raphson

method initialised with x(0) = xk−1, which satisfies at iteration j:

x(j+1) = x(j) −[l′′(x(j)

)]−1l′(x(j)

)(32)

We now present two simpler importance functions which lead to algorithms which previ-

ously appeared in the literature.

3. Prior importance function

A simple choice uses the prior distribution of the hidden Markov model as importance func-

tion. This is the choice made by (Handschin et. al, 1969)(Handschin 1970) in their seminal

work. This is one of the methods recently proposed in (Tanizaki et al., 1998). In this case,

we have π (xk|x0:k−1,y0:k) = p (xk|xk−1) and w∗(i)k = w

∗(i)k−1p

(yk|x(i)

k

). The method is

often inefficient in simulations as the state space is explored without any knowledge of the

12

Page 21: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

observations. It is especially sensitive to outliers. However, it does have the advantage that

the importance weights are easily evaluated. Use of the prior importance function is closely

related to the Bootstrap filter method of (Gordon et al., 1993), see Section III..

4. Fixed importance function

An even simpler choice fixes an importance function independently of the simulated trajec-

tories and of the observations. In this case, we have π (xk|x0:k−1,y0:k) = π (xk) and

w∗(i)k = w

∗(i)k−1p

(yk|x(i)

k

)p(x

(i)k

∣∣∣x(i)k−1

)/π(x

(i)k

)(33)

This is the importance function adopted by (Tanizaki, 1993)(Tanizaki, 1994) who present

this method as a stochastic alternative to the numerical integration method of (Kitagawa,

1987). The results obtained are rather poor as neither the dynamic of the model nor the

observations are taken into account and leads in most cases to unbounded (unnormalised)

importance weights which will give poor results (Geweke, 1989).

III. Resampling

As has previously been illustrated, the degeneracy of the SIS algorithm is unavoidable. The

basic idea of resampling methods is to eliminate trajectories which have small normalised

importance weights and to concentrate upon trajectories with large weights. A suitable

measure of degeneracy of the algorithm is the effective sample size Neff introduced in (Kong

et al., 1994)(Liu, 1996) and defined as:

Neff =N

1 + varπ( ·|y0:k) (w∗ (x0:k))=

N�

π( ·|y0:k)

[(w∗ (x0:k))

2] ≤ N (34)

One cannot evaluate Neff exactly but, an estimate�

Neff of Neff is given by:

Neff =N

1N

∑Ni=1

(w∗(i)k

)2 =1

∑Ni=1

(w

(i)k

)2 (35)

13

Page 22: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

When�

Neff is below a fixed threshold Nthres, the SIR resampling procedure is used (Rubin,

1988). Note that it is possible to implement the SIR procedure exactly in O (N) operations

by using a classical algorithm (Ripley, 1987 p. 96) and (Carpenter et al., 1997)(Doucet,

1997)(Doucet, 1998)(Pitt et al., 1999). Other resampling procedures which reduce the MC

variation, such as stratified sampling (Carpenter et al., 1997) and residual resampling (Liu

et al., 1998), may be applied as an alternative to SIR.

An appropriate algorithm based on the SIR scheme proceeds as follows at time k.

SIS/Resampling Monte Carlo filter

1. Importance sampling

• For i = 1, ..., N , sample x(i)k ∼ π(xk|x(i)

0:k−1,y0:k) and x(i)0:k

� (x

(i)0:k−1, x

(i)k

).

• For i = 1, ..., N , evaluate the importance weights up to a normalising constant:

w∗(i)k = w

∗(i)k−1

p(yk| x(i)

k

)p(x

(i)k

∣∣∣ x(i)k−1

)

π( x(i)k

∣∣∣ x(i)0:k−1,y0:k)

(36)

• For i = 1, ..., N , normalise the importance weights:

w(i)k =

w∗(i)k∑N

j=1 w∗(j)k

(37)

• Evaluate�

Neff using (35).

2. Resampling

If�

Neff ≥ Nthres

• x(i)0:k = x

(i)0:k for i = 1, ..., N .

otherwise

• For i = 1, ..., N , sample an index j (i) distributed according to the discrete distribution

with N elements satisfying Pr{j (i) = l} = w(l)k for l = 1, ..., N .

14

Page 23: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

• For i = 1, ..., N , x(i)0:k = x

j(i)0:k and w

(i)k = 1

N .

If�

Neff ≥ Nthres, the algorithm presented in Subsection B. is thus not modified and if

Neff < Nthres the SIR algorithm is applied and one obtains

P (dx0:k|y0:k) =1

N

N∑

i=1

δx

(i)0:k

(dx0:k) (38)

Resampling procedures decrease algorithmically the degeneracy problem but introduce

practical and theoretical problems. From a theoretical point of view, after one resampling

step, the simulated trajectories are no longer statistically independent and so we lose the

simple convergence results given previously. Recently, (Berzuini et al., 1998) have however

established a central limit theorem for the estimate of I (fk) obtained when the SIR procedure

is applied at each iteration. From a practical point of view, the resampling scheme limits

the opportunity to parallelise since all the particles must be combined, although the IS

steps can still be realized in parallel. Moreover the trajectories{x

(i)0:k, i = 1, ..., N

}which

have high importance weights w(i)k are statistically selected many times. In (38), numerous

trajectories x(i1)0:k and x

(i2)0:k are in fact equal for i1 6= i2 ∈ [1, . . . , N ]. There is thus a loss of

“diversity”. Various heuristic methods have been proposed to solve this problem (Gordon et

al., 1993)(Higuchi, 1997).

IV. Rao-Blackwellisation for Sequential Importance Sampling

In this section we describe variance reduction methods which are designed to make the most of

any structure within the model studied. Numerous methods have been developed for reducing

the variance of MC estimates including antithetic sampling (Handschin et. al, 1969)(Hand-

schin 1970) and control variates (Akashi et al., 1975)(Handschin 1970). We apply here the

Rao-Blackwellisation method, see (Casella et al. 1996) for a general reference on the topic.

In a sequential framework, (MacEachern et al. 1998) have applied similar ideas for Dirichlet

15

Page 24: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

process models and (Kong et al. 1994)(Liu et al. 1998) have used Rao-Blackwellisation for

fixed parameter estimation. We focus on its application to dynamic models. We show how it

is possible to successfully apply this method to an important class of state space model and

obtain hybrid filters where a part of the calculations is realised analytically and the other

part using MC methods.

The following method is useful for cases when one can partition the state xk as(x1

k,x2k

)and

analytically marginalize one component of the partition, say x2k. For instance, as demon-

strated in example 6, if one component of the partition is a conditionally linear Gaussian

state-space model then all the integrations can be performed analytically on-line using the

Kalman filter. Let us define xj0:n

� (xj

0, . . . ,xjn

). We can rewrite the posterior expectation

I (fn) in terms of marginal quantities:

I (fn) =

∫ [∫fn

(x1

0:n,x20:n

)p(y0:n|x1

0:n,x20:n

)p(x2

0:n

∣∣x10:n

)dx2

0:n

]p(x1

0:n

)dx1

0:n∫ [∫p(y0:n|x1

0:n,x20:n

)p(x2

0:n

∣∣x10:n

)dx2

0:n

]p(x1

0:n

)dx1

0:n

=

∫g(x1

0:n)p(x1

0:n

)dx1

0:n∫p(y0:n|x1

0:n

)p(x1

0:n

)dx1

0:n

where

g(x10:n)

�∫

fn

(x1

0:n,x20:n

)p(y0:n|x1

0:n,x20:n

)p(x2

0:n

∣∣x10:n

)dx2

0:n (39)

Under the assumption that, conditional upon a realisation of x10:n, g(x1

0:n) and p(y0:n|x1

0:n

)

can be evaluated analytically, two estimates of I (fn) based on IS are possible. The first

“classical” one is obtained using as importance distribution π(x1

0:n,x20:n

∣∣y0:n

):

IN (fn) =

∑Ni=1 fn

(x

1,(i)0:n ,x

2,(i)0:n

)w∗(x

1,(i)0:n ,x

2,(i)0:n

)

∑Ni=1 w∗

(x

1,(i)0:n ,x

2,(i)0:n

) (40)

where w∗(x

1,(i)0:n ,x

2,(i)0:n

)∝ p

(x

1,(i)0:n ,x

2,(i)0:n

∣∣∣y0:n

)/π(x

1,(i)0:n ,x

2,(i)0:n

∣∣∣y0:n

). The second “Rao-

Blackwellised” estimate is obtained by analytically integrating out x20:n and using as im-

portance distribution π(x1

0:n

∣∣y0:n

)=∫

π(x1

0:n,x20:n

∣∣y0:n

)dx2

0:n. The new estimate is given

by:

IN (fn) =

∑Ni=1 g

(x

1,(i)0:n

)w∗(x

1,(i)0:n

)

∑Ni=1 w∗

(x

1,(i)0:n

) (41)

16

Page 25: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

where w∗(x

1,(i)0:n

)∝ p

(x

1,(i)0:n

∣∣∣y0:n

)/π(x

1,(i)0:n

∣∣∣y0:n

). Using the decomposition of the variance,

it is straightforward to show that the variances of the importance weights obtained by Rao-

Blackwellisation are smaller than those obtained using a direct Monte Carlo method (40), see

for example (Doucet 1997)(Doucet 1998)(MacEachern et al. 1998). We can use this method

to estimate I (fn) and marginal quantities such as p(x1

0:n

∣∣y0:n

).

One has to be cautious when applying the MC methods developed in the previous sec-

tions to the marginal state space x1k. Indeed, even if the observations y0:n are independent

conditional upon(x1

0:n,x20:n

), they are generally no longer independent conditional upon the

single process x10:n. The required modifications are, however, straightforward. For exam-

ple, we obtain for the optimal importance function p(x1

k

∣∣y0:k,x10:k−1

)and its associated

importance weight p(yk|y0:k−1,x

10:k−1

). We now present two important applications of this

general method.

Example 6 Conditionally linear Gaussian state space model

Let us consider the following model

p(x1

k

∣∣x1k−1

)(42)

x2k = Ak

(x1

k

)x2

k−1 + Bk

(x1

k

)vk (43)

yk = Ck

(x1

k

)x2

k + Dk

(x1

k

)wk (44)

where x1k is a Markov process, vk ∼ N (0nv×1, Inv

) and wk ∼ N (0nw×1, Inw). One wants to

estimate p(x1

0:n

∣∣y0:n

),

� (f(x1

n

)∣∣y0:n

),

� (x2

n

∣∣y0:n

)and

�(

x2n

(x2

n

)t∣∣∣y0:n

). It is possible

to use a MC filter based on Rao-Blackwellisation. Indeed, conditional upon x10:n, x2

0:n is a

linear Gaussian state space model and the integrations required by the Rao-Blackwellisation

method can be realized using the Kalman filter.

Akashi and Kumamoto (Akashi et al., 1977)(Tugnait, 1982) introduced this algorithm

under the name of RSA (Random Sampling Algorithm) in the particular case where x1k is a

17

Page 26: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

homogeneous scalar finite state-space Markov chain. In this case, they adopted the optimal

importance function p(x1

k

∣∣y0:k,x10:k−1

). Indeed, it is possible to sample from this discrete

distribution and to evaluate the importance weight p(yk|y0:k,x

10:k−1

)using the Kalman filter

(Akashi et al., 1977). Similar developments for this special case have also been proposed by

(Svetnik, 1986)(Billio et al., 1998)(Liu et al., 1998). The algorithm for blind deconvolution

proposed by (Liu et al., 1995) is also a particular case of this method where x2k = h is a time-

invariant channel of Gaussian prior distribution. Using the Rao-Blackwellisation method in

this framework is particularly attractive as, while xk has some continuous components, we

restrict ourselves to the exploration of a discrete state space.

Example 7 Finite State-Space HMM

Let us consider the following model

p(x1

k

∣∣x1k−1

)(45)

p(x2

k

∣∣x1k,x

2k−1

)(46)

p(yk|x1

k,x2k

)(47)

where x1k is a Markov process and x2

k is a finite state-space Markov chain whose param-

eters at time k depend on x1k. We want to estimate p

(x1

0:n

∣∣y0:n

),

� (f(x1

n

)∣∣y0:n

)and

� (f(x2

n

)∣∣y0:n

). It is possible to use a “Rao-Blackwellised” MC filter. Indeed, conditional

upon x10:n, x2

0:n is a finite state-space Markov chain of known parameters and thus the inte-

grations required by the Rao-Blackwellisation method can be done analytically (Anderson et

al., 1979).

18

Page 27: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

V. Prediction, smoothing and likelihood

The estimate of the joint distribution p (x0:k|y0:k) based on SIS, in practice coupled with a

resampling procedure to limit the degeneracy, is at any time k of the following form:

P (dx0:k|y0:k) =N∑

i=1

w(i)k δ

x(i)0:k

(dx0:k) (48)

We show here how it is possible to obtain based on this distribution some approximations of

the prediction and smoothing distributions as well as the likelihood.

A. Prediction

Based on the approximation of the filtering distribution P (dxk|y0:k), we want to estimate

the p step-ahead prediction distribution, p ≥ 2 ∈ � ∗ , given by:

p (xk+p|y0:k) =

∫p (xk|y0:k)

k+p∏

j=k+1

p (xj |xj−1)

dxk:k+p−1 (49)

Replacing p (xk|y0:k) in (49) by its approximation obtained from (48), we obtain:

N∑

i=1

w(i)k

∫p(xk+1|x(i)

k

) k+p∏

j=k+2

p (xj|xj−1) dxk+1:k+p−1 (50)

To evaluate these integrals, it is sufficient to extend the trajectories x(i)0:k using the evolution

equation.

p step-ahead prediction

• For j = 1 to p

– For i = 1, ..., N , sample x(i)k+j ∼ p

(xk+j|x(i)

k+j−1

)and x

(i)0:k+j

� (x

(i)0:k+j−1,x

(i)k+j

).

We obtain random samples{x

(i)0:k+p; i = 1, ..., N

}. An estimate of P (dx0:k+p|y0:k) is

given by

P (dx0:k+p|y0:k) =

N∑

i=1

w(i)k δ

x(i)0:k+p

(dx0:k+p)

19

Page 28: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Thus

P (dxk+p|y0:k) =N∑

i=1

w(i)k δ

x(i)k+p

(dxk+p) (51)

B. Fixed-Lag smoothing

We want to estimate the fixed-lag smoothing distribution p (xk|y0:k+p), p ∈ � ∗ being the

length of the lag. At time k + p, the MC filter yields the following approximation of

p (x0:k+p|y0:k+p):

P (dx0:k+p|y0:k+p) =N∑

i=1

w(i)k+pδx

(i)0:k+p

(dx0:k+p) (52)

By marginalising, we obtain an estimate of the fixed-lag smoothing distribution:

P (dxk|y0:k+p) =N∑

i=1

w(i)k+pδx

(i)k

(dxk) (53)

When p is high, such an approximation will generally perform poorly.

C. Fixed-interval smoothing

Given y0:n, we want to estimate p (xk|y0:n) for any k = 0, ..., n. At time n, the filtering

algorithm yields the following approximation of p (x0:n|y0:n) :

P (dx0:n|y0:n) =

N∑

i=1

w(i)n δ

x(i)0:n

(dx0:n) (54)

Thus one can theoretically obtain p (xk|y0:n) for any k by marginalising this distribution.

Practically, this method cannot be used as soon as (n− k) is significant as the degener-

acy problem requires use of a resampling algorithm. At time n, the simulated trajectories

{x

(i)0:n; i = 1, ..., N

}have been usually resampled many times: there are thus only a few dis-

tinct trajectories at times k for k � n and the above approximation of p (xk|y0:n) is bad.

This problem is even more severe for the bootstrap filter where one resamples at each time

instant.

20

Page 29: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

It is necessary to develop an alternative algorithm. We propose an original algorithm to

solve this problem. This algorithm is based on the following formula (Kitagawa, 1987):

p (xk|y0:n) = p (xk|y0:k)

∫p (xk+1|y0:n) p (xk+1|xk)

p (xk+1|y0:k)dxk+1 (55)

We seek here an approximation of the fixed-interval smoothing distribution with the following

form:

P (dxk|y0:n)� N∑

i=1

w(i)k|nδ

x(i)k

(dxk) (56)

i.e. P (dxk|y0:n) has the same support{x

(i)k ; i = 1, . . . , N

}as the filtering distribution

P (dxk|y0:k) but the weights are different. An algorithm to obtain these weights{w

(i)k|n; i = 1, . . . , N

}

is the following.

Fixed-interval smoothing

1. Initialisation at time k = n.

• For i = 1, ..., N , w(i)n|n = w

(i)n .

2. For k = n− 1, ..., 0.

• For i = 1, ..., N , evaluate the importance weight

w(i)k|n =

N∑

j=1

w(j)k+1|n

w(i)k p

(x

(j)k+1

∣∣∣x(i)k

)

[∑Nl=1 w

(l)k p

(x

(j)k+1

∣∣∣x(l)k

)] (57)

This algorithm is obtained by the following argument. Replacing p (xk+1|y0:n) by its

approximation (56) yields

∫p (xk+1|y0:n) p (xk+1|xk)

p (xk+1|y0:k)dxk+1 '

N∑

i=1

w(i)k+1|n

p(x

(i)k+1

∣∣∣xk

)

p(x

(i)k+1

∣∣∣y0:k

) (58)

21

Page 30: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

where, owing to (48), p(x

(i)k+1

∣∣∣y0:k

)can be approximated by

p(x

(i)k+1

∣∣∣y0:k

)=

∫p(x

(i)k+1

∣∣∣xk

)p (xk|y0:k) dxk (59)

'N∑

j=1

w(j)k p

(x

(i)k+1

∣∣∣x(j)k

)

An approximation P (dxk|y0:n) of p (xk|y0:n) is thus

P (dxk|y0:n) (60)

=

[N∑

i=1

w(i)k δ

x(i)k

(dxk)

]N∑

j=1

w(j)k+1|n

p(x

(j)k+1

∣∣∣xk

)

[∑Nl=1 w

(l)k p

(x

(j)k+1

∣∣∣x(l)k

)]

=

N∑

i=1

w(i)k

N∑

j=1

w(j)k+1|n

p(x

(j)k+1

∣∣∣x(i)k

)

[∑Nl=1 w

(l)k p

(x

(j)k+1

∣∣∣x(l)k

)]

δ

x(i)k

(dxk)

�N∑

i=1

w(i)k|nδ

x(i)k

(dxk)

The algorithm follows.

This algorithm requires storage of the marginal distributions P (dxk|y0:k) (weights and

supports) for any k = 0, ..., n. The memory requirement is O (nN). Its complexity is

O(nN2

), which is quite important as N � 1. However this complexity is a little lower

than the one of the previous developed algorithms of (Kitagawa et al., 1996) and (Tanizaki

et al., 1998) as it does not require any new simulation step.

D. Likelihood

In some applications, in particular for model choice (Kitagawa, 1987)(Kitagawa et al., 1996),

we may wish to estimate the likelihood of the data

p (y0:n) =

∫w∗

nπ (x0:n|y0:n) dx0:n

A simple estimate of the likelihood is thus given by

p (y0:n) =1

N

N∑

j=1

w(j)n (61)

22

Page 31: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

In practice, the introduction of resampling steps makes this approach impossible. We will

use an alternative decomposition of the likelihood:

p (y0:n) = p (y0)

n∏

k=1

p (yk|y0:k−1) (62)

where:

p (yk|y0:k−1) =

∫p (yk|xk) p (xk|y0:k−1) dxk (63)

=

∫p (yk|xk−1) p (xk−1|y0:k−1) dxk−1 (64)

Using (63), an estimate of this quantity is given by

p (yk|y0:k−1) =

N∑

i=1

p(yk| x(i)

k

)w

(i)k−1 (65)

where the samples{x

(i)k ; i = 1, . . . , N

}are obtained using a one-step ahead prediction based

on the approximation P (dxk−1|y0:k−1) of p (xk−1|y0:k−1). Using expression (64), it is pos-

sible to avoid a MC integration if we know analytically p(yk|x(i)

k−1

):

p (yk|y0:k−1) =

N∑

i=1

p(yk|x(i)

k−1

)w

(i)k−1 (66)

VI. Simulations

In this section, we apply the methods developed previously to a linear Gaussian state space

model and to a classical nonlinear model. We make for these two models M = 100 simulations

of length n = 500 and we evaluate the empirical standard deviation for the filtering estimates

xk|k =�

[xk|y0:k] obtained by the MC methods:

√V AR

(xk|l

)=

1

n

n∑

k=1

1

M

M∑

j=1

(xj

k|l − xjk

)2

1/2

where:

• xjk is the simulated state for the jth simulation, j = 1, ...,M .

23

Page 32: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

• xjk|l

� ∑Ni=1 w

(i)k|lx

j,(i)k is the MC estimate of

�[xk|y0:l] for the jth test signal and x

j,(i)k

is the ith simulated trajectory, i = 1, ..., N , associated with the signal j. (We denote

w(i)k|k

�w

(i)k )

These calculations have been realised for N = 100, 250, 500, 1000, 2500 and 5000. The

implemented filtering algorithms are the bootstrap filter, the SIS with the prior importance

function and the SIS with the optimal or a suboptimal importance function. The fixed-

interval smoothers associated with these SIS filters are then computed.

For the SIS-based algorithms, the SIR procedure has been used when�

Neff < Nthres =

N/3. We state the percentage of iterations where the SIR step is used for each importance

function.

A. Linear Gaussian model

Let us consider the following model

xk = xk−1 + vk (67)

yk = xk + wk (68)

where x0 ∼ N (0, 1), vk and wk are white Gaussian noises mutually independent, vk ∼

N(0, σ2

v

)and wk ∼ N

(0, σ2

w

)with σ2

v = σ2w = 1. For this model, the optimal filter is the

Kalman filter (Anderson et al., 1979).

1. Optimal importance function

The optimal importance function is

xk| xk−1, yk ∼ N(mk, σ

2k

)(69)

where

σ−2k = σ−2

w + σ−2v (70)

mk = σ2k

(xk−1

σ2v

+yk

σ2w

)(71)

24

Page 33: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

and the associated importance weight is equal to:

p (yk|xk−1) ∝ exp

(−1

2

(yk − xk−1)2

(σ2v + σ2

w)

)(72)

2. Results

For the Kalman filter, we obtain√

V AR(xk|k

)= 0.79. For the different MC filters, the

results are presented in Table 1 and Table 2.

With N = 500 trajectories, the estimates obtained using MC methods are similar to those

obtained by Kalman. The SIS algorithms have similar performances to the bootstrap filter

for a smaller computational cost. The most interesting algorithm is based on the optimal

importance function which limits seriously the number of resampling steps.

B. Nonlinear series

We consider here the following nonlinear reference model (Gordon et al., 1993)(Kitagawa,

1987)(Tanizaki et al., 1998):

xk = f (xk−1) + vk (73)

=1

2xk−1 + 25

xk−1

1 + (xk−1)2 + 8 cos (1.2k) + vk

yk = g (xk) + wk (74)

=(xk)

2

20+ wk

where x0 ∼ N (0, 5), vk and wk are mutually independent white Gaussian noises, vk ∼

N(0, σ2

v

)and wk ∼ N

(0, σ2

w

)with σ2

v = 10 and σ2w = 1. In this case, it is not possible

to evaluate analytically p (yk|xk−1) or to sample simply from p (xk| xk−1, yk). We propose

to apply the method described in 2. which consists of linearising locally the observation

equation.

25

Page 34: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

1. Importance function obtained by local linearisation

We get

yk ' g (f (xk−1)) +∂g (xk)

∂xk

∣∣∣∣xk=f(xk−1)

(xk − f (xk−1)) + wk

=f2 (xk−1)

20+

f (xk−1)

10(xk − f (xk−1)) + wk

= −f2 (xk−1)

20+

f (xk−1)

10xk + wk (75)

Then we obtain the linearised importance function π (xk| xk−1, yk) = N(xk;mk, (σk)

2)

where

(σk)−2 = σ−2

v + σ−2w

f2 (xk−1)

100(76)

and

mk = (σk)2

[σ−2

v f (xk−1) + σ−2w

f (xk−1)

10

(yk +

f2 (xk−1)

20

)](77)

2. Results

In this case, it is not possible to estimate the optimal filter. For the MC filters, the results

are displayed in Table 3. The average percentages of SIR steps are presented in Table 4.

This model requires simulation of more samples than the preceding one. In fact, the

variance of the dynamic noise is more important and more trajectories are necessary to

explore the space. The most interesting algorithm is the SIS with a suboptimal importance

function which greatly limits the number of resampling steps over the prior importance

function while avoiding a MC integration step needed to evaluate the optimal importance

function. This can be roughly explained by the fact that the observation noise is rather small

so that yk is highly informative and allows a limitation of the regions explored.

VII. Conclusion

We have presented an overview of sequential simulation-based methods for Bayesian filtering

of general state-space models. We include, within the general framework of SIS, numer-

26

Page 35: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

ous approaches proposed independently in the literature over the last 30 years. Several

original extensions have also been described, including the use of local linearisation tech-

niques to yield more effective importance distributions. We have shown also how the use of

Rao-Blackwellisation allows us to make the most of any analytic structure present in some

important dynamic models and have described procedures for prediction, fixed-lag smoothing

and likelihood evaluation.

These methods are efficient but still suffer from several drawbacks. The first is the de-

pletion of samples which inevitably occurs in all of the methods described as time proceeds.

Sample regeneration methods based upon MCMC steps are likely to improve the situation

here (MacEachern et al. 1998). A second problem is that of simulating fixed hyperparame-

ters such as the covariance matrices and noise variances which were assumed known in our

examples. The methods described here do not allow for any regeneration of new values for

these non-dynamic parameters, and hence we can expect a very rapid impoverishment of

the sample set. Again, a combination of the present techniques with MCMC steps could be

useful here, as could Rao-Blackwellisation methods ((Liu et al. 1998) give some insight into

how this might be approached).

The technical challenges still posed by this problem, together with the wide range of

important applications and the rapidly increasing computational power, should stimulate

new and exciting developments in the field.

References

[1] Akashi H. and Kumamoto H.(1975) Construction of Discrete-time Nonlinear Filter by

Monte Carlo Methods with Variance-reducing Techniques. Systems and Control, 19,

211-221 (in Japanese).

[2] Akashi H. and Kumamoto H.(1977) Random Sampling Approach to State Estimation

27

Page 36: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

in Switching Environments. Automatica, 13, 429-434.

[3] Anderson B.D.O. and Moore J.B. (1979) Optimal Filtering, Englewood Cliffs.

[4] Berzuini C., Best N., Gilks W. and Larizza C. (1997) Dynamic Conditional Independence

Models and Markov Chain Monte Carlo Methods. Journal of the American Statistical

Association, 92, pp.1403-1412.

[5] Billio M. and Monfort A. (1998) Switching State-Space Models:Likelihood Function,

Filtering and Smoothing. Journal of Statistical Planning and Inference, 68, pp.65-103.

[6] Carpenter J., Clifford P. and Fearnhead P. (1997) An Improved Particle Filter for Non-

linear Problems. Technical report University of Oxford, Dept. of Statistics.

[7] Casella G. and Robert C.P. (1996) Rao-Blackwellisation of Sampling Schemes.

Biometrika, 83, pp. 81-94.

[8] Chen R. and Liu J.S. (1996) Predictive Updating Methods with Application to Bayesian

Classification. Journal of the Royal Statistical Society B, 58, 397-415.

[9] Clapp T.C. and Godsill S.J. (1999) Fixed-Lag Smoothing using Sequential Importance

Sampling. forthcoming Bayesian Statistics 6, J.M. Bernardo, J.O. Berger, A.P. Dawid

and A.F.M. Smith (eds.), Oxford University Press.

[10] Doucet A. (1997) Monte Carlo Methods for Bayesian Estimation of Hidden Markov

Models. Application to Radiation Signals. Ph.D. Thesis, University Paris-Sud Orsay (in

French).

[11] Doucet A. (1998) On Sequential Simulation-Based Methods for Bayesian Filtering. Tech-

nical report University of Cambridge, Dept. of Engineering, CUED-F-ENG-TR310.

Available on the MCMC preprint service at http://www.stats.bris.ac.uk/MCMC/.

28

Page 37: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

[12] Geweke J. (1989) Bayesian Inference in Econometrics Models using Monte Carlo Inte-

gration. Econometrica, 57, 1317-1339.

[13] Godsill S.J and Rayner P.J.W. (1998) Digital Audio Restoration - A Statistical Model-

Based Approach, Springer.

[14] Gordon N.J., Salmond D.J. and Smith A.F.M. (1993) Novel Approach to Nonlinear/Non-

Gaussian Bayesian State Estimation. IEE-Proceedings-F, 140, 107-113.

[15] Gordon N.J. (1997) A Hybrid Bootstrap Filter for Target Tracking in Clutter. IEEE

Transactions on Aerospace and Electronic Systems, 33, 353-358.

[16] Handschin J.E. and Mayne D.Q. (1969) Monte Carlo Techniques to Estimate the Condi-

tional Expectation in Multi-stage Non-linear Filtering. International Journal of Control,

9, 547-559.

[17] Handschin J.E. (1970) Monte Carlo Techniques for Prediction and Filtering of Non-

Linear Stochastic Processes. Automatica, 6, 555-563.

[18] Higuchi T. (1997) Monte Carlo Filtering using the Genetic Algorithm Operators. Journal

of Statistical Computation and Simulation, 59, 1-23.

[19] Jazwinski A.H. (1970) Stochastic Processes and Filtering Theory, Academic Press.

[20] Kitagawa G. (1987) Non-Gaussian State-Space Modeling of Nonstationary Time Series.

Journal of the American Statistical Association, 82, 1032-1063.

[21] Kitagawa G. and Gersch G. (1996) Smoothness Priors Analysis of Time Series, Lecture

Notes in Statistics, 116, Springer.

[22] Kong A., Liu J.S. and Wong W.H. (1994) Sequential Imputations and Bayesian Missing

Data Problems. Journal of the American Statistical Association, 89, 278-288.

29

Page 38: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

[23] Liu J.S. and Chen R.(1995) Blind Deconvolution via Sequential Imputation. Journal of

the American Statistical Association, 90, 567-576.

[24] Liu J.S. (1996) Metropolized Independent Sampling with Comparison to Rejection Sam-

pling and Importance Sampling. Statistics and Computing, 6, 113-119.

[25] Liu J.S. and Chen R. (1998) Sequential Monte Carlo Methods for Dynamic Systems.

Journal of the American Statistical Association, 93, 1032-1044.

[26] MacEachern S.N, Clyde M. and Liu J.S. (1998), Sequential Importance Sampling for

Nonparametric Bayes Models: The Next Generation, forthcoming Canadian Journal of

Statistics.

[27] Muller P. (1991) Monte Carlo Integration in General Dynamic Models. Contemporary

Mathematics, 115, 145-163.

[28] Muller P. (1992) Posterior Integration in Dynamic Models. Computing Science and

Statistics, 24, 318-324.

[29] Pitt M.K. and Shephard N. (1999) Filtering via Simulation:Auxiliary Particle Filters.

forthcoming Journal of the American Statistical Association.

[30] Ripley B.D., Stochastic Simulation, Wiley, New York, 1987.

[31] Rubin D.B. (1988) Using the SIR Algorithm to Simulate Posterior Distributions. in

Bayesian Statistics 3 (Eds J.M. Bernardo, M.H. DeGroot, D.V. Lindley et A.F.M.

Smith), Oxford University Press, 395-402.

[32] Smith A.F.M. and Gelfand A.E. (1992) Bayesian Statistics without Tears: a Sampling-

Resampling Perspective. The American Statistician, 46, 84-88.

30

Page 39: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

[33] Stewart L. and McCarty P. (1992) The Use of Bayesian Belief Networks to Fuse Contin-

uous and Discrete Information for Target Recognition, Tracking and Situation Assess-

ment. Proceeding Conference SPIE, 1699, 177-185.

[34] Svetnik V.B. (1986) Applying the Monte Carlo Method for Optimum Estimation in

Systems with Random Disturbances. Automation and Remote Control, 47, 818-825.

[35] Tanizaki H. (1993) Nonlinear Filters: Estimation and Applications, Lecture Notes in

Economics and Mathematical Systems, 400, Springer, Berlin.

[36] Tanizaki H. and Mariano R.S. (1994) Prediction, Filtering and Smoothing in Non-linear

and Non-normal Cases using Monte Carlo Integration. Journal of Applied Econometrics,

9, 163-179.

[37] Tanizaki H. and Mariano R.S. (1998) Nonlinear and Non-Gaussian State-Space Modeling

with Monte-Carlo Simulations. Journal of Econometrics, 83, 263-290.

[38] Tugnait J.K. (1982) Detection and Estimation for Abruptly Changing Systems. Auto-

matica, 18, 607-615.

[39] West M. (1993) Mixtures Models, Monte Carlo, Bayesian Updating and Dynamic Mod-

els. Computer Science and Statistics, 24, 325-333.

[40] West M. and Harrison J.F. (1997) Bayesian Forecasting and Dynamic Models, Springer

Verlag Series in Statistics, 2nd edition.

[41] Zaritskii V.S., Svetnik V.B. and Shimelevich L.I. (1975) Monte Carlo Technique in Prob-

lems of Optimal Data Processing. Automation and Remote Control, 12, 95-103.

31

Page 40: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

VIII. Tables

√V AR

(xk|k

)bootstrap prior dist. optimal dist.

N = 100 0.80 0.86 0.83

N = 250 0.81 0.81 0.80

N = 500 0.79 0.80 0.79

N = 1000 0.79 0.79 0.79

N = 2500 0.79 0.79 0.79

N = 5000 0.79 0.79 0.79

Table 1: MC filters: linear Gaussian model

32

Page 41: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Percentage SIR prior dist. optimal dist.

N = 100 40 16

N = 250 23 10

N = 500 20 8

N = 1000 15 6

N = 2500 13 5

N = 5000 11 4

Table 2: Percentage of SIR steps: linear Gaussian model

33

Page 42: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

√V AR

(xk|k

)bootstrap prior dist. linearised dist.

N = 100 5.67 6.01 5.54

N = 250 5.32 5.65 5.46

N = 500 5.27 5.59 5.23

N = 1000 5.11 5.36 5.05

N = 2500 5.09 5.14 5.02

N = 5000 5.04 5.07 5.01

Table 3: MC filters: nonlinear time series

34

Page 43: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Percentage SIR prior dist. linearised dist.

N = 100 22.4 8.9

N = 250 19.6 7.5

N = 500 17.7 6.5

N = 1000 15.6 5.9

N = 2500 13.9 5.2

N = 5000 12.3 5.3

Table 4: Percentage of SIR steps: nonlinear time series

35

Page 44: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

SEQUENTIAL MONTE CARLO SIMULATION OF DYNAMICAL MODELS WITH SLOWLYVARYING PARAMETERS: AN EXTENSION

William Fong and Simon Godsill

Signal Processing Group, University of Cambridge,Cambridge, CB2 1PZ, U.K.

[email protected] [email protected]

ABSTRACT

In this paper, we improve on the slow time-varying par-tial correlation (STV-PARCOR) model recently suggestedby us [2] to include any deterministic interpolator. We thensuggest a modification to the on-line filtering algorithm toaccomodate the changes. It is believed that the modificationwill improve on the simulation results as it takes into ac-count the underlying trend of the parameter evolution. Thesuggested algorithm is tested with real speech data and pre-liminary results are shown and compared with those gener-ated using existing approaches.

1. INTRODUCTION

Many real world data analysis problems involve sequentialestimation of filtering distribution ����������, where �� isthe unobserved state of the system at time � and ���� �

���� � � � � ��� are observations made over some time inter-val � � ��� � � � � ��. In most cases, the data structures can bevery complex, typically involving elements of non-Gaussianity,high-dimensionality and non-linearity, which may not besolvable analytically. Sequential Monte Carlo methods, alsoknown as Particle Filters (PF), have been proposed to over-come these problems. Refer to [1] for an up-to-date surveyof the field. Within the particle filter framework, the filteringdistribution is approximated with an empirical distributionformed from point masses, or particles,

���������� ������

����� �����

���� ��

�����

����� � �� �

���� � �

where ��� is the Dirac delta function and ����� is a weight

attached to particle ����� .

In recent years, various approaches have been developedto apply the sequential Monte Carlo filtering strategies forthe purpose of audio signal enhancement (see, for exam-ple, [3] and citations therein). These approaches assumea Gaussian random walk model for the system parameter

evolution at every time step, which may not give a suf-ficiently slow or smooth variation with time. This makesthe standard PF inefficient as it is known that the filter be-comes highly degenerate for random walks with very lowvariance. Recently, Fong and Godsill [2] propose a slowtime-varying partial correlation (STV-PARCOR) model tosolve this problem. In their work, the system coefficients areconsidered to evolve stochastically on a block-to-block ba-sis and all coefficients in-between are found by linear inter-polator. Based on the STV-PARCOR model and under theparticle filtering framework, an algorithm for on-line jointestimation of system parameters signal is developed.

This work serves as an extension to the work of [2]. Inthis paper, we generalised the STV-PARCOR particle fil-ter, so that any deterministic interpolator can be used. Wethen describe the modified algorithm for the generation of“delayed” state realisations. Finally, preliminary simulationresults are shown.

2. STATE-SPACE REPRESENTATION AND AUDIOMODEL

In this section, we describe the model adopted in this paper— the STV-PARCOR model. A lengthy time series is di-vided into non-overlapping blocks. If is the block size,we define ����� � ������� � � � � ������ as a group of unob-served states of the system and ����� � ������� � � � � ������as observations made over some blocks � � ��� � � � � ���.

Assuming a Markovian structure for the model, the prob-lem can then be formulated in a state-space form as follows,

����� � ���������� � State evolution density����� � �������������� Observation density

(1)where ����� and ������ are pre-specified state evolution andobservation densities. It should be noted that the state-spacemodel adopted here is different from the standard one [5],which relates the unobserved states ���� and observations���� made over a time interval � � ��� � � � � ��. (1), how-ever, defines the state evolution between different blocks.

Page 45: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

For the choice of audio model, we suggest a time vary-ing partial correlation (TV-PARCOR) model. The advan-tage of adopting such model is that approximate stabilitycan easily be enforced, provided that the PARCOR coeffi-cients ���� vary sufficient slowly with time [3]. The audiosignal process ���� is then modelled as

�� �

������

�������� � ��

where �� is the TVAR coefficient at time �, which is foundby transforming the PARCOR coefficient �� via the Levinson-Durbin recursion. ��� is the log-excitation variance and ��is the time-varying model order. Refer to [3] for a detaileddescription of the audio model adopted here.

The full specification of the state-space model is as fol-lows: at any time �, the state vector �� is partitioned as���� ���

� with �� � ��������� � � � � ���� and �� � ���� ��� � ���

being the signal state and the parameter state respectively.In the setup of the particle filter, a proposal distribu-

tion [1, 3] similar to that of [2] has been adopted, whichtakes the form:

���� ������� � ������������� �������������

����� ���������� (2)

As in [2], the block variation of the log-excitation varianceand model order take the form,

����� ���������� � ���������

� ���� (3)

������������� ����

���

����� � �������� � ��

�(4)

where ���� � ��� � ����� � ���� for � � ��������� � � � � ��,i.e. both ��� and �� are assumed to be fixed within a block.

For the PARCOR coefficients, a constrained random walkmodel [3] is assumed for the block variation,

������������� �

� �������� � ����� if �

���������� � �

� otherwise

A constrained random walk model of this form will en-sure approximate stability provided that the PARCOR co-efficients vary sufficiently slowly. Having sampled the lastPARCOR coefficient of the block � , ���, all the interme-diate PARCOR coefficients are found by some determinis-tic methods using previously sampled PARCOR coefficients���������� � �� � � � � � � ��,

�� � ������� ����� � � � � ���� (5)

where � � ��� � � � �� � �� � � � � ��. For the interpo-lator functions, ��, the Legendre polynomials, Fourier basisfunction and B-splines are popular choices, all of which will

ensure a slow and smooth evolution of the reflection coef-ficients [4]. In [2], we have implemented a linear interpo-lator, which should be considered as a special case of (5).We believe that (5) will give a better approximation than thesimplified linear interpolator case as it takes into account thebetter underlying smooth trend of the coefficient evolution.

3. SEQUENTIAL AUDIO SIGNAL ANDPARAMETER ESTIMATION

Based on the STV-PARCOR model, [2] describes a way forjoint estimation of the signal and parameter state under thesequential Monte Carlo filter framework. The suggested al-gorithm proceeds as follows:

Random samples are to be drawn from the joint filteringdistribution ����� � ��� ������ � which can be factorised as fol-lows,

����� � ��� ������ � � ����� ���� � ����� ������ ������ � (6)

������ ������ � ����� �������� ������ ���������

Assume there exists a particulate approximation for the marginalparameter filtering distribution,

������� ������ � ������

����� ������ � ��

������ � (7)

as it is assumed that the proposal distribution for the pa-rameter state being the prior (2), the importance weight willsimply take the form,

����� �

�������������

������������� ������� (8)

with ������ � ���������� � � � � �� are parameter states re-cently updated using the deterministic interpolator (5). Thejoint filtering distribution (6) can then be approximated by

����� � ��� ������ � �

������ ������ � ����� �

�����

����� ���

������ �

����� ���������

������

����� ��������� � ����� ��

���� ������� � ��� �

Hence, given ������� , signal realisations can be drawn from

������ � ����� ��������� � ����� �

For instance, if we assume a conditional Gaussian state-space model, then all the computations can be done under

Page 46: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

the framework of the Kalman filter and smoother [3]. e.g.for � � ��� � �� � �� � � � � ��, ������

������� ������� from (8)

can be found by the prediction error decomposition [5] andthe marginal signal filtering distribution can be rewritten as:

����� ��������� � ����� � �

�������������

������������������� �������� ������

�������������

���� ��������� �

�������� (9)

with �������� � ���������� �

������� and �

������� � ��������

�������� ���

��������������� �

������� are sufficient statistics found by the Kalman

smoother.

4. IMPLEMENTATION

We modify the STV-PARCOR particle filter suggested in [2]to facilitate the generalised STV-PARCOR model. Let � �

� � � � �, assuming that the parameter realisations ���������

� � � � ����������� and the signal sufficient statistics (� ���

�������

and �����������

) are available for � � �� � � � � !� from theprevious iteration of the filter, state random samples can bedrawn from the filtering distribution as follows:For � �� � � � � ! ,

� Generate random sample from ������ � ������

����������

and ������� � ����� ��

����������

�. For each ������, sample

������ from �������

����������, where

����������� �

�������

�����������

�if �

����� " �

���������

����

�����������������

��otherwise

� Make each of �������� � � � � � ������� the same size by ap-

pending 0 to the vector if necessary. Using these fixedgrids, ������ � � � � � �� � � #� for # being inte-ger are found by deterministic interpolator (5). In oursimulations, we have implemented the cubic splinewith � � �, i.e. for $ � �� � � � � �

�����,

������� �

�����

%������

���� ��

where %�� is the spline coefficient to be determinedand ����� is the ��� order B-spline basis function.

� Given the sufficient statistics (� ����������

and �����������

)

and ������ � � � � � ��, we run a forward sweep

of the Kalman filter over blocks � � � to � . We thenevaluate the importance weight �

���� according to (8).

Having generated the parameter set ������� � � �� � � � � !�,we then resample (see [1] for details) it ! times with re-placement according to �

���� . For the resampled parame-

ter set ������� � � �� � � � � !�, we run a backward sweep of

the Kalman smoother and generate �� �������� �

�������� � � � �

�� � � � � ��.

Theoretically, signal realisations �� ���� � � �� � � � � !� � �� � �� � � � � �� can be generated according to (9). How-

ever, as ������ � �� � �� � � � �� � � #� are go-ing to change in the next iteration, only �� �� � � �� ��� � � � � �� � ��� will be drawn. Hence, the suggested al-gorithm will only give “delayed” state realisations.

In addition, continuity can be ensured by taking into ac-count �

������ in the sampling of �

���� , as there is only one free

variable owing to overlapping betweem ������� and �

���� .

5. EXPERIMENTAL RESULTS

Experiments are conducted to investigate the effectivenessof the suggested algorithm (STV-PARCOR PF) for the pur-pose of audio noise reduction. In particular, we would liketo verify our suggestion that the generalised STV-PARCORmodel is a better model for slow time-varying processes(e.g. speech) then the TVAR model. Preliminary simula-tion results are shown and compared with those generatedusing the standard extended Kalman smoother (eKS) [5].The clean speech clips used in this experiment are:

S1: Good service should be rewarded by big tipsS2: Draw every outer line first, then fill in the interior

The experiment setup is as follows, the clean speech sig-nal is assumed to be submerged in white Gaussian noise(WGN) with known variance ��

� , i.e. �������� � ������� ����.

The output SNR from different algorithms are recorded andcompared. Owing to the stochastic nature of the MonteCarlo algorithm, simulation results for the STV-PARCORPF are found by averaging the SNR improvement over fiveindependent applications of the algorithm.

In our simulations, we have chosen a value of ! � ��,the block size is fixed to 100 for the STV-PARCOR PF.The hyperparameters (��

�, ��� and ��� � � ��� �����)adopted here are assumed to be known and fixed. In con-sideration of the computational cost, it is assumed that themodel order �� � ��� � � � � ��� with �� being limitedto 20. We note that ! � �� is extremely small for MonteCarlo simulation but the preliminary simulation results sug-gest that the algorithm works pretty well in such a case. Asin other applications of the Particle Filter, simulation resultsimprove as ! increases.

Page 47: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

For the eKS, a Gaussian random walk is assumed di-rectly on the AR coefficients and the model order is fixedto 10. This model is employed as the TV-PARCOR modeland time-varying model order is not straight forward to im-plement with the extended Kalman smoother. The hyperpa-rameters adopted are adjusted so that the system parameterswill cover the same range as the generalised STV-PARCORmodel in time steps. We note that this may not be theoptimum setup for the eKS, however, this will give a faircomparison for both algorithms.

Figure 1 and Figure 2 show the 3D histogram plots ofthe first reflection coefficients (����) at different input SNRs(SNR��) for the word “‘reward” in S1 using the STV-PARCORPF. The plots are generated by grouping all PARCOR co-efficients particles from five independent simulations. Asshown in the plots, the suggested algorithm gives consistentresults at different noise levels.

We then compare the performance of the suggested al-gorithm with the the eKS. The SNR improvements for dif-ferent clips at different noise levels are summarised below:

Clip SNR�� STV-PARCOR PF eKSS1 0dB 3.86dB 1.92dBS1 10dB 2.54dB 0.99dBS1 20dB 1.08dB 0.87dBS2 0dB 4.31dB 2.21dBS2 10dB 2.80dB 1.57dBS2 20dB 1.35dB 1.09dB

Audio outputs can be found at http://www-sigproc.eng.cam.ac.uk/�wnwf2/Eusipco2002.html. Comparing the SNRimprovements, the suggested STV-PARCOR PF consistentlyoutperforms the eKS, which justifies using it in practice,even it induces a much heavier computational load whencompare with the eKS.

6. CONCLUSION

We propose a generalisation to the STV-PARCOR modelrecently suggested by us to include any deterministic inter-polator functions, ��. We then describe an adaptation tothe algorithm for joint estimation for signal and parameter.The algorithm is tested on real speech signals and comparedwith other standard approaches. Encouraging results are ob-tained. Further simulations will be conducted to investigatethe effects of different interpolator functions and differentlags, �. The results will be published in due course.

7. REFERENCES

[1] A. Doucet, N. de Freitas, and N. J. Gordon, editors. Se-quential Monte Carlo Methods in Practice. New York:Springer-Verlag, 2001.

[2] W. Fong and S. Godsill. Sequential Monte Carlo simu-lation of dynamical models with slowly varying param-eters: Application to audio. In Proceedings of the IEEEICASSP, 2002. To appear.

[3] W. Fong, S. J. Godsill, A. Doucet, and M. West. MonteCarlo smoothing with application to audio signal en-hancement. IEEE Transactions on Signal Processing,Special Issue, 50(2):438–449, February 2002.

[4] Y. Grenier. Time-dependent ARMA modeling of non-stationary signals. IEEE Transactions on Acoustics,Speech and Signal Processing, 31(4):899–911, 1983.

[5] A. C. Harvey. Forecasting, structural time series mod-els and the Kalman filter. Cambridge University Press,1989.

20

40

60

80

100

120

140

160

180

200

0

500

1000

1500

2000

2500

3000

3500

−0.98

−0.96

−0.94

−0.92

−0.9

−0.88

−0.86

−0.84

−0.82

0

100

200

tρt

Figure 1: 3D histogram plot of ���� for the word “reward”using the STV-PARCOR particle filter for SNR��=0dB

20

40

60

80

100

120

140

160

180

200

220

0

500

1000

1500

2000

2500

3000

3500

−0.98

−0.96

−0.94

−0.92

−0.9

−0.88

−0.86

−0.84

−0.82

0

100

200

tρt

Figure 2: 3D histogram plot of ���� for the word “reward”using the STV-PARCOR particle filter for SNR��=20dB

Page 48: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

������������� ����� ������������� ��� ���������! #"$�%�'&)(*����+, -.��(� /����+0

132/465�7/8:9;2=<>4/?A@�B34DCFEHGDI$4KJ

L�M�NPORQTS3UVO

WYXVZR[\ZR]_^$`>ab]_^$cdZRegfV]ih�abfj]:k\lnmo[\X�p!fV[rqnZHsg^$]i`of�cdZ qit3f>u3l�mv[�u>wx[\^Vcdmvyzlnw>l{qiZRc|l�m}l�~3]ifT�xmvu3ZPud^$[\u

moqil�ezm}u>Z�^$~\~3`om}yR^$qnmvfV[\l�mo[\u3mvyP^TqnZ�uD����[\u3ZR]�qit3m}l�ab]_^$cdZRegfV]ih��VlnZR�VZP]i^V`3y k3]i]iZR[rqn`vw�^��T^$mv`}^$�3`vZ�qnZ�y_t3[3m}�jk\ZPl

^V]nZdl�qnk\u>mvZPu�^$[\u�XVZP[3ZR]_^$`vmv�RZPu!qnf�^VyRyRfVcdcdf>u3^TqiZ%cdfV]iZyRfVcd~3`vZ ��abZP^$qnk3]iZPlP�%W�`o`�fVa�qnt3Z�l�ZdcdZ qit3f>u3l

^V]nZg~\^V]�qimv^V`xyRfVc��3mo[�^TqnmvfV[�l�fVa\qit3]nZPZ�mo[3Xj]nZ�u>mvZR[rqilP��mvc~�fV]nqi^V[\y Zzli^$cd~3`vmo[\X�^V[\u%]iZPli^$cd~3`vmv[3X\�T]iZ{�{Z�y�qnmvfV[

li^$cd~3`vmv[3X\��^$[\u!p�^V]nhjfT��y_t\^Vmo[�moqnZR]_^Tqimofj[\lP���=Z�u>ZP`omv�VZP]�^dXVk\mvu>ZP`omv[3Z�fj[!t3fTe�qnt3ZPw�lnt3fjk3`vu���Z%k\lnZPu

^V[\u�k3[\u>ZP]zezt\^Tq�yRmo]_y k3c|l�qi^V[\y ZHZP^jy_t*cdZRqnt3f>u�mvl�cdfrl{q�lnk3m�q_^$�3`vZV����t\]nfjk3XVt*qnt3Z�^V[\^$`vw>l�m}l�f$a�u>m���ZR]n�

ZP[\y Z�lz^$[\u�y fj[3[3Z�y�qnmvfV[�lR�xegZHyRfV[\lnfV`vm}u3^TqiZ�qnt\ZPlnZ�cdZRqnt3f>u3lzmv[rqnfd^XVZR[\ZR]imvy�^$`vXVfj]nmoqnt3c��xw�yRfVc%�\mo[3mv[3X

u3ZPlnmo]_^$�3`vZ�abZ�^Tqik3]nZ�lR�� ¡[�^Vu3u>moqnmvfV[/�regZ�~3]nfj~�frl�Z¢^%XjZR[3ZP]i^V`\k\lnZ¢fVa�£�^Vf$�¡¤�`}^Vy_hxegZR`v`omv�P^$qnmvfV[�qnfmocd~3]ifT�VZ

~�ZR]nabfV]ic|^$[\yRZPlP�|¥��3^$cd~3`vZPl�ab]ifVc¦Z�y fj[3fVcdZ qi]nm}yRl�^V[\u�ZR[3Xjmo[3ZPZR]imo[\X�^V]nZd~3]iZPlnZR[rqiZPu§qnf�u>ZPcfj[\l�qn]_^TqnZ

qit3Z�mvc~�fV]nqi^V[\y ZHfVa�£�^Vf$�¡¤g`v^jy_hregZR`v`vmo��^TqnmvfV[�^V[\u*qnf|y fjcd~\^$]iZ�u>m���ZR]iZR[rq�p!fj[jqiZ�sg^$]i`of~3]if>y ZPu3k3]nZ�lR�

¨ ZPwxe�fj]iu3l �/¤g`omv[\uu>ZPyRfV[x�Vfj`ok>qimofj[�©T¤gfrfVqil�qn]_^$~�ª�`�qiZR]�©j«�mo�\�\l�ln^Vcd~3`omv[3X�©$¬�m}u3u>ZR[dp�^V]nhjfT�¢cdf>u>ZR`­©

¨ ^V`oc|^V[dª\`�qiZR]�©rp§^$]ihVfT�%y_t\^Vmo[�p�fV[rqnZ¢sg^V]n`vf\©r®�^$]nqnm}y `vZ�ª\`�qiZR]�©3¯xZP�rk3ZP[jqimv^V`3mvc~\k>qi^$qnmvfV[�©3¯rq_^TqnZ�l�~�^Vy Z

cdf>u>ZP`°©\��^$]iXVZRq�qn]_^Vy_hxmo[\X\�±{²P³r´=µ3¶/·�¸¹³§¸¹º�»P´�»Pº¡º¡¸¹º½¼�»P´T¼�¾V¿{ÀPÁ}Âiº¡º¡ÀP¿%ÀPÁ�µ$¼�» ¼{¸�º½¼{¸¹Ã_º_Ä�Å�Â_¾x» ¿¡¼{Æ�Â_´T¼�ÀRÁzµT¼�»R¼{¸¹º½¼{¸¹Ã_º_Ä:µT¼�»P´jÁvÀP¿�Ç�Èg´j¸�É�Âi¿{º¡¸Ê¼­ËTÄ�µ$¼�»R´jÁ}ÀR¿�Ç\Ä

Ì�ÍÏÎPÐ�ÑPÒ�Ó ¶�Ô:À�´jÕ Ì�Ö Â_´*¸¹º�»R´�»Rº¡º¡ÀTÃ_¸�»R¼{Â�¾j¿{ÀRÁ}Â_º¡º¡ÀR¿�ÀPÁ�µT¼�»R¼{¸¹º½¼{¸¹Ã_º_Ä�Å�Â_¾x» ¿¡¼{Æ�Â_´T¼gÀRÁ�µT¼�»R¼{¸¹º½¼{¸¹Ã_º_Ä�×�ÂiØj»Pº Í�Ù�Ú Èg´j¸�É�Âi¿{º¡¸Ê¼­ËTÄ

Ì À�Û¹Û¹Â_ÕPÂ�µT¼�»R¼{¸¹À�´\Äj×�ÜÞÝ�ÝRß Ð�Ñ ¶�·�¸¹³�à º�¿{Â_º¡Â�» ¿{Ã Ö ¸�º�¾r»R¿¡¼{ÛÊË%º¡³r¾j¾>ÀP¿¡¼{Â�Ç�áTË�âzµjã*ÕP¿�»P´T¼{º�Å Ú µ Î�Ó�ä°ÒVå Ó Ý Ò »R´xÇ�Å Ú µ Î�Ó ä°Î�æPÒ�Î�æ Ä

»P´rÇ*¼ Ö Â�×�Âi¿{Æ�»P´�ÁvÂ_Û¹Û¹À ç�º Ö ¸¹¾�Áo¿{À�Æ#µ$¼�»R´jÁvÀP¿�Ç�Èg´j¸¹É�Âi¿{º¡¸Ê¼­ËT¶ Ì�Ö Âi´�à º�¿{Âiº¡Â�»R¿{Ã Ö ¸¹º¢¾r»R¿¡¼{ÛÊË�º¡³j¾r¾>ÀP¿¡¼{Â_Ç�áTË*â�µjãèÕP¿�»R´T¼¢Å Ú µ

Î�æ ä­éPæVåPå�Ñ ¶dê�Â�» ¿{Â�ÕP¿�»R¼{ÂnÁ}³rÛ/¼{À|ëD¿{ÀPÁ}Âiº¡º¡ÀP¿Hê踹´jÕdìg³j´rÕ�ê�À�´jÕÁ}ÀR¿�º½¼{¸¹Æ�³jÛ�»R¼{¸¹´rÕ|ÇV¸¹º¡Ã_³rº¡º¡¸¹À�´jº�¼ Ö »R¼ Ö ÂiÛ�¾>Â_Ç*¼{ÀÁ}ÀR¿{Æ�³jÛo» ¼{Â

¼ Ö Â�ÕPÂ_´rÂn¿�»PÛ�µ$í¡µ�Áo¿�»PÆ�Âiç�ÀP¿{î>Ä�¼{À�ëD¿{ÀPÁ}Âiº¡º¡ÀP¿{º�ê�»PÛ¹ÛÊË�ïg¸¹Û¹îVº¢»P´rÇ�âgÂ_¸¹Û�µ Ö Â_¾ Ö »R¿�Ç*ÁvÀP¿¢Û�Ân¼¡¼{¸�´jÕ�³rºz¿{Â�»�Ç�¼ Ö Â_¸Ê¿�Âi´rÛ¹¸¹Õ Ö ¼{Â_´r¸¹´jÕ

Æ�»P´$³jº¡Ãi¿{¸¹¾j¼�á>ÂnÁ}ÀP¿{Âg¾r³rájÛ¹¸�Ã_»R¼{¸¹À�´\ÄV¼{À¢ëD¿{ÀPÁ}Âiº¡º¡ÀP¿{º:²�À Ö ´�Ô:¸¹Ã_Â�»R´xÇ Ú ¸¹î�Â�ê�Â_º½¼�Á}ÀR¿�¾>À�¸¹´T¼{¸¹´rÕ�À�³j¼�¿{Â_Û�»R¼{Â�Ç�¿{ÂiÁ}Ân¿{Â_´rÃ_Âiº_Äx»R´xÇ�¼{À

¼ Ö Â�»Pº¡º¡ÀTÃ_¸�»R¼{Â�Â�ÇV¸¹¼{ÀR¿�»P´xÇ%¿{ÂiÁ}Âi¿{ÂiÂ_º:ÁvÀP¿:Æ�»P´TË�Ã_ÀP´rº½¼¡¿{³jÃi¼{¸¹É�Âzº¡³rÕPÕ�Â_º½¼{¸¹À�´jº_¶

Page 49: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

� �������� &� � ����� �

����4/<���9����!B�?DI��b9b4�C 9��è<>4#9�����B���� <>4���� � <!� 9�� � 9��T<���<>4�<��"�#� 9��$��B�B%��<>4�? G�<��6<!� � �R<��&��I$? ��2'�PG <!�)(

��I$4�� 9 B34+*,� B%�-� I���I$<!�.�RGDI/�0��9b4�?�921�I/� I$4��43/I��b?5�T776d4DI8�!B%� �:9%9b?DI����K2'� I$?�?���4�<���9;�<�!B�?DI��>=?� GDI$�b9b4DI$<!�

� � <!��I7� �/<���I@�!B�?DI��>=�G�<��<� B34�C�A�ITI$4 <>4 <��&� 9"B>IC� 2'A#D_I��&��9;4E� � 2�?���9b4DCF� 9��!IG� I/�R9 I���?�<!� <Ï<>4�?E��B34H� �RB%�

� �#� ��I��<��IKJ<!��B>I/��=ML�N�O�NQPSRÞI�� �!<>4�?+J�<!���P9���B34T=SL�N�O�N%UP7V�I�� �/9"��I8� GDI$9"�W��B%���/2�� <!� 9bB34�<��X��B%�<�5� I&YD9"� 9 I��/=

4DB34'�;9b4DI$<!�[Zr4DB34�(]\d<>2'��� 9;<>4^� � <!��I_� �/<���IM�!B�?DI�����<!� I�<�����B�9�����B���� <>4��z9b4:Br<!�P9 B32'�z<!�'�5�;9��T<!� 9 B34'�T7a`b��<!��� 9b<��

�b9�����B�*?� I/*­I/� I$4'��I��%9���C39"B>I$4A9;4�cdYD<����5� Ife:A�I�� Bg9�7

h�B�?DI��;��B�*H?���4�<���9���4�<!� 2��RI�G/<�B>I�<�����B$A�ITI$4Þ2'��I$? 9b47Bx<!�R9 B32'�B#�/�T<�� 9 B34'��=i� 2'�PGÞ<��|2���?�<!� 9b4DCF<>4�?

� I$<!�R4/9b4DC�9b4dC��R<!�/G/9��T<��%�!B�?DI�����B��j� GDI?kml nHo�p�o&qsrtq,u[vKq>w_x0y[k�x/l[v�u[zgu&v{x&|:u}I{5Q�/9 ITC>I��;G�<��"��I/��<>4�?�8�<>2��R9"� ~TI$4�L�N�N!�#=

�|B34DC�=�8�9b2 <>4�?7RÞB34DCVL�N�N���U.=?� 9���2'�b<!� 9b4DC��'� B���I$9b4F� � �P2'�&� 2�� I���I½8�I$<��PG�L�N�N��QPa��<�����2�I/~�<>4�?Ï5#�RG�I/�R<xC>B

L�N�O��%U.=�C>I$4DI/� 9��/��Is�]��9%9b4DC�=�EHB�Y�<>4/?��|B34DC�L�N�N���U.=�<>4/?:��B%�^A/9b4/<!��B��R9b<���B��'� 9���9�~$<!� 9 B34'��IsRÞB34DC�<>4�?!8�9b<>4DC

L�N�N%��UP7S`�4èI&YD<����5�bI|B�*�I&Y#��I/����� �#� ��I�� 2���?�<!� 9;4DC:�T<>4@A�I�*°B32�4/?è9b4���I/��~$2�9b4�9�I/��<��¡7MI)L�N�N%��UP7

�_4W� G�9��z<!��� 9��/�bI�=%9¢I���� 2�?��Wh6B34���IEH<!�0�bB���B%�<�/2�� <!� 9 B34<��I/� GDB�?5�d*°B���l�x�p�rmvKqs|<x�<>4�<��"�#� 9���B�*�?���4�<���9��

� �#� ��I��<�T7H5�2'�RGè<�� �Q����I����T<>4�A�I�<!A5� � �P<��&���"�è?DI&3�4DI$?è<���*­B%��� Bg9M�/�

�����?�a�,�g�s�m� ����u�x����'x/ mw.x$n ¡8x&¢!n!rt¢gq, Q£�kml)n�o�pHo/q,r¤qsvKz¦¥�q;u&vKl.q§o&��vKq>n! �u^¨5©0IKª?©«U.¬Xqs m¥Hx0y�x�¥­o&z7¥�q;u/w/l�x/v{x$vKqs|�x

®�¯ �#¬�L!¬0eQ¬/°/°/°,±_q,u�w0p!rsr"x0¥Fp�k�l)nHo0pHo&qsr¤q;u&vKqKw�²j³i�j´5µ­�s¶­·/³¸·/�¹�#µ»º½¼i¾#x<u[v§p%v{x�¢�p!l[qKp�o&r�x:ª?©:w0p� �x/¢�n!r¤¢!x�qs 

vs¾#x¿¡/n!rsr2n�Àdq, �£Gvs¾Hl x.x:À}p!z�u&Á�§qtÃ:Ä. mw/l�x�p!u.qs �£@¥!qs|�x& �u[qKn� mÁ�ª?©;Å J ¾Qp�u:n! Æx:|<n!l x<w0n!|_k#n! Æx& 5v�vs¾#p! Gª?©)±}q>ºsx¹º

ª ©;Å J ¯ IKª © ¬ Ç ©;Å J U0±ÈÀ?¾#x&l�x�Ç ©;Å J w0p! ¦o.x^p�|W�#r¤vKq>¥!qs|<x/ �u.q>n! mp�r?w.n!|_k#n! �x/ 5vsÉ�Â{q,qtÃ^Êfq;u/w&¾Qp!l>£�qs �£�ÁSª ©;Å J ¾Qp�u

n! �xX¡�x&ÀXx/l^w0n�|Mk�n! �x& mv�vs¾Qp! �ª © ±ÈqKºKx�ºSª © ¯ IKª ©;Å J ¬�Ë © U0±¿p! �¥CÂ{q,qsqtÃ:Ìfn8w&¾Qp� �£�x¹ÁSª ©;Å J ¯ ª © º

h�B%� ��B�*�� G/9��§<!��� 9;�/� I@9%9;����A�I6?DI/B>B���I$?���B½� 9"� 2�<!� 9 B34ÍI½9,U.=�9%GDI/�RI$<���� 9�� 2�<!� 9 B34'�7I½9b9,U�<>4�?I½9b9b9sU:�T<>4»A�I

G�<>4�?5� I$?�� 9���9;�b<!�0�"�>7�Î�G��RB32DC3GDB32���� GDI§<!�0� 9��/� I�=j¨ÈI_Ud<��"9H<��#�f� I/*­I/�0����BC� GDI�� <!� C>I/��?�9�� � �P9"A/2�� 9 B34�B�*S� GDI

?���4�<���9���� �#� ��I���=�<>4�?WÏaIÈUH9;��<!C>I$4DI/�R9������Q�^A�B%�Æ*°B��_�'� B�A/<!A/9;�b9"�{��?/9�� � �R9"A�2�� 9 B34'�T7

L

Page 50: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

�_4f�!B%� ��<!�'�5�b9��T<!� 9bB34'�/=¹� G�I�?�921�I/� I$4'��IÈA�I/�«9¢ITI$4:¨'©;Å J <>4/?f¨'©�9��¸�T<>2'��I$?^AQ��� G�I�9b4'��B�����B��R<!� 9bB34|B�*D4DI/9

9b4�*­B��0��<!� 9bB34*9b4^� GDIH<>4�<��"�#� 9��T7d6�*/9b4���I/� I�� ����9b4^� GDI���I�� �#� ��I��<��<!� I�2'�R2�<����"�<I½<HUi�'� I$?�9;�&� 9 B34T�i¨5© IsÇm©;Å J � ª?©«U

I½9¡7ÊI>7"=�9%G�I$48¨ © �T<>4$A�IdI&Y#��I$4�?DI$?$��B!<!4DI/9 ��B%����B34DI$4��SÇ ©;Å J =#� GDI�A�I�� �¿�'� I$?�9;�&� 9 B34=B�*iÇ ©;Å J o.xK¡/n!l x�4DI/9

9b4�*­B��0��<!� 9bB346<!���R9�B>I���9��MB�9;<�¨ © U.PÈIsA�Ui2���?�<!� 9;4DCCIK���!B�B�� G�9b4�C�U.�S¨ ©;Å J IKª © U4I½9¡7ÊI>7"=Æ� GDI^� I/B�9�� 9 B346B�*d�'� I/B�9 B32'�

� � <!��I�C39"B>I$4K4DI/9#9;4�*°B��.��<!� 9 B34mU.P�<>4�?+IK�¹U%4DI/9 I�� � 9���<!� 9bB34T��¨'©;Å J IsÇ5©;Å J U^I½9¡7ÊI>7"=�9%G�<!�M9�IW�T<>4C� <¹�6<!A�B32��

Ç5©;Å J 9;4V�b9 C3G��|B�*�4DI/9 9b4�*­B��0��<!� 9bB34mUP7<Î�GDI<*­B%��� Bg9%9b4DC$�«9¢B�I&Y�<����m� I���<!� I<�«����9��T<���?���4�<���9;�T<��d� �Q����I��<�

<>4�?�� G�I/��9%9;����A�I��RI/*°I/���RI$?$��B<� I/��I$<!��I$?'�"�8� G�� B32DC3G�B32���� G�9���<!��� 9;�/� I>7

��� ´5µ����s�E����p!zHx[u[qKp� ¦|Wq;u.u.qs �£7¥%p�v§p�kml)n�o&r�x&|�7�5�2��5��B%� I � J ¬/°/°/°¹¬����6<!� I!9b9;?G*s� B%� �!B�?�I��'ÏaI�� ��� U.=

A/2�����B%��I��A<!� I!B34'�"�@�/<!��� 9b<������AB�A5��I/��B>I$?�7�8:I/����� ¯ I����)¬ Ç��§U_9%GDI/� I����¢9;��� GDI!B�A5��I/��B>I$?F��<!����<>4�?FÇ��

� GDI<��9;��� 9b4DC��/<!���T7!8�I/��� © ¯ I�� J ¬/°/°/°¹¬���©{U<>4�?�ª?© ¯ IsÇ��!¬ Ç J ¬/°/°/°¹¬ Çm©«U.=¸9%GDI/�RI:Ç�� ¯ � 7WÎ�GDI!?���4�<���9��

� �#� ��I�� 9b4�� G/9����T<���IÞ9��8¨'©�IKª?©«U ¯ ÏaIKª?© � � © UP7 6�*�9b4���I/� I�� �=9��=2'� 2�<������­� GDI7��B%� ��I/�R9 B��F?�9���� �R9"A/2�� 9bB34

¨ � IsÇ �¹U ¯"! ¨ � IKª � U]Ë%Ç J$#%#%# Ë%Ç � 7?R�G�I$4 � I½9{7ÊI>7"=%Ç �¹Uj�T<>4�A�I�I&YQ�5�;9��T9"���"��9b4���ITC��R<!��I$?!B32'�a*s� B%��ÏaIKª?© ¬�� © U ¯

ÏaI�� J ¬/°/°/°¹¬���©)¬ Ç J ¬/°/°/°¹¬ Ç5© �&� U;Ï?I � U.=¿�R2'�RG <���9b4½� GDIG�T<���I�B�*��*2'��� 9"Br<!�P9b<!��Iè4DB��0��<���?/<!� <�9%9"� GE��9;��� 9b4DC

��B%����B34DI$4����fIK�|B34DC!I/��<��¡7�L�N�N���U.=�<!C>B�B�?è<!�'�'� B3<��PGA9��S��B§?��P<�9 Ç J ¬/°/°/°¹¬ Ç � *,� B%� ¨ � IsÇ J ¬/°/°/°g¬ Ç � UH<>4�?

� GDI$4A25��I�@%<xB!({�S�b<��('�9�I����b9"~$<!� 9bB34G��B§<!�'�5� B�YD9���<!��If¨���I � UP7

��� ´5µ����s�*)+� ¼i¾#x-,�v§p�v{x.,�k#pHw.x-/¦n�¥Hx/ro7�5�25�RGÏ<C�!B�?DI��È��B34'� 9;� ����B�*��«9¢B7�/<!�����/��I)L¹UdB�Am��I/��Bx<!� 9 B34

I���2�<!� 9 B34T=a9%G�9��PGV�T<>4­A�I�*°B��0��2'�b<!��I$?�<��0��©2143�©�I # � Ç5© ¬(5TU.P�<>4�?»I>e%Uf� � <!��I=I���2/<!� 9 B34T=a9%G�9��PGV�T<>4¦A�I

� I/�'�RI���I$4H��I$?CAQ�A<�hK<!��'>B¹BC�'� B#��I�����<��_Ç © 176 © I # � Ç ©�8 J ¬ � UP7�Î�GDI � © <!� I�B�A5��I/��Bx<!� 9 B34'��<>4�?@� G�IfÇ © <!� I

� I/*­I/��� I$?@��B�<���� G�I8I½2�4DB�Am��I/��B>I$?mUS��� <!��I��T7M6�*z9b4H��I/�RI�� �%<!��<>4���� 9��!I ® 9;��� GDIf��B%� ��I/�P9 B��%?�9���� �R9"A/2�� 9bB34�B�*

ª?©:9 I;5j¬ � ¬ Ç J ¬/°/°/°�¬ Ç5©«UP7}J�I$4'��I^� GDI�� <!� C>I/��?/9�� � �R9"A�2�� 9 B34=<!��� 9��!I ® 9��

¨'©�IKª?©«U ¯ ¨'©�I;5j¬ � ¬ Ç J ¬/°/°/°g¬ Ç5©«U ¯ ÏaI;5j¬ � ¬ Ç J ¬/°/°/°�¬ Ç5© � � © U&<­ÏaI � ¬(5ÆU©=>@?�J

3 > I�� > � Ç > ¬(5ÆUA6 > IsÇ > � Ç > 8 J ¬ � U.¬

9%GDI/�RI�� GDI§9b4�9�� 9b<���?�9�� � �R9�A/2�� 9 B34B6 J IsÇ J � Ç ��¬ � U|9���<���� 25�!I$?C'�4DB¹9%4�7<R�G�I$4V� GDI<��<!�R<��!I/��I/�0� � <>4�?D5

e

Page 51: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

<!� I�C39�B>I$4CIK� 2'�PG§<���9b4���<>4���I$4DC39b4DITI/�R9;4DC��'� B�A5�bI��<�.U.=Hª?©T� I/�5� I���I$4����_IsÇ J ¬/°/°/°g¬ Ç5©«UP7?�_4��'�R<��&� 9���I�=Q� GDI�Ç � �

�T<>4CA�I^� GDI*2/4DB�A5��I/��B>I$?@� �R2DIf�R9 C34�<�����9b4@� 9bC34�<��T�'� B#��I���� 9;4DCCI½8:9;26<>4�? EHGDI$4¦L�N�N��%U.PT� G�I*<��&� 2�<��j9¢B��P?'�

9b4G����ITI��PG@�RI���B>C34�9"� 9 B34+I½@%<!A/9b4�I/�fL�N�O�N%U.PÆ� GDIf� <!� C>I/���RG/<!�R<��&��I/�R9�� � 9;�/�:I­I>7ÊCD7"=Æ� B#�T<!� 9 B34T=5B>I��bBQ�T9"�«�èI/���x7tU=9b4

<:�*2'��� 9"� <!� C>I/�¿� �R<���'�9b4DC:�'� B�A5�bI���I§\B��R?�B34�I/�H<��¡7�L�N�N��Q=¸L�N�N��QP5`_B�9"� ~TB32���L�N�N��%U.P'� GDId9���<xC>I��RG�<!�R<��&��I/��(

9�� � 9;�/�¢9b4���B%���/2���I/�¿B�9�� 9 B347Is�]� <!�P?=<>4/?��S�b< '>IWL�N�N��%U.P'� GDIC>I$4DId9b4�?�9��T<!��B��H9b4�<^����` ��I���2DI$4'��Id<>4�<��"�#� 9��

I{EHG�2��0�PG�9�����L�N�O�N%U.P�� GDI¢2�4/?DI/�0�"��9b4DC¿B>B%�b<!� 9��b9��{��9b4�<>4*I���B34DB%��9;�T<��H� 9���I}��I/�R9 I��¿I���9"� �g<>4�?�5DGDI/�/G�<!�R?�L�N�N%��UP7

Î�GDId<!�'�5�;9��T<!� 9 B34'��B�*�?'��4�<��§9��M� � <!��I^� �/<���I��!B�?DI���9b4�����`#<>4/?��5� B���I$9b4$��I���2DI$4'��I�<>4�<��"�#� 9���<!� I|B�*s��I$4

� I/*­I/��� I$?8��B!<��¿� GDI:¾HqK¥H¥Hx&  /Vp!l��!n�¢^|<n�¥Hx/r u�IK��� B>C3G=I/��<��¡7�L�N�N���P/8�9b2T=�%I$2�9�<��;?=<>4/?=8�<�9��RI$4'��I�L�N�N%��UP7

cdY���I/�'�¸*­B��:<S*­I/9¦� ��I��T9b<����T<���I���=!�/�bB%��I$?#(§*­B��0� <>4�<��"�#� 9���B�*�?���4�<���9��T<������Q� ��I�����9���2'� 2/<����"��*­B��0��9b?/<!A5� I>7

@�I���I$4����"��=�� GDI/�RId9;��<W� 2'� C>I�B�*g9b4���I/� I�� ��9b4=?DI�� 9 C34�9;4DC^h�B34H��I*E�<!�0� BW�!I/� GDB�?'�¿*°B���� GDId<>4�<��"�#� 9���B�*¸� G�I���I

�!B�?DI����T7��_4G*½<��&�/=T�!B%� �B�*d� GDIf�RI/*°I/� I$45��I���C39�B>I$4�9b4CcdYD<����5� I:e§2'� I^h�B34H��I�EH<!�0� B�B��9���I/�R<!� 9"B>IW�!I/� G#(

B�?'�T7dÎ:B!9��<�5� I��!I$4��Xh�B34H��I*E�<!�0� Bf*­B���<�?���4/<���9��M���Q� ��I��G=Q9�I|4DITI$?T=�<!��<>4H��� 9��!I ® =Q�R<>4�?DB%� � <��<�5� I��

I$9"� GDI/��?'�R<�9%48*s� B%� ¨'©0IKª?©«U¢?�9"�RI��&���"��=�B���?��R<�9�4�*s� B%� <>4DB�� GDI/��?�9�� � �R9�A/2�� 9 B34T=�� <����©0IKªd©{U.=�<>4�?$9�I$9 C3G���I$?

�'� B���I/�0���»I½9�����B���� <>4'��IG�R<����5�b9b4�C�UP7 5Q� <!� 9��G�!I/� G�B�?'��=¢I>7ÊCD7"=��!B%� �§B�*�� G�IG��B��/2'�b<!��hÞESh E ���RGDI���I��

I{EH<!�0�;9b4�I/��<��¡7�L�N�N�eQ=�E�<!����I/��<>4�?$�|B3G�47L�N�N���U.=/<��RG/9 I/B>I�� G�9��¢I$4�?8A���� � I$<!� 9b4DC�I$<��RG@¨ © ��I/�/<!�R<!��I��"�=<>4�?

� I/��I$<!� 9b4DC4�R<��!I '�9b4�?�B�*�9"��I/�R<!� 9"B>IM�'� B#��I�����I��T7?�_4!B�� GDI/�}9�B��R?'��=�<�����B�*Æ� GDI��RI�� 2'�"���MI½9{7ÊI>7"=H�R<>4�?�B%�¦?��R<¹9M�.U

B�A'� <>9b4�I$?A<!��� 9��!I ® <!� I�?/9����T<!�R?DI$?$9�GDI$4$� GDIf� �#� ��I�� I/B>B%��B>I��M*s� B%��¨5©a��B�¨'©;Å J 7

J�B¹9�I/B>I/��=49%GDI$4�� G�IV� �Q����I�� 9������ Bg9M�"�»Br<!����9b4DC�=�I½9¡7ÊI>7 � GDI ��� ?�9�� � <>4'��IFA�I/�{9�ITI$4 ¨5©�IKªd©{UF<>4�?

¨'©;Å J IKª?©«Uz9��È����<��;�,U.=%�R<>4�?�B%� � <����5�bI��zB�A'� <>9b4DI$?§<!�}� 9��!I ® �T<>4�A�I�� �RI&(�2'��I$? � ��B�GDI��"�<��B34'��� �R2'�&�}�P<>4�?DB%�

� <����m� I��¢<!�S� 9���I ®�� L���B�<��¿��B!9����5� B¹B>I�I����T9 I$4'�&�>7È�)��<xC39b4�IM� G�<!�S9�IG�<¹B>I|<:�R<����5� I���© ¯�� ª�� ���© ¬�� ¯

L!¬/°/°/°¹¬! #"%=�?'�R<�9%4�*s� B%� ¨ © 7�R GDI$4 � G�IV� �Q����I�� I/B>B%�"B>I��@��Bb¨ ©;Å J =|9"�A9��è?DI��R9"�R<!A5� I7��B*'>ITI/� � G�B%��I

ª � ���© <>4�?K<!� � <��RG���BFI$<��RG B�*}� GDI�� B34�I�B�����I/B>I/�R<��?Ç � ���©;Å J ?��P<�9%4C*,�RB%����B%��I�<!�5�'� B��'�R9;<!��I*?�9���� �R9"A/2�� 9bB34

Page 52: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

!©;Å J I # � ª�� ���© UP7�8:I/����©;Å J ?DI$4DB���If� G�I:� <��<�5� If� ��<���I�B�*dÇ5©;Å J 7�Î%GDI$4G� GDI^*°B��RITC>B39b4DC�9;?DI$<§9���I���2�9�Br<�� I$4��

��B�?'�R<�9%9;4DC�� <����5� I�*s� B%� � GDI<�'� B�?�2'�&��� ��<���I � ©�� � ©;Å J 7��zI/��� B�*s��I$4¦� GDI§I/B>B%�"B>I$?¦� <��<�5� I<ª � ���©;Å J ¯

IKª � ���© ¬ Ç � ���©;Å J U�4DITI$?5�i��B�A�IMl�x&ÀXx&q"£!¾Qv{x�¥ B��¿l x&u&p!|_kmr�x�¥S��Bd<��/��B%�<�!B�?�<!��I�¨5©;Å J 7?Î�G�9;�:9��i� GDIXA�<�� 9��d�'�P9b4'�T9"�5�bI

A�I$G�9b4�?�<����!B%� �d<�����<�Bx<>9��b<!Am� I:� I���2DI$4�� 9b<��?hÞE ��I/� GDB�?5�/=�I>7ÊCD7"=i��I/��~$2�9b4�9�I/�|<��¡7�I)L�N�N%��U.=?\dB��R?DB34 I/�|<��¡7

I)L�N�N��%U.=ÆJ%I$4�?'���§<>4�?è@�9��RG�<!�P?iI)L�N�N!�HU.=m��9�� <xC3<�9H<@I)L�N�N��%U.=���B34DC�I/�%<��¡7MI)L�N�N���U.=�8�9b2è<>4�?6EHG�I$4FI)L�N�N��%U.=

hK<��/c�<��RG�I/�R4T=�E��"��?�Id<>4/?A8:9b2VI)L�N�N�O%U.= ��9�� ��<>4�?�5�GDI/��G�<!�R?�I)L�N�N%��U.=mRÞI����:I)L�N�N�e%U�I/���x7

Î�B^*½2���� GDI/��I��b<!A�B��R<!��I�B34�� GDI���I9b?DI$<��/=�9b4�� G/9���<!��� 9;�/� IM9�I�?DI��0�&�R9"A�I%<�C>I$4�I/�R<��5*s�R<��!I/9�B���'<*­B��¢25� 9b4DC

��I���2DI$4�� 9b<��Sh6B34���I6EH<!�0� BF�!I/� GDB�?'��9b4�?'��4�<��§9���� �#� ��I��<�$7���4�?�I/�^� G�9;�f*s�R<��!I/9�B���'�=È9¢I=I&Y#��I$4�? <>4�?

2�4�9�*,�:�'� I/B�9 B32'�����:�!B��RI_� I���� �R9��&� 9"B>I��!I/� GDB�?'�/=���� 2�?��WBr<!�P9 B32'�X� I/9¢I$9bC3GH� 9b4�C*<>4�?�� I�� <����5�;9b4DC���I��RG/4�9���2DI��

�'� B���B%��I$?¸=�<>4�?Y?/9����T2'���f��B34�4DI��&� 9 B34'�*<>4�?¦��B%�<�/<!�R9���B345�|B�*�� GDI���I§<!�5�'� B3<��RG�I��T7�` ��<>9b4V�!I��0� <xC>I�9�I

9H<>4H�?��B4��B%�<��2�4�9��T<!��I�9b4^� G�9���<!��� 9;�/� I�9;�j� G/<!�a� G�IS� I���2DI$4�� 9b<���9�����B���� <>45��I¿� <��<�5�b9b4DC:I{5Q��55U¸��I/� � 9;4DC��'� B!(

B�9b?DI��H2'�H<�C>B�B�?$*,�P<��!I/9¢B��('�*­B��H2/4�?DI/�0� � <>4/?�9b4DC:��<>4H�§I&Y�9���� 9b4DCW�!I/� GDB�?'�H<>4�?$*°B��S*½2���� GDI/��9��<�'� B¹B�9b4DC

� GDI�� IsB�9b<�@�<xB!({�S�b<���'Q9¢I��;�b9"~$<!� 9 B34T=m��B%���b<!�5� 9;4DC�I/���x7tUP7

5�I��&� 9 B34�ed?DI����&�R9"A�I��a� GDI�C>I$4DI/�P<���9b?DI$<B�*�� GDI���I���2DI$4H� 9;<��D9�����B���� <>4'��I�� <����m�b9b4DC�I{5Q� 5'Uj�!I/� GDB�?�<>4�?

��I/B>I/�R<�� '>I/�A9����5�bI��!I$4H� <!� 9bB34è9���� 2DI���='� 2'�RG6<���� GDIf�PGDB39���I*B�*a� <��<�5�b9b4DC!?�9;� � �R9"A/2'� 9 B34T=H�RI�� <����5�b9;4DC�=�<>4�?

h�B34H��I§EH<!�0�bB§9b4�*­I/� I$4'��I>7�5�I��&� 9bB34 �§?�9����T2'�0��I��M��I/B>I/�P<��j� B#�T<��ah6B34���I!E�<!�0� B���I/� GDB�?5��� G�<!�d<!� I*4�ITI$?DI$?

9%GDI$4K5Q��5�I$45��B32�4H��I/�.�M��I/��� <>9b46?�9 ���T2'��� 9 I��T7�5�I��&� 9 B34����'� B���B%��I���� G��RITIf�!I/� GDB�?'��*­B���� I�� <��<�5�b9b4DC:*s� B%�

��©�<>4�?��'� B¹B�9b?DI��!<YC>I$4DI/�R9��è<�� C>B��P9"� G'� � G�<!����B%�fA/9;4DI���5Q� 5 <>4�?E� I��R<����5�b9b4�CD7 5�I��&� 9 B34b�FA'�P9b4DC%��9b4

@%<xB!({�S�b<��('�9�I����b9�~$<!� 9 B347*°B��|9����5� B¹B�9b4DC�I���� 9���<!� 9 B34�7|5�I��&� 9 B34F��C39�B>I��_� G'� ITI*I&YD<����5�bI��M��BF?DI��!B34'��� �R<!��I

� GDI�2'��I�B�*�@�<xB!({�¿�;<���'Q9¢I����;9"~$<!� 9 B34K<>4�?G��B$��B%���/<!� I�?�921�I/� I$4����'� B#��I$?�2�� I��$7�5�I��&� 9bB347����B345�/�b2�?DI��M9%9"� G

<�A'�P9 I/*j� 25�<��<!���>7

Page 53: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

� ����� �� �������� � & �b�»��� � ��� &�� ��� � � � ��� � � � �

6d4DI�B�*d� GDI^�!B%���_� 2'�/��I��0� *­25�i�!I/� GDB�?'�M*°B��<>4�<��"�Q~$9b4DC§<���B%���5�b9��T<!��I$?C�'� B�A�<!A/9��b9���� 9��_���Q� ��I�� IK� 2'�PG

<��|<è4DB34'�;9b4DI$<!��� � <!��I�� �/<���I<��B�?DI��sU�9���� GDI�\|9"A'A5��� <����5�bI/��I{E�<!�0�b9b4KI/��<��¡7�L�N�N�eQ=�EH<!����I/��<>4/?��|B3G�4T=

L�N�N���=¿\I���*­<>4�?�<>4�? 5���9"� GbL�N�N!�#=ÈÎg<>4�4DI/��<>4�?¦RÞB34DC+L�N�O%��UP7FJ�B¹9�I/B>I/��=È� GDI�\d9"A'A5�^� <����5�bI/�*9��^� I����

<!� � �R<��&� 9"B>IF9%G�I$4 B34DI � �=9b4���I/� I�� �F9��§9b4 l x0p!r�vKqs|�x��'� I$?�9��&� 9bB34 <>4�?�2���?�<!� 9b4DCY9b4 <�?���4�<���9;�G� �#� ��I��è7

`4DB�� GDI/�<� 9"� 2/<!� 9 B34½*°B���� G�IC\d9"A'A5�:� <����m� I/����B�A�IF9;4DI&1�I��&� 9�B>IF9;�W9%GDI$4+� GDIG� � <!��I��§B�*M� GDI�� I��R2'�"� 9b4DC

� <����m� I��:<!� I¿B>I/����� � � 9;��'Q���#=�� I$4/?DI/�R9b4DC�� G�I¿� <��<�5� I/�iB>I/�0��?�9 ���T2'���T��B��!BgB>I�IKhK<��/c�<��PGDI/�R4�I/��<��¡7SL�N�N�O%UP7

�_4�� G/9��X�T<�� I�9"��<!�5��I$<!�.�X� G/<!�H9;4H��I����;9 C>I$4H�����<�RGDB�B%� 9b4DC�<�?���4�<���9���� �#� ��I�� *°B�����I���2DI$4�� 9b<���2���?�<!� 9b4DC:�T<>4

A�I8��B�� IFI����T9 I$4���IsRÞB34�CÏ<>4�? 8:9b<>4�C+L�N�N%��UP7+RÞI$35�0� ��?DI����&�P9"A�I§B34DIFB�*_�R2'�RGE��I���2DI$4�� 9b<��H2���?�<!� 9b4DC

� � �R<!��ITC39bI��/=5� GDI$46?�9����T2'�0��9�������I/B>I/�R<�� '>I/�F9;���5� I��!I$4�� <!� 9 B34è9��0� 2DI��T7

����� �����! "�$#�%&�$')(+*�,.-0/21436587+(+,.'&9:�� ",.143;-�*<'�=?>@ �/+ ;A

` 25��I/*­25�X9H<��+��B�� I/�'�RI���I$4H�!<F��B%���5�;9��T<!��I$? G�9 C3G ?�9���I$4'� 9 B34�<��¢?�9�� � �P9"A/2�� 9 B34¸=a� 25�RG <��<¨5© IKª?©«U.=�9��:A��

��2'�"� 9"�5�bI�h�B34H��I�EH<!�0� B�� <����m� I���?��R<¹9%4$*,�RB%� 9"�T7ÈhK2'�"� 9"�5�bI9��<�/2�� <!� 9 B34�I½@%2'A/9b4FL�N�O%��U�9���<��R2'�/��I���� *½2'�

I&YD<����5� I*B�*}� 2'�PG�<��'�R<��&� 9���I:*°B���� 2��0B>I/�A?�<!� <�7_�_4G� G/9���<!�0� 9��/� I�=59�I�<>?�B>BQ�T<!��I!<8� 9���9;�b<!�M�!I/� GDB�?DB%� B>C��

��B<� G�<!�%B�*�@�2�A/9b4 � �X*­B��%<>4�<�����~$9b4�C�?'��4�<��§9���� �#� ��I��<�T7

�����?�a�,�g�s�m� ) ��l p! m¥Hn!|¢�p!l[qKp�o&r�x�B ¥!l p!Àd M¡[l n!|p�¥�q;u&vKl.q§o&��vKq>n! �<q;udu/p!q>¥�v§n4o0x��"Cg� �È��C �,³ED:�#�GFIH��¹�Q²

o&z8p<ÀXx/q2£�¾�vKq, Q£M¡[�# mw�vKq>n! �J:IGB�U4ÀdqKvs¾$l x&u§k�x0w�vSv§n�vs¾�x^¥�q;u&vKl.q§o&��vKq>n! �¨�q ¡d¡&n!lfp� 'z�qs 5v{x§£%l)p�o&r�xX¡.�# mw¹vKqKn! LKƱ

MON � KjIGBFU<JWIGBFU " ¯ MOP � KiIGB�U "%°

� u/x�v^n)¡�l p! m¥Hn!| ¥!l)p�À?u7p! m¥­ÀXx&q"£!¾QvsuCIsÇ � ��� ¬QJ � ��� U0± � ¯ L!¬0eQ¬/°/°/°�¬�q;u$u&p�qK¥C�'� B���I/�0���V9¢I$9bC3GH��I$?ÍÀdqKvs¾

l�x[u§k'x�w�vSv§n:¨»q ¡

�b9;�RTSVU

W R� ?�J KjIsÇ � ��� U<J � ���W R� ?�J J � ���

¯ MXP�IYKjIGBFU U I)L¹U

Page 54: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

¡/n!l<p! 'z�qs 5v{x§£%l)pHo/r"x�¡[�Q �w�vKq>n!  KiºWÄ. +p:kml)pHw�vKq>w0p!rdu�x& �u�xWÀXx�w0p! +vs¾Hqs  �$n)¡�¨ p�u�o0x/q, Q£@p.k%k�l)n�y%q,|<p�v{x0¥Vo&z

vs¾#x^¥!q,u&w/l�x/v{x^¥!q,u[vKl[q>o/��vKqKn! Cu.�gk%k�n!l[v{x0¥$n! Cvs¾#x�Ç � ��� Àdqsvs¾^kml nHo�pHo/q,r¤qKvKq>x&uXk�l)n.k�n!l[vKq>n! mp�rav§n�vs¾#x�ÀXx/q2£�¾�vsuTJ � ��� º

8�I/� ��© ¯ � ª�� ���© ¬�� ¯ L!¬/°/°/°�¬! #"�?DI$4DB���IK<V��I/��B�*��R<>4/?DB%� ?��P<�9M��� G/<!�§<!� I@�5� B���I/�0�"�¦9�I$9 C3G���I$?

AQ��� GDI8� I/�*B�*�9�I$9 C3G������ © ¯ � J � ���© ¬�� ¯ L!¬/°/°/°¹¬! #"$9%9"� G¦� I�� ��I��&�:��B7¨ © 7�8�I/� � ©;Å J A�I�� GDI8�R<����5� I

� �/<���I|B�*"B�©;Å J =D<>4�?�� I/��!©;Å J A�I|<W� �R9;<���?/9�� � �R9"A�2�� 9 B34�7aÎ�GDI$48� GDI*5Q� 5��'� B#��I$?�2�� I���B34'� 9������HB�*j� I��T2'�0� 9"B>I

<!�'�5�;9��T<!� 9 B34'�HB�*a� GDI4*­B%��� Bg9%9b4DC�5Q� 5$� ��I/�5�/�

������� �¹��� �S*­B���� ¯ L!¬/°/°/°¹¬! ��

IK`�U����R<�9�B ©;Å J ¯ Ç � ���©;Å J *s� B%� ©;Å J IsÇ ©;Å J � ª � ���© U.P/<!� � <��RGK9"����B�ª � ���© ��B<*°B��.��ª � ���©;Å J ¯ IKª � ���© ¬ Ç � ���©;Å J UP7

IK��U%E�B%�<�/2���I

� � ���©;Å J ¯ ¨'©;Å J IKª � ���©;Å J U¨ © IKª � ���© U� ©;Å J IsÇ � ���©;Å J � ª � ���© U

P <>4�?G� I/� J � ���©;Å J ¯ � � ���©;Å J J � ���© ° I>e%U

J�I/� I � ©�9����T<��;� I$? <>4 ��9b4'�&� I���I$4H� <���9�I$9 C3G��T7 ���]��9���I$<�� �+��B¦� GDBg9 � G�<!�CIKª � ���©;Å J ¬QJ � ���©;Å J U�9��!<F�'�RB���I/�.�"�

9�I$9 C3GH��I$?��R<����5� IdB�*j¨ ©;Å J 7}Î�G�2'�/=Q� GDI�5Q��5��T<>4$A�I|<!�'�5�b9bI$?�� I��T2'�0� 9"B>I��"��*­B�� ®È¯ L!¬0eQ¬/°/°/°,=#��B�<��/��B%�<��B!(

?�<!��I�<>4AI/B>I/� ({�RG�<>4�C39b4DC�?'��4�<��§9��T<��T� �#� ��I��è7

Î�GDIè5Q� 5��!I/� GDB�? 9���<���� B�2'��I/*½2'�¢9b4Ï4DB34�({��<¹�>I�� 9b<>4½��B%���/2'� <!� 9 B34­�R2'�RG <���I/Bx<��b2�<!� 9;4DCC�b9 '>I��b9bGDB�B�?

*½2�4'�&� 9 B34'�$7a`_�'�5�b9;�T<!� 9 B34'��9b4<� G�9���?/9"� I��&� 9 B34��T<>4�A�I�*­B32�4�?�9b4<J�I$4�?����!<>4�?�@%9��PG�<!�R?CI)L�N�N!�HU¢<>4�?��]��9%9b4DC

I/�*<��{7¦I)L�N�N���UP7C�¿�R9 I�5��=?� 2��5��B%� I�9¢I=<!� I�9b4���I/� I�� ��I$? 9b4ÞI/Bx<��b2�<!� 9;4DCG� GDI8�b9 '>I��b9bG�B�B�?7*­2�45�&� 9 B34 ��I � U ¯

ÏaI�� J ¬/°/°/°¹¬���©)P � U�9b4<� GDI���9��0� 9b4DC�?/<!� <f�'�RB�A5� I���IKcÈY�<��<�5� I^L¹UP7XÎ�GDI$4�*°B���I$<��RG83'Y�I$? � Bx<��b2DI�=Q9¢Id<!�'�5���

� GDI�5#��5��'�RBQ��I$?�2'� I���B§9����/2'��I�IsÇ J ¬/°/°/°�¬ Ç © U���I���2�I$4H� 9b<��;�"��9%9�� G J IsÇ J U ¯ ÏaIsÇ J � � J P � U�<>4�?

> IsÇ > � Ç J ¬/°/°/°¹¬ Ç > 8 J U ¯ ÏaIsÇ > � ª > 8 J ¬�� > P � U.¬� ¯ eQ¬ �Q¬/°/°/°

�|B34DC�I/�*<��¡7¦I)L�N�N���U:�RGDB¹9 � G�<!� W R� ?�J J � ���© � 9;��<>4 2�4HA�9b<���I$?ÞI�� � 9;��<!��I�B�* ��I � UP7$�i4 5�I��&� 9 B34+��9�I

� GDBg9 � G�<!��@%<xB!({�S�b<��('�9�I����b9�~$<!� 9 B34bI{EH<���I��;�b<F<>4�?Þ@�B�A�I/����L�N�N��Q=�8:9b2T=ÆRÞB34DCA<>4/?��|B34DC�L�N�N���U��T<>4�A�I

Page 55: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

<!�'�5�;9 I$?8��B�B�A'� <>9b4A<<A�I/� ��I/�%I�� � 9���<!��I>7

��� � � �&5 *�9:� 5��V( �&� �, 143 -�*�'&=�� *�� ( 7 *��;%�( * 5 ' ©;Å J *<' "/+

Î�GDI��PGDB39���I|B�*a� GDI4� <����5�;9b4DC*?/9�� � �R9"A�2�� 9 B34 !©;Å J 9��H?�9"�RI��&���"��� I��;<!��I$?$��B�� GDI|I����T9 I$4'�&�=B�*i� G�I��'� B���B%��I$?

5Q� 5��!I/� G�B�?�7��DB����H<��>I��R9b<>4���9����R9b4DC�?�<!� <^�'� B�A5�bI��<�_I­I&YD<����5� IWL¹U.=#�|B34DC�I/��<��¡7�I)L�N�N���U¿� 2�C>C>I�� ��25� 9b4DC

!©;Å J IsÇ5©;Å J � ª?©{U ¯ ¨'©;Å J IsÇ5©;Å J � ª?©«U ¯ ÏaIsÇ5©;Å J � � ©;Å J ¬�ª?©«U.¬

9%9"� GC� GDI�9b4'�&� I���I$4H� <��j9¢I$9bC3GH� � ©;Å J <»Ï?I���©;Å J � � © ¬�ªd©{UP7���B���IW� G�<!�|<��"� GDB32DC3GC� GDI�I&YD<��&��Bx<��b2DI�9��4DB��

I$<�� 9���� '�4DBg9%4T= � ©;Å J �T<>48��B%�!I/� 9;�!I��}A�I���B%���/2���I$?=2�����B�<�4�B��0��<��b9"~$9;4DC4��B34'� � <>4��/=#9%G�9��PG§9��X� 2���T9 I$4��

*­B��I���� 9���<!� 9 B34K2'� 9b4�C<*°B��.�*2'�;<7I)L¹UP7�Î�G�9;�M�RG�B39���I�B�* ©;Å J 9��<�����B=2'��I$?K9b4�8�9b26<>4/?ÞE�GDI$4+I)L�N�N��%UP7 ��B��� GDIf��� <!��If� �/<���If��B�?DI��ÈIKcÈY�<��<�5� Ife%U¿9%9�� G '�4DBg9%4�Ç�� ¯ I � ¬(5TU.=/<�� 9��§9��b<!�S� �R9b<���?�9;� � �R9"A/2'� 9 B34�9;�

!©;Å J IsÇ5©;Å J � ª?©«U < 3�©;Å J I���©;Å J � Ç5©;Å J ¬(5TUA6/©;Å J IsÇm©;Å J � Ç5©)¬ � U

� ©;Å J ¯ � 3�©;Å J I���©;Å J � Çm©;Å J ¬(5TUA6/©;Å J IsÇ5©;Å J � Çm©)¬ � U]Ë%Çm©;Å J °

�_4�� GDI|C>I$4DI/�R<���?���4�<���9���� �#� ��I�����I/� � 9;4DC�='9�If� 2DC>C>I�� � ���B<A�I4�RG�B%��I$46<��

�©;Å J IsÇm©;Å J � ª?©{U ¯ ¨'©;Å J IsÇm©;Å J � ª?©«U.¬ ®È¯ L!¬0eQ¬/°/°/°g¬ I �%U

9%9"� G$� G�I�9b4'�&� I���I$4H� <��Æ9�I$9 C3G��

� ©;Å J ¯ ¨'©;Å J IKª?©{U¨ © IKª © U ° Is��U

��B���IX� G�<!� � ©;Å J 9b4�Is��U:?DB�I���4�B���?DI/��I$4�?|B34^� GDI¿Br<��;2DI�B�*'Çm©;Å J <>4�?^� G�9��¸*­I$<!� 2�� I�9��:9;����B���� <>4H�j��B�� I/B>I/�R<��

9����R2DI���?�9;���T2'����I$?��b<!��I/�$7ÈÎ�GDI_� I$<���B34�� G�<!�H?��R<�9�9b4DC4Çm©;Å J *s� B%� ¨5©;Å J IsÇ5©;Å J � ª?©{U�9��}��B�� I?�I�� 9"�R<!A5�bI�� G�<>4

*s� B%�0<7�!B�� I�B��:� I��0��<!��A/9�� �R<!���7*­2�45�&� 9 B34 !©;Å J IsÇ5©;Å J � ª?©{U|9��^�/� I$<!�^*s� B%�-� I/9��R9�� 9b4DC�� GDI=9b4'�&� I��!I$4�� <��

9�I$9 C3GH�:I>e%UH<��

� ©;Å J ¯ ¨'©;Å J IKª?©«U¨'©0IKª?©«U

¨'©;Å J IsÇm©;Å J � ª?©{U�©;Å J IsÇ5©;Å J � ª?©«U °

Page 56: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

�_4H� 2�9�� 9"B>I��"��=i� G�I���I���B34�?­�R<!� 9 B�9���4DITI$?�I$?V��BC��B��0� I��&�^� G�I§?�9����&�RI/�/<>4'�&�FA�I/�{9�ITI$4 !©;Å J IsÇ5©;Å J � ª?©«U�<>4�?

¨'©;Å J IsÇm©;Å J � ªa©«U¿9%GDI$4$� G�I/�F<!�RI�?�921�I/� I$4H�T7

6�� GDI/���PGDB39���I��dB�* ©;Å J <!� I!<�����B8��B%�0� 9"A5� I>7 �DB���9b4'��� <>4'��I�=�9"*Ȩ © IKª © UM�T<>4FA�I �_I&Y#��I$4�?DI$?��8*­B���Ç ©;Å J =

B34DIf�§<��è2'��I

�©;Å J IsÇm©;Å J � ª?©«U ¯ ¨5©0I IsÇ5©;Å J � ªd©{U.° I>�%U

�DB��:cdYD<����5� IGL!=j� G�9�����B���� I�����B34/?'����B �©;Å J IsÇ5©;Å J � ª?©§U ¯ ÏaIsÇm©;Å J � ª?©)¬�� © U.°iÎ�G�I<��B���� I�� ��B34�?�9;4DCè9b4'�&� I&(

�!I$4�� <��¸9�I$9 C3G���9�� � ©;Å J <+ÏaI���©;Å J � � © ¬�ª?©;Å J U.° ��B��_cdYD<����5�bI�eQ=m�RGDB39���I$I>�%US��B���� I�����B34/?'�S��B

�©;Å J IsÇ5©;Å J � ª?©«U ¯ 6/©;Å J IsÇ5©;Å J � Çm© ¬ � U

<>4�? � ©;Å J < 3dI���©;Å J � Ç5©;Å J UP7$Î�G�9;��9���2'� I$?Ï9b4V`_B�9"� ~TB32��8I)L�N�N��%U.=X\dB��R?DB34YI/��<��¡7�I)L�N�N��Q=�L�N�N��%U.=�<>4�?

��9"� <xC3<¹9�<CI)L�N�N��%UP7 ��B���I�� G�<!��� G�9��¿� �R9;<���?/9�� � �R9"A�2�� 9 B34�C>I$4�I/�R<!��I���Çm©;Å J 25� 9b4DC*B34'�"��� G�I���� <!��I�I���2/<!� 9 B34�7

E�B%����<!� I$?�9%9"� G»I �%U.=�?/9�� � �R9"A�2�� 9 B34½I>�%U�9���2'� 2�<������6I$<�� 9 I/�^��B62'��I�A/2�����I$4�?'�f��B@� I��R2'�"��9b4ÞC��RI$<!��I/�

h�B34H��IÏEH<!�0�bB­Bx<!�R9b<!� 9 B34IK��I/��~$2�9b4/9�I/�è<��¡7 L�N�N%��UP7 �i4»� GDIF� � <!��I�� �/<���IF�!B�?�I��_�T<���I�=9"�F9��§B�AQB�9 B32'�

� G�<!��� GDIW�RGDB39���I�I �%U%9��_��B�� I*?DI��R9"�R<!A5� If� G�<>4+I>�%UMA�I��T<>2'��I^� GDI:*°B��0��I/�9b45��B�����B��R<!��I���� GDIW�!B%� ��� I���I$4��

9b4�*­B��0��<!� 9bB34§9b4-� ©;Å J 9%GDI/�RI$<��X� G�I_�b<!� ��I/��?DB�I��H4DB��T7 �_�R9b4DC�I �%U¢G�<���<>4DB�� GDI/��<>?�Bx<>4H� <xC>I�9;4�I�� � 9��§<!� 9 B34T=

9%G�9;�RG�9%9�����A�I?�9����T2'�0��I$?§9b48��I��&� 9 B34�e�7 �D7È�_48��<>4H��<!�'�5�b9��T<!� 9bB34'�/=�G�B¹9�I/B>I/��=�9"�X�§<���4�B��XA�I%I$<�������B�2'��I

I �%UP7H5�I��&� 9 B34 �W�'�RB¹B�9b?DI����!I/� GDB�?'��*­B��M��B��/9;4DC�9%9"� G$� G�9;��?�9 ���T2'���{�>7

����� � � �2,.1!3 -�*<'�=!*<' "/+ �> �/+ �� A

5�2��5��B%� I ��© ¯ � ª � ���© ¬�� ¯ L!¬/°/°/°¹¬! #"%9��?�5� B���I/�0�"�49�I$9 C3G���I$?�A����7© ¯�� J � ���© ¬�� ¯ L!¬/°/°/° #"�9�9"� GW� I�� ��I��&�

��B�¨'©_7�8�I/�H2'�S�T<�����I$<��RG�ª � ���© <��>u&vKl x0p!|<º��}�_4'� ��I$<>?=B�*i�T<!�0����9;4DCf� GDI�9�I$9 C3GH� �7©g<��¿� GDI����Q� ��I�� I/B>B%�"B>I���=

9"�H9��H<�����BW� ITC39"� 9��§<!��I�=�<>4/?8� B%�!I/� 9��!I��SA�I$4DI&3m�T9;<��aI½8�9b2�<>4/?èE�GDI$4T=ÆL�N�N��%U.=5��B!9b4'��I/�0�¢<:� I�� <����m�b9b4DC:� ��I/�

?DI����&�P9"A�I$?�<��S*­B%��� Bg9M�}A�I/�«9¢ITI$4�5#��5<� I��T2��0�R9 B34'�/=�<>4�?$� 2'�PG=<:�'�RBQ��I$?�2'� I9;�X�RI/*°I/���RI$?���B!<��S� GDI�5Q� 5<9%9"� G

O

Page 57: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

� I�� <��<�5�b9b4DC�I{5Q� 5�@�UP7

�8��·�´mµD� �K�s�)F�·/�g� � �8I½9,U_� <��<�5� I�<è4DI/9 ��I/�dB�*¿��� � I$<��<�<I½?DI$4DB���I$?Þ<�� ���© U_*,� B%� ��©�<��/��B��R?/9b4DC$��B�� GDI

9�I$9 C3GH���XJ � ���© P�<>4�?�� GDI$4¦I½9;9,U�<���� 9 C34FI���2�<��T9�I$9 C3G�������B<� G�If� � � I$<��<�%9b4 ���© 7

�]�9��4DB��d9��<�!I$?/9b<!��I��"���/� I$<!��9%G��èB34DI�4�ITI$?'�M�RI�� <����5�b9;4DC§<!����I/��� <>9b47� � <xC>I ® 7_`�����2'�PGK?�I/� <>9�� I$?

� GDITB�� I/� 9;�T<���?�9����T25��� 9 B34�9��HC39"B>I$4$AQ��8:9;2=<>4/?èE�GDI$4VI)L�N�N��%U.=59�I|B34'�"����I$4H� 9 B34è<:*­I/9 GDI$2��R9;� � 9��/��B34$� GDI

9����R2DI>7 ��9"�0�����"��=r9�*�� GDIX9�I$9 C3G����"J � ���© <!� IS��B34'� � <>4���I­B��z4DI$<!�?��B34'� � <>4��.Ui*°B��z<���� ® IK� 2'�PG*<��T<���IHB#�/�T2��0�j9%GDI$4

B34DIf�T<>4�?��R<¹9 *s� B%��¨'©¢?�9"� I��&���"�'U.=�� I�� <��<�5�b9b4DC�B34'����� I$?/2'��I���� GDI�4�2'�^A�I/��B�*z?�9�� � 9b45�&� 9"B>I�� � � I$<��<�<>4�?

9b4�� � B�?�2'��I���I&Y#� �R<<h�B34H��I�EH<!�0�bB�Br<!�R9;<!� 9 B34�7}Î�G/9���� 2DC>C>I�� ���_� G�<!��B34�I^� GDB325�b?F4DB��M��I/��*­B��0� � I�� <��<�5�b9b4DC

9%GDI$4$� G�If��B�I��8�T9 I$4H�%B�*jBx<!�R9b<!� 9 B34¸=���� �© =�*°B��M� GDIVJ � ���© 9�������<��;�¡7d`_��<!� C32DI$?A9b4���B34DC!I/�%<��}I)L�N�N���U.=�� GDI

� x��x0w�vKqs¢!x�u&p!|_kmr�x_u.q�¹x �R9;�z9b4HB>I/�.��I��"�4�'�RB���B��0� 9 B34�<��Q��B<L � ��� �© 7�5�I���B34�?5�"��=H�|B34DC|I/��<��{7�I)L�N�N���UÈ� GDBg9E� G�<!�

<���� GDI4���Q� ��I�� I/B>B%�"B>I������ �© qs mw/l�x�p!u/x[u�� ��B#�RG�<���� 9��T<����"�>7ÈR GDI$4$� GDI�9�I$9 C3G�����C>I/��B>I/���$��'>I/9�I$?A<!��� 9���I ® =

�T<!������9b4DC<�§<>4H�G� � �RI$<��<��9%9�� G�B>I/���G����<��;�T9¢I$9bC3GH����9��%<!�'��<!� I$4H�����=<�9�<�� ��I>7�@%I�� <����5�;9b4DC��T<>4@�5� B¹B�9b?DI

�RG/<>4'��I��M*­B���� GDI*C>B�B�?¦I½9¡7ÊI>7"= ��9�����B���� <>4�� �HUS� � � I$<�������B�<��<�5�b9"*s��� GDI�����I��"B>I��%<>4/?AGDI$4'��I �)�RI]D�2�B>I$4/<!��I �

� GDI��R<����5� I/����BC�'� B�?�2'��I!<�A�I/� ��I/�f� I��R2'�"��*­B���¡.��vK�#l�x4� � <!��I���<��f���Q� ��I���I/B>B%�"B>I��/=?� GDB32DC3GY9���?DB�I��*4DB��

9����5� B¹B>I�9b4'*°I/� I$45��I���B34¦w/�#l[l x/ 5v¸� � <!��I:ª?©_7}cdYD<����5� I���9;465�I��&� 9 B34C��9;���b2'� � �P<!��I�� G�I���I�GDI$2��P9�� � 9��/�T7

Î�GDIX� I�� <����m�b9b4DCM�0�RGDI$?�25� I�I½9{7ÊI>7"=�9%GDI$4^��B��RI�� <����5� IgU¸�T<>4:A�I�I$9"� GDI/��?DI/��I/�0�§9b4�9�� � 9;�zB��z?���4/<���9��!=><>4�?

� GDIW� <����5�;9b4DC����PGDI��!I��T<>4FA�I�I$9"� G�I/���R9����5� If�R<>4�?DB%� � <����5�;9b4DCCIs9%9�� G@9�I$9 C3G����.U.=T� I�� 9b?/2�<��i� <��<�5�b9b4DC�=

B��f� B#�T<��Èh�B34H��IèEH<!�0� B$� I��R<����5�b9b4�C�I{5�I��&� 9 B34���UP7�Î�G�I<�!I/� GDB�?'�|B�*�\B��R?DB34ÞI/�*<��¡7@I)L�N�N��%U.=ÈJ��2'��~TI�� I/�

<>4�?G���2�4'���PGiI)L�N�N��%U.=���9"� <xC3<�9H<7I)L�N�N��%U.=Æ��I/��~$2�9b4/9�I/�%<��¡7�I)L�N�N%��U.=�<>4/? ��9"� ��<>4�?�5�GDI/��G�<!�R?VI)L�N�N%��U_�T<>4

<����ÆA�I4��ITI$4A<��d5Q� 5�9%9"� G�� ��I��T9b<��T�PGDB39���I���B�* !©;Å J <>4�?�9%9�� G$� I�� <����5�;9b4DC!<!��I/B>I/����� � <xC>I>7

N

Page 58: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

����� / ' � ��7 �$';9:��� *Y( ����5 ')( � � , 7 -�5 �,.1!3 - � ��_4è?���4/<���9���� �#� ��I��<�/=�9"��9��HB�*s��I$4AB�*�9b4���I/� I�� ����B!B�A'� <>9;4Vn! Frtqs �x�9;4�*°I/�RI$4'��IdB34�� GDI4� � <!��I4Bx<!�R9b<!Am� I��/=D9¡7ÊI>7

I�� � 9��§<!� 9b4DC MXP��<KjIKª?©«U�<!��� 9��!I^�T7_Î�G�9;��9���� � �R<>9 C3G�� *°B��09�<!�R?CAQ�A2'� 9b4DC7I)L¹U�9%G�I$46<�Bx<>9��b<!Am� I*9���<��R<����5� I

� ª � ���© "��'�RB���I/�.�"�W9¢I$9 C3G���I$?$AQ� J � ���© 7ÈJ%Bg9¢I/B>I/��=m��I/B>I/�R<���9����R2DI��X��B345��I/�R4�9b4DCW� � <!� 9���� 9��T<���I����T9 I$4'�&��B�*j� GDI

I�� � 9��§<!��I��%<!� I49�B���� GG��I$4H� 9 B34/9b4DCD7¢EH<���I��;�b<�I)L�N�N%��U��'�RB¹B�9b?DI���<�C>I$4DI/�P<��¸� � I$<!���!I$4��%B34G� G�9���9����R2DI>7

� cX� � 9���<!� 9bB34�� GDB32'�;?½A�IA?�B34DI­o0x>¡&n!l�x�<�� I�� <��<�5�b9b4DCF� ��I/�¸=�� 9;4'��I�� I�� <����m�b9b4DCÞ9b4�� � B�?�2'��I���I&Y#� �R<

�P<>4�?DB%� Bx<!�R9b<!� 9 B34è9;4$� GDI�w/�#l[l x/ 5v¸� <��<�5� I>7

� @�<xB!({�S�b<���'Q9¢I��;�b9"~$<!� 9 B34 �T<>4 9����5� B¹B>I7� GDI <��/�T2��R<��&� B�*f� GDI�I���� 9���<!� 9 B34�7 �DB��èI&YD<����5� I�=M9%GDI$4

9�I$9 C3G��WI>e%U�?DB�I��4DB��?�I/��I$4/?èB347Ç5©;Å J =Æ� 2'�RGK<��d9b4G� G�I:�T<���I�B�*�2'�R9b4DC<� GDI*B��'� 9���<�� !©;Å J 9b4½I �%U.=

� G�If�T2���� I$4���� � <!��I^Ç5©;Å J � GDB325�b?8A�I|I���� 9���<!��I$?+o.xK¡/n!l�x�9"��9;��?��R<�9�4�*,�RB%� �©;Å J =�AQ�F2'� 9b4DC

MOP��� ± KiIsÇm©;Å J U ¯W R� ?�J J � ���©;Å J M P �� ± IYKjIsÇ ©;Å J U � ª � ���© U

W R� ?�J J � ���©;Å J

¬ I>�%U

�5� B¹B�9b?DI$?W� G�<!�;MOP �� ± IYKiIsÇm©;Å J U � ª � ���© Ua�T<>4�A�IS�T<��;�T2'�b<!��I$?!I$<�� 9;�"�>7?�i4���9"YQ� 2��RIH4�B��0��<���� � <!��IM���/<���I

��B�?DI��;�¹IKcdYD<����5� I4e!<>4/?65�I��&� 9 B34C��7 �%U�<>4�?èB�� GDI/��I&Y�<����m� I��/=�� G�9���9;��9b4�?DITI$?è<��PG�9 I/Bx<!A5� I>7

� �I��b<¹�>I$?ÞI�� � 9��§<!� 9 B34EI½9¡7ÊI>7dI�� � 9���<!��I�B�*;MXP���KjIsÇ ©�8� U�<!�_� 9���I ® U�2'� 2�<������F9;�_�!B��RI�<��/�T2��P<!��I�� G�<>4

��B345�T2���� I$4��¢I���� 9���<!� 9 B34�I­I�� � 9��§<!��I M P ����� KiIsÇ ©�8� U¢<!�¿� 9��!I ®���� U.=�� 9;4'��I_� G�I�I�� � 9;��<!� 9 B34=9��}A�<���I$?

B34E��B�� I=9;4�*°B��.��<!� 9 B34�7FJ%Bg9¢I/B>I/�¹=X�'�RI��T<>2�� 9 B34 4DITI$?'�:��BFA�I8� < '>I$4E9%9�� G¦*s� I���2DI$4H�:� I�� <��<�5�b9b4DC

A�I��T<>2'� I4� I�� <����m�b9b4DC:� I$?�2'��I��%?�9�� � 9b45�&�}k�p�u[vj� <����5� I��$7

L/�

Page 59: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

����� "5)1!� � �$-�,I(+��� � � ( �&5�� �Î�GDI_� � <!��I����/<���I���B�?DI��/<��¢?DI��0�&�R9"A�I$?�9b4<cÈY�<��<�5� I_e�G�<���<:� ��I��T9b<��'hK<!��'>B¹B�9b<>4�*°I$<!� 2'� I_� G/<!�}� GDI��!B�� I

C>I$4DI/�R<��:?���4/<���9����!B�?DI�����?DB�4�B�����B%�0��I����T7dR 9"� GèC39"B>I$4@Ç � ¯ I;5j¬ � U.=5cÈY�<��<�5� Ife�� <!� 9;�)3/I��S� G�<!�

ÏaIsÇ5©;Å J � ª?© ¬�� © ¬���©;Å J U ¯ ÏaIsÇm©;Å J � Ç5©)¬�� ©;Å J U <"3�©;Å J I���©;Å J � Ç5©;Å J U;ÏaIsÇm© � � © U.°

Î�G�<!�!9���=È9%9"� G C39"B>I$4½Çm© =}�'�RI/B�9 B325�:ª?©�8 J <>4�? � © �T<>4EA�I �)*­B�� C>B�� ��I$4�7 �­`_�!9;4 <���<�����<>4+3m�"��I/�¹=È� GDI

��B%� ��I/�R9 B��!?�9;� � �R9"A/2'� 9 B34$ÏaIsÇm© � � © U^�T<>4½A�I=B�A5� <>9b4DI$?+� I��T2��0�R9"B>I��"��=�<!�<� I$<�����9;4­�5�R9b4'�T9"�m� I>77Î%GDI���<>9b4

?�9 �8�T2'�"�{�*9��?� G�<!��<>4�<������ 9��T<��#*­B��0��2'�b<��?*­B��È� G�9��?�RI��T2��0� 9"B>I�2���?�<!� 9;4DC�B345�"�*I&YD9�� �?*­B��}��I/��� <>9b4�I&YQ��B34DI$4�� 9b<��

*½<���9��"�8�!B�?�I����4IsRÞI�� ��<>4/?GJ�<!�0�R9���B34�L�N�O�N%U�B���3�4�9���Id?/9����&� I/��I&({� � <!��I:� �/<���If��B�?DI��ÈI½@�<!A/9b4DI/�4L�N�O�N%UP7

��I��T<>2'��I�B�*a� GDI���B��/2'�;<!�R9"�{��<>4/?�� 9����5�;9��T9"�{�!B�*a� GDI4� � <!��I^� �/<���I4�!B�?DI��>=���I/B>I/�R<��i��I���2�I$4H� 9b<��Th�B34���I

EH<!�0�bBG�!I/� GDB�?'��G�<¹B>I�A�ITI$4��'� B���B%��I$?V��B6?DI$<��È9�9"� GÞ4DB34'�b9;4DI$<!�[Zr4DB34#(]\|<>2'��� 9b<>4F�T<���I��$7��_4��/<!��� 9;�T2'�b<!��=

J�I$4�?���� <>4�?�@%9��PG�<!�R?ÍI)L�N�N!�HU§4�B���IC� GDIC��B���I$4H� 9b<���2'��I�B�*�� GDIÞ5#��5 9;4b� 2'�PG»��B�?DI��;�T7�RÞI�� �FI)L�N�N�e%U

� 2DC>C>I���������B 2'��IÞ<E��9"YQ� 2��RI ?/9�� � �R9"A�2�� 9 B34���B <!�5�'� B�YD9���<!��I$Ï?IsÇ5© � � © U=<!�AI$<��PG � 9��!I ® =<>4/?�� GDI$4

�'� B#��ITI$?­9%9"� GÏ<>4 <>?�<!�5� 9"B>I�9��<��B��0� <>4'��I�� <��<�5�b9b4DCC� � �R<!��ITC��V��BF�'� B�?�2'��I�<C��9"YQ� 2��RI§<!�'�'� B¹Y�9;��<!� 9 B34

B�*¿ÏaIsÇ ©;Å J � � © U�<!�<� 9;�!I ®�� Lx7��d9 ���T25�"� 9 I��W9%9"� G�� G�9���<!�'�5� B3<��RG <!� IG� G�<!��3�4�?�9b4�C�C>B�B�?»��92Y#� 2�� I

<!�'�'�RB�YD9���<!� 9 B34'�È*°B��¢I/B>I/��� ® �T<>4�A�I�� 9��!I&({��B34'�R2'��9b4DC�<>4/?�9"�}�T<>4�A�I�?/9 ���T2'�"�?��B�9����5� I���I$4H�d9%G�I$4�� GDI

?�9���I$4'� 9 B34�<��;9"�{��B�*aÇ5©�9;��G�9 C3G�7

\B��R?�B34AI/�%<��¡7�I)L�N�N��%U%<>4/?G��9"� <xC3<¹9�<7I)L�N�N��%U��'�RB���B%� I���B�2'��I�9;����B���� <>4'��I�� I�� <��<�5�b9b4DCW��B�B�A5� <>9b4

< ?�9��0�&� I/��IÞ<!�'�'� B¹Y�9��§<!� 9 B34�B�*�ÏaIsÇm©;Å J � � ©;Å J U.=�9%9"� G < C39"B>I$4 ��I/�èB�*:� <����5� I��è?'�R<�9%4�*s� B%� ÏaIsÇm© � � © UP7

Î�GDI/�C�T<����j�R2'�RG <��'� B#��I$?�2�� I�o�n�n�vsu&vKl)p[k��}r¤v{x/l%B��_k#p!l&vKqKw�r"x��Èr2v{x&l�7�Î�GDI:�!I/� GDB�?KG/<��_A�ITI$47� 2'�/��I�����*­2'�;�"�

<!�'�5�;9 I$?���B��*25�"� 9"�5� I�� <!�RC>I/�¿� �P<���'�9b4DC�I§\B��R?DB34=I/�H<��¡7�L�N�N��Q='`MB�9"� ~TB32���L�N�N��%U¢<>4�?�� 9��!I���I/�R9 I��¢<>4�<��"�#� 9��

IK��9"� <xC3<¹9�<#=�L�N�N��%UP7$Î�GDI$9"���!I/� G�B�?Y9;�dI�����I$4�� 9b<����"� <>4�5Q� 5C9%9"� G !©;Å J �RGDB%� I$4Ï<���I>�%U|<>4�?�� I�� <��<�5�b9b4DC

<!��I/B>I/��� ® 7<c}� � 9��§<!� 9 B34'��9�I/� I���I/��*°B��.�!I$?Ep)¡[v{x/l�� I�� <����5�;9b4DC�=Æ9%G�9��PGÞ9���� I����dI��8�T9 I$4H�T7<J �2���~TI��bI/��<>4�?

L�L

Page 60: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

���2�45���RG¦I)L�N�N��%U�<>4�? ��9"� �%<>4/?K5�GDI/��G�<!�R?VI)L�N�N%��U%G�<�B>I:�'� B���B%��I$?A9;���'� BgB>I$?A<�� C>B��R9"� G5�<��*­B��M� GDI:� � <!��I

� �/<���I^�!B�?�I��¡7?RÞI*?�9��0�T2'���S� GDI$9"��<!�5�'� B3<��RG�I��%9b4è?DI/� <>9;��9b465�I��&� 9 B34 ��7

� ��� � � � � ���� � � � � �F� � � ��� � & ��� � � � � �

`��49�I�G�<¹B>I=?�9;���T2'����I$?Þ9;4�5�I��&� 9 B34½e�7 �Q=z<G*½<�B>B��R<!A5�bI��RG�B39���I§B�*�� GDI�� I��T2��0� 9�B>I�� <����5�;9b4DCA?�9���� �R9"A/2�� 9bB34

9���!©;Å J IsÇ5©;Å J � ª?©«U ¯ ¨5©;Å J IsÇm©;Å J � ª?©«UP7�J�B¹9�I/B>I/��=�?��R<¹9%9b4DC�Çm©;Å J *,� B%�-¨'©;Å J IsÇ5©;Å J � ª?©«U��§<��Þ4DB��fA�I

?�9"�RI��&���"�A<��RG�9bI/Br<!A5�bI�<>4/?C� G�I�9;4'�&� I��!I$4�� <��i9�I$9 C3G�� � ©;Å J ��<��K4DB���A�I*I$<�� �C��B���B%���/2���I>7 �4�?DI/�:u[��w/¾

pfkml�x&|Wq,u/x&=�<���B%�;� I��&� 9 B34AB�*d�!I/� G�B�?'��G�<�B>I^A�ITI$46?DI/B>I�� B���I$?@��B§B¹B>I/�.��B%�!IW� GDI�?�9 �8�T2'�"�{�8*­B��M� GDI:� � <!��I

� �/<���I$�!B�?DI��¡7K5�ITI�=d*°B���I&Y�<��<�5� I�=È��I/��~$2�9b4/9zI/�!<��¡7­I)L�N�N%��U.=XJ �2���~TI��bI/��<>4�?+���2�45���RGiI)L�N�N��%U.=¢<>4�? �¢9"� �

<>4�?Þ5�GDI/�/G/<!�R?­I)L�N�N%��UP74RÞIW�'�RB���B%� I*GDI/� I^��BFI&YQ��I$4�?7� GDI$9����!I/� G�B�?'�_��B=B32��dC>I$4DI/�R<���5Q� 5���I/� � 9b4�C�*­B��

� 9���2'�"� <>4�ITB32'���"�èI�� � 9;��<!� 9b4DC�� GDI!4DI/9 9¢I$9 C3G���J ©;Å J <>4�?Þ?��R<¹9%9b4DC$Ç ©;Å J 7^RÞI<�RI/*°I/����BG� G�I���I<�!I/� GDB�?'�

<�� � �bBQ�T<��¸h�B34���I�EH<!�0�bBW��I/� GDB�?5� �W*°B��|5Q� 5�7

����� �������E, �2*<9 / ����,`���25� 2�<��>=d9�I8�bI/� �Æ© ¯ � ª � ���© ¬�� ¯ L!¬/°/°/°¹¬! #"A<>4�? �7© ¯ � J � ���© ¬�� ¯ L!¬/°/°/°g¬! #"37¦Î�GDI$��I$4�� �R<��H9b?DI$<

B�*d� G�9��_��I��&� 9 B34K9�����B�� ITC3<!�R?7¨'©�<��_A�I$9b4DC<�RI/�'� I���I$4���I$?@AQ�$� GDI^h�B34���I�E�<!�0� B�� <��<�5� I ��©}9%9"� GG9�I$9 C3G����

�7©_7CÎ%G�2'��=�<!�W� � <xC>I ® � L!=Ȫ?©_�T<>4­A�I�� � I$<!��I$? <���<@�R<>4/?DB%� Bx<!�R9b<!A5�bI�9%9"� G¦� G�9��*?�9����&� I/��IFp�kml[qKn�l.q

?�9���� �R9"A/2�� 9bB34�7�Î:B+� 9����m�b9"*s�Ï4DB�� <!� 9 B34'��=�9¢IK9b4H� �RB�?�25��IA<V�R<>4�?�B%� Bx<!�R9b<!A5� I��d=�9%G�BV� < '>I���Bx<��b2DI���9b4

� GDIG��I/� � L!¬/°/°/°�¬! #"%=}��BY9b4/?�9��T<!��I$� GDI�� � � I$<��<�§9b4 ��©_7 �¢9"� �!<>4�? 5�GDI/�/G�<!�P?�I)L�N�N%��U!<�����BY2'��I�� 25�RG <

*­B��0�*25�b<!� 9 B34T=D<>4�?G�T<��;���� GDI�<>2#YD9��b9b<!�0�<Br<!�P9b<!A5� I>7

8�I/�M� GDI¿D_B39;4H�%?/9�� � �R9"A�2�� 9 B34�B�*�Y<>4�?�Çm©;Å J A�I

ÏaI���¬ Ç ©;Å J U <¨5©;Å J IKª � � �© ¬ Ç5©;Å J U

¨'©0IKª � � �© UJ ��� �© ° I§��U

L�e

Page 61: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Î�GDI$4$� G�If��<!� C39b4/<���?�9�� � �R9�A/2�� 9 B34�B�*jÇm©;Å J *,�RB%� I§��U�9��

¨'©;Å J IsÇ5©;Å J U <R�� ?�J

¨5©;Å J IsÇ5©;Å J ¬�ª � ���© U¨'©�IKª � ���© U

J � ���© ¬ I>O%U

9%G�9;�RG+9¢B325�b?+A�IF<KC>B�B�? <!�'�'�RB�YD9���<!� 9 B34+��B�� GDI$� �R2DI���<!�RC39b4�<��H?�9�� � �P9"A/2�� 9 B34¦¨ ©;Å J IsÇ ©;Å J Uf�'�RB¹B�9b?DI$?

� G�<!��� GDI�h�B34H��IFEH<!�0� B$� <����5�bIW�R9"~TI 9;���;<!� C>I!<>4�?F� GDI!?�9�� � �P9"A/2�� 9 B346B�*}� GDI J�©H9��|4DB�����B�BG��'>I/9�I$?�7

Î�GDI4�§<!� C39b4�<���?�9;� � �R9"A/2'� 9 B34§B�* �Ï9��

��I�� ¯ �QU <4J � ���© � ¨'©;Å J IKª � ���© ¬ Çm©;Å J U¨'©0IKª � ���© U

Ë%Çm©;Å J ¯ J � ���© ¨'©;Å J IKª � ���© U¨'©0IKª � ���© U

¯ J � ���© � � ���©;Å J ¯ J � ���©;Å J ¬

9%G�9;�RGè9���I&YD<��&���"�$� GDI�4DI/9 9¢I$9 C3G��%<!��� 9;�!I ® � L�*°B��_ª � ���© <��/��B��R?�9;4DC���BCI>e%U�<>4/?VIs��UP7

J�I$4'��I�=D9"*T9¢IdG�<¹B>Id<W�!I/� G�B�?���B�?'�R<�9�<:� <����5� I�=TI � J ¬ Ç � � ± �©;Å J U.¬/°/°/°g¬¹I ���¹¬ Ç � ��� �©;Å J U.=�B�*XI���¬ Çm©;Å J Ud*s� B%� I§��U.=

� GDI$4�� G�I�5Q� 58� ��I/�@�T<>4GA�I�<��PG�9 I/B>I$?GAQ�

IK��U�cX� � 9���<!��I�J � ���©;Å J AQ� 3 � ¯ *s� I���2�I$4'�&��B�* � � ¯ � "�9b4$� GDI4�R<����5� I>7

IK`�U ��B��0� ª � ���©;Å J ¯ IKª � ���© ¬ Ç �©;Å J U�9"*3 ���¯ �#=d9%GDI/� I�Ç �©;Å J 9���<>4H�VBx<��b2DI�B�*�Ç5©;Å J � G�<!��9��^�/<>9"� I$?¦9%9"� G

� ¯ �§9b4$� G�I4� <����5� I>7

5�I/B>I/�R<��Æ�!I/� G�B�?'�X*­B��¢C>I$4DI/�P<!� 9b4DC�� <����5�bI��È*s� B%� I§��U�<!�RI?DI��0�&�R9"A�I$?�9b4�� GDI�*°B%���bB¹9%9b4�Cf� 2�A5� I��&� 9 B34'�/=�<>4�?

<�*­I/9 � I���<!�('Q��<!�RI�<���*­B%��� B¹9_�T7

��x/|�p�l �%ÁX`_�S� B34�C�<��S� GDII�� � 9��§<!��I��HB�*i� GDI�9¢I$9bC3GH����<!� I|2�4�A/9b<�� I$?T=H� GDId4DI/9 � <����m� I9;�}�'�RB���I/�.�"�

9�I$9 C3GH��I$?+AQ� 3 � 9�9"� GV� I�����I��&�^��B7¨5©;Å J 7@`�4 <��/�T2��R<!��IFI�� � 9;��<!� 9 B34�B�*�� GDI�9�I$9 C3G�����9;�< mn�v�4DI���I����R<!���>7

Î�GDB%� Ifª � ���© 9�9"� G 3 � ¯ ���T<>4@A�I4� I/�m�b<���I$?�AQ�A<��R<>4�?DB%� ?��R<¹9 *s� B%��� GDB%� If9%9"� G 3 ���¯ ��7¿�]*�B�*�9b4���I/� I�� �

9��M� GDI�I�� � 9;��<!� 9 B34�B�*d� GDI*9b45�&� I��!I$4�� <��i9�I$9 C3G�� � � ���©;Å J =�B34DI:�T<>47� I/�XJ:I��dU 9 L^*­B���� � ��©¢9;4�� GDI�<!A�BgB>I

�T<����T2'�;<!� 9 B34'�T7

��x/|�p�l ����Á�5D9b4'��I:� GDI<� B#�T<��ahÞEÍ�!I/� GDB�?'���'� BgB�9;?DI:�R<����5� I��B�*�I��#¬ Çm©;Å J UM9%9"� G ?�9�� � �P9"A/2�� 9 B34+I§��U.=

� GDI/�è<��RG/9 I/B>I4� I�� <��<�5�b9b4DC�I&1�I��&�%<>2���B%��<!� 9;�T<����"�>7�5�ITI*?DI/� <>9�����9b4K5�I��&� 9 B34��D7

L �

Page 62: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

��x/|�p�l � � Á �%B34�I�B�*m� GDI���I/� GDB�?5�z?DI����&�R9"A�I$?�9b4W� G�9��?��I��&� 9bB34�<!� I�4DI���I����R<!���^9�GDI$4!?�9"� I��&�d� <��<�5�b9b4DC

*s� B%� � GDI|B��'� 9;��<�� !©;Å J B�*�I �%U�9���<��RG/9 I/Br<!Am� I>7

��� � � ���2�$9�( * 5 ' � ��(+�&5 � �5�2��5��B%� I:9�I��T<>4�?��R<¹9 Ç5©;Å J *,� B%�,<�� �R9b<���?�9�� � �R9�A/2�� 9 B34 �©;Å J IsÇ5©;Å J � ª?©{U�9%G/9��RGY9;��4DB���I���2�<��È��B½I �%UP7

Î�GDI/�RI*<!� I:�{9�B$� I]D_I��&� 9bB347��I/� GDB�?5�M��B$� <����5�bI8I���¬ Ç5©;Å J U�*,�RB%� I§��U.��B34DI*9��_A/<���I$?�B347� GDIMD_B39b4���?/9�� � �R92(

A/2�� 9bB34ÞB�*4I��#¬ Çm©;Å J U|<>4�?V� GDI�B�� GDI/�:A/<���I$?ÞB34¦� GDI��§<!� C39b4�<���B�*SÇ5©;Å J 7F8�I/�f� GDI � ��BgB>I/�R9b4DC7��B34'� � <>4�� �

A�I

� ©;Å J ¯ � 2����� � �� ±

¨'©;Å J IKª�� ���© ¬ Çm©;Å J U¨'©�IKª � ���© U� �©;Å J IsÇ5©;Å J � ª � ���© U

°

�8���¹��¶����,����µ¦��� Hj�T² ��

� ���R<¹9 � ¯ ��9%9"� G$�5� B�A/<!A/9��;9"�{���'�RB���B��0� 9 B34�<��Æ��B J � ���© P

� \|9"B>I$4 � ¯ �'=/?��R<¹9 Ç5©;Å J *s� B%� �©;Å J IsÇ5©;Å J � ª � ���© U.P

� `��/��I/�'�WI ��¬ Ç5©;Å J U¿9%9"� G$�'�RB�A/<!A/9��b9��{�

Ï ¯ ¨5©;Å J IKª � ���© ¬ Ç5©;Å J U�[©;Å J ¨5©0IKª � ���© U� �©;Å J IsÇ5©;Å J � ª � ���© U

°

�8���¹��¶����,����µ¦��� Hj�T² )+��Î�G�I�35�0� ���«9¢B�����I/�5�%<!� I�9;?DI$4H� 9;�T<��T��B�h�I/� GDB�?¦Lx7È�i4K5Q��I/� �Q�

� `��/��I/�'�_Ç5©;Å J 9%9"� G$�5� B�A/<!A/9��;9"�{�

Ï ¯W R� ?�J J � ���© ¨ ©;Å J IKª � ���© ¬ Ç ©;Å J U � ¨ © IKª � ���© U�[©;Å J W R� ?�J J � ���© �©;Å J IsÇ5©;Å J � ª � ���© U

Î�GDI$4C� GDI:� <����5� I^Ç ©;Å J <��/��I/�'��I$?F*,�RB%� 2'�R9b4DC�I$9"� G�I/��B�*È� G�I:�!I/� G�B�?'�_*­B%��� B¹9_�^I>O%UP7��i47�!I/� GDB�?FeQ=m9�I

4DITI$?���B�� I$?��R<¹9 �V9%9"� G$�'�RB�A/<!A/9��b9��{�

��I�� ¯ � � Ç ©;Å J U <¨5©;Å J IKª�� ���© ¬ Ç5©;Å J U

¨'©�IKª � ���© UJ � ���© ° I>N%U

L&�

Page 63: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

h�I/� GDB�?5�VLÞ<>4�? e <!� I�9b?DI$4�� 9��T<��|9b4�� GDIV� � <!��I+� �/<���IV�!B�?DI����T<���IÏ<>4/? <!� IY<>4�I���� I$4H� 9b<��4�/<!���

B�*�J��2��0~TI�� I/��<>4�?E���2�45���RG I)L�N�N��%UP7�\dI$4DI/�R<����"��=¿�!I/� GDB�?be 9��!<Þ@�<xB!({�¿�;<���'Q9¢I����;9"~$<!� 9 B34 B�*��!I/� GDB�? L

I{EH<���I��;�b<!<>4�?A@%B�A�I/�0�/=¸L�N�N��%U�<>4�?��T<>4@A�I4�!B�� I|I����T9 I$4��T7

����� / 1436587 ( ,.';9:� � � �2,.143;-�*<'�=�)����B���� <>4'��I�� I�� <��<�5�b9b4DCW�!I/� GDB�?G�T<>4�<�����B�A�I�2'��I$?���B§C>I$4DI/�R<!��I*<!�5�'� B�YD9���<!��I4�R<����5� I��S*s� B%� I§��UP7

� ���R<¹9 � ¯ ��9%9"� G$�5� B�A/<!A/9��;9"�{���'�RB���B��0� 9 B34�<��Æ��B J � ���© P

� \|9"B>I$4 � ¯ �'=/?��R<¹9 Ç ©;Å J *s� B%����B%��I� �©;Å J IsÇ ©;Å J � ª � ���© U.P

� `���� 9bC34���B<� GDI4� <��<�5� I�I ��¬ Ç ©;Å J U¿� GDI�9�I$9 C3G��

J:I ��¬ Çm©;Å J U ¯ ¨'©;Å J IKª � ���© ¬ Çm©;Å J U¨'©�IKª � ���© U� �©;Å J IsÇ5©;Å J � ª � ���© U

°

Î�GDI%B�A'� <>9b4DI$?<�R<����5� I^I �%¬ Ç5©;Å J Uz9��d�'� B���I/�0���f9�I$9 C3GH��I$?�AQ�:� G�IOJ:I �%¬ Ç5©;Å J Ud9%9"� GW� I�����I��&�X��B�I§��UP7}`S�X� G�9��

��B39b4��/=�B34DI§G�<��4� G�� ITI<�RG�B39���I��/�8I½<HU|?DB�� I�� <����m�b9b4DC$��B6<��PG�9 I/B>I7I§��U|<!�'�'� B¹Y�9��§<!��I��"��=�<���9;���5� I��!I$4���I$?

9b4E\B��R?DB34 I/�!<��¡7bI)L�N�N��%U!<>4�?½��9"� <xC3<�9H<bI)L�N�N��%U.PfIsA�U�I�� � 9;��<!��I ��I�� ¯ ��U < J � ���©;Å J ?�9"� I��&���"�Y25� 9b4DC

� GDI:9¢I$9bC3GH��I$?F� <����m� I�B�*_I �%¬ Ç ©;Å J U.P�B��<IK�¹U��5� BQ��ITI$?79%9�� GG� GDI�4DI/9M�"��� <��<�5� I$?­IKª � ���© ¬ Ç ©;Å J U^Is9%9"� G�4DI/9

9�I$9 C3GH���XJ:I �%¬ Ç ©;Å J U U.=�<����'� B���B%��I$?$AQ� ��9"� ��<>4�?K5�GDI/�/G�<!�P?FI)L�N�N%��UP7

�_4Ï<>?�?�9�� 9 B34T= �¢9"� ��<>4�? 5�GDI/�/G/<!�R?bI)L�N�N%��U^� 2DC>C>I�� ��2'� 9;4DCA<>4�<>?!D�2'�����!I$4H�^�*2'��� 9"�5�b9 I/����BK9����'�RB¹B>I

I����T9 I$45�&�>7@�¿�R9 I�5��=gB34DI��T<>4 9b4'����I$<>?Ï?��R<¹9 � ¯ �79%9"� G��'�RB�A/<!A/9��b9��{�7�'� B���B���� 9 B34�<��?��B J � ���© �� ���©;Å J <>4�?

� GDI$4F<>?�D 2'� �S� GDI�9�I$9 C3GH��<��/��B��R?/9b4DC%�"�>7d�]��9��X��B345��I$9"Br<!Am� I�� G�<!��A����T<!� I/*­25���"���PGDB�B%�R9b4DC�� ���©;Å J I½<:*­2/4'�&� 9 B34

B�*_ª � ���© U*<>4�? �©;Å J B34DIG�T<>4 <��PG�9 I/B>IèC>B�B�? I����T9bI$4'�&�>7+Î%G�9���9b?DI$<7�T<>4 <�����BFA�I=<!�5�5�b9 I$?­��B�� I]D�I��&� 9 B34

� <����m�b9b4DC�<>4�?�� GDIf*°B%���bB¹9%9b4�CWhÞEShÞE <!�'�'�RB3<��RG�7

L��

Page 64: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

����� � , � ( *�'&= �L/2' ���$36�$' �&�$'&9:� � �&,.*�'�� 3&3;7 58,.9 �`��"��I/�R4�<!� 9"B>I�����=�B34DI:�T<>4 <�����B�25��I^J<�� � 9b4DC%�^qs m¥Hx«k'x& m¥Hx/ mw.x�w&¾#p!q, è<!�'�5� B3<��RGEIKJ<�� � 9b4DC%�:L�N%���HU.=�<���� 2DC!(

C>I�� ��I$?FAQ�C��I/��~$2�9;4�9�I/�|<��¡7�I)L�N�N%��U�*­B���� GDI�� � <!��I�� �/<���I��!B�?DI��¡7�J�I/� IW9¢IW�'� I��0�&�R9"A�I�<=C>I$4DI/�R<��b9�~$<!� 9 B34

B�*S� GDI$9"�f��I/� GDB�?V*°B���?���4/<���9��W� �#� ��I��<�T78��I/� <>9;� I$?Ï?DI��0�&�R9"�'� 9 B34 B�*S� GDI§C>I$4DI/�R<���9;4�?DI/��I$4�?DI$4'��IW�RG/<>9b4

�!I/� GDB�?A9��HC39�B>I$469b4$� GDI�<!�5��I$4/?�92Y�7

5�2��5��B%� I�9¢I:�T<>4�?��P<�9 B�©;Å J *,�RB%� <<� �P9b<���?�9���� �R9"A/2�� 9bB34� �©;Å J IsÇ5©;Å J � ª?©«UP7�Î%GDI$4G� � <!��� 9;4DC�9%9�� GA<>4

<!��A/9�� �R<!��� � � ¯ � �!=�9¢I*9"��I/�R<!��I4� GDIf*°B%���bB¹9%9b4�CW����I/�5�/�

� ���R<¹9 � ¯ � �Æ9%9"� G$�'� B�A�<!A/9��b9"�«�<�'� B���B���� 9bB34�<�����B J � ��� �© 7

� ���R<¹9B�©;Å J ¯ Ç �©;Å J *s� B%� �©;Å J IsÇ5©;Å J � ª � � � �© U�B���*s� B%� <$� I/B>I/�0� 9�A5� IWh EShÞE� ��I/��9�9"� G �©;Å J I # �

ª � � � �© UH<��%9"����9b4�Br<!�P9b<>4H��?/9�� � �R9"A�2�� 9 B34CIK� ITIf� GDI��'�RB�B�*gB�*g9�������B���� I��&� 4DI��0�%9b4�`_�'��I$4�?�92Y5UP7

� 5�I/��I�� /Å J ¬ Ç /Å J©;Å J UzI���2�<�����BGI � �K¬ Ç �©;Å J U}9%9�� G��'�RB�A/<!A/9��b9��{�_Ï���=D<>4�?=I���2/<��5��BGI�� ¬ Ç ©;Å J U}9%9"� G��'� B�A�(

<!A�9��b9"�«�CL � Ï���='9%GDI/�RIÏ � ¯ ��9b4

� L!¬ ÏaI � �s¬ Ç �©;Å J U<J ��� � �© �©;Å J IsÇ ©;Å J � ª � �

��© U

ÏaI�� ¬ Ç ©;Å J U<J � � � �© �©;Å J IsÇ �©;Å J � ª � � � �© U

� � ¬

9�GDI/� ISÏaI���¬ Ç5©;Å J U�9���?DI&3�4�I$?è9b4VI§��UP7

Î�GDI:� I�� 25�"� 9b4DC=I���2/9��b9"A'�P9b2'� ?�9���� �R9"A/2�� 9bB34èB�*�I���¬ Ç5©;Å J U%9;��I&YD<��&���"�EI§��UP7:Î%GDITB�� I/� 9��T<��a�'� B���I/��� 9bI���B�*X� GDI

J<�� � 9b4DC%� � �RG�<>9b4�<!� IM� � 2/?�9 I$?!9b4�8�9b2GI)L�N�N��%U}9%GDB4� G�B¹9M�}� G�<!�È� G�9��}�!I/� GDB�?§9��d��B%����<!�R<!A5� I���B^� I]D�I��&� 9 B34

�!I/� GDB�?F9b4<��I/�.�<�HB�*i� � <!� 9���� 9��T<���I����T9 I$4'�&�>7ÈÎ�G�I���I���B34/?��RI]D_I��&� 9 B34���I/� GDB�?=?DI����&�P9"A�I$?§9b4�� GDI��'� I/B�9 B32'�

� 2�Am��I��&� 9 B34��T<>46<��;��B�� < '>If� G�9;��h E�h E��«9%9�� �T7}�«����?�I/� <>9���9���B%��9�� ��I$?AGDI/� I>7

Î�GDIè<>?�Bx<>4�� <xC>I��§B�*�� I]D�I��&� 9 B34��!I/� GDB�?'�!<!� I@� G�<!��4DBY9"��I/�R<!� 9bB34'�!<!� I64DITI$?�I$? <>4�?E� GDI�� I��R2'�"� 9b4DC

� <����m� I�9����_I&YD<��&�/= �W9%GDI/� I$<��d� G�I%?�9�� <>?'Br<>4�� <xC>I%9��d� G/<!� �[©;Å J 4DITI$?'�d��BfA�I���B%�<�/2���I$?§<>4/?�� GDI�� I��R2'�"� 9b4DC

���PGDI��!IF�T<>4bA�I@B>I/�0� 9b4DI����T9 I$4��T7 8�9b2 I)L�N�N��%U��'� BgB�9;?DI����!B��RIK?DI/� <>9;� I$?b��B%����<!�R9���B34'�§B�*�� GDI7� G�� ITI

L��

Page 65: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

�!I/� GDB�?'�T7a`�4�9b4���I/� I�� � 9b4�C�Br<!�P9b<!� 9 B34�9;�¸��B���B%�fA/9;4DIÈ� I]D�I��&� 9 B34�<>4�?�9��<��B��0� <>4'��I}� <����m�b9b4DC%��<��a� 2DC>C>I�����I$?

AQ��8�9b2T=\EHGDI$4!<>4�?<RÞB34DC�I)L�N�N%��UP7ÈR GDI$4�� GDI�?/921�I/�RI$4'��ISA�I/�«9¢ITI$4�¨'©;Å J IsÇ5©;Å J � ª?©«Ug<>4�? �©;Å J IsÇm©;Å J � ª?©«U

9����;<!� C>I�=/4DB34DI�B�*a� G�I^�!I/� GDB�?'��9;��9b?DI$<��¡7XÎ:B=<���� I/B�9b<!��I�� GDI4�'�RB�A5� I�� *­B��_� � <!��I:� �/<���If��B�?DI��§=5J��2'��~TI�� I/�

<>4�?7���2�4'�0�RG­I)L�N�N��%U_�5� B���B%��I^��B%��I:����B�B�� G�9;4DC<��I��RG/4�9���2DI���<>4/? �¢9"� ��<>4�?Þ5�GDI/��G�<!�R?¦I)L�N�N%��U�� 2�C>C>I�� �

2'� 9;4DC��!B�?�I�<!�'�'� B¹Y�9��§<!� 9 B34$��B�3�4�?è<�C>B�B�?A<>?�D 2'� ���!I$4�����2'�"� 9"�5�;9 I/�$7

����� / -<-�% � (+7 , ( * 5)' � * (+� ( ���! )(+, (+� �3&,.9:� � 5 ���$-

5�2��5��B%� I�B34DI!9��9b4H��I/�RI�� ��I$? 9b46I���� 9���<!� 9b4DC�� GDI�� � <!��I�� �/<���I�� 9 C34/<��¸Ç © n! ­rtqs �xWIKcdYD<����5�bI:e%UM9%9"� GC� GDI

�/<!�R<���I/��I/�0� � <>4/? 5ÏC39"B>I$4�7 ��B��4� 9����5�;9��T9"�{��=m9¢I�� 2'�'�'� I���� 5Y<>4/? � 9b4 <����j� I�� I/Bx<>4H��*­B��0��2'�b<��T7�Î�G�2'�/=

� GDI�?'��4�<��§9���� �Q����I�� *°B��M� GDIf� � <!��I:� �/<���If�!B�?DI���9���¨5©0IKª?©«U&< �©>@?�J 3 > I�� > � Ç > UA6 > IsÇ > � Ç > 8 J U.¬/<>4�?

¨5©;Å J IsÇ5©;Å J � ª?©«U < ¨5©;Å J IsÇ5©;Å J ¬�ªd©{U¨'©�IKª?©«U ¯ 3�©;Å J I���©;Å J � Çm©;Å J UA6/©;Å J IsÇ5©;Å J � Çm©{U.°

`��"� GDB32DC3G<�R<����5�b9b4�C_*s� B%� ¨5©;Å J IsÇ5©;Å J � ªa©«U?�T<>4�A�I�?�9 ���T2'�"�/=>B34DI_�T<>4§2'� 2/<����"��?��P<�9�*s� B%� � GDI_� � <!��I

I���2�<!� 9 B34 �©;Å J IsÇm©;Å J � ª?©«U ¯ 6/©;Å J IsÇm©;Å J � Ç5©{U�I$<�� 9����>7�@%I]D_I��&� 9bB34��!I/� GDB�?'��L<>4/?8e�<!� I|9b?DI$4�� 9��T<���9b4�� G�9��

� 9"� 2/<!� 9 B34�7�8�I/� �.©;Å J ¯ � 2����� ± 3�©;Å J I���©;Å J � Ç5©;Å J UP7}Î�G�I��'� B#��I$?�2�� I|9��

� ���R<¹9 � ¯ ��9%9"� G$�5� B�A/<!A/9��;9"�{���'�RB���B��0� 9 B34�<��Æ��B J � ���© =�� GDI$4A?'�R<�9 Çm©;Å J *,�RB%� 6/©;Å J IsÇ5©;Å J � Ç � ���© UP7

� `��/��I/�'�WI��#¬ Çm©;Å J U¿9%9"� G$�5� B�A/<!A/9��;9"�{��Ï ¯ 3�©;Å J I���©;Å J � Ç5©;Å J U � �[©;Å J

`����Æ� GDIf� <��<�5� I��HB�*�I��#¬ Ç ©;Å J UH?��P<�9%4$*s� B%� � G�9;�����RGDI���If*­B%��� B¹9_�S� GDI�?�9���� �R9"A/2�� 9bB34

ÏaI�� ¯ �%¬ Ç5©;Å J U&<4J � ���© 3�©;Å J I���©;Å J � Çm©;Å J UA6/©;Å J IsÇ5©;Å J � Ç � ���© U.°

5�9��§9��b<!�0�"��=¿9%9"� G �©;Å J ¯ 6/©;Å J IsÇ5©;Å J � Ç5©{U.=M� GDI 9�����B���� <>45��I@� I�� <��<�5�b9b4DC¦�'� B#��I$?�2�� IGA�I���B%�!I����»I½9,U

?��R<¹9 � ¯ �V9%9"� G­�'� B�A/<!A�9��b9"�«���'� B���B���� 9 B34�<��X��B J � ���© =_I½9b9,U�?��R<¹9 Ç ©;Å J *,�RB%� 6 ©;Å J IsÇ ©;Å J � Ç � ���© U.=�<>4�?

L¹�

Page 66: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

I½9b9b9sU�<���� 9bC34@9�I$9 C3G�����B$� GDI�� <��<�5� I8I �%¬ Ç5©;Å J U�<�� 3�©;Å J I���©;Å J � Ç5©;Å J UP74Î%GDIf�'�RBQ��I$?�2'� I�B�*�\B��R?DB34KI/�|<��¡7

I)L�N�N��%U�<>4�?<��9"� <xC3<¹9�<8I)L�N�N��%U�9;�zI&Y�<��&�����:� G�I%<!A�B¹B>I�=Q9%9"� G�<>4�<>?/?�9"� 9 B34�<�������I/��B�*Æ� I�� <����5�;9b4DC�*,�RB%� � GDI

B�A'� <>9b4�I$?@� <����m� I�2'� 9b4�C�� GDI�<���� 9bC34DI$?G9�I$9 C3G��T7¿�_46<>?/?�9"� 9 B34$��B�� GDI*2'� I�B�* �©;Å J ¯ 6 ©;Å J IsÇ ©;Å J � Ç © U.= �¢9"� �

<>4�?A5�GDI/��G�<!�R?CI)L�N�N%��U�9b4'��B�����B��R<!��Id<>4F<>?�D�25� ���!I$4��S��2'�"� 9��5�b9 I/��� ���©;Å J ¯ 3 ©;Å J I�� ©;Å J � � � ���©;Å J U.=#9%GDI/� I � � ���©;Å J

�T<>4�A�IW�!B�?�I�=¸�!I$<>4T=�B��|B�� GDI/�^�b9 '>I��"�GBx<��b2DI�B�*¿Ç � ���©;Å J 7^Î%G�2'��=�� G�I�� I�� 2'�"� 9;4DC�9�I$9 C3G���*°B���� GDI!B�A5� <>9b4DI$?

� <����m� I�9��TJ:I �%¬ Ç5©;Å J U ¯ 3�©;Å J I���©;Å J � Ç � ���©;Å J U � 3�©;Å J I���©;Å J � � � ���©;Å J U.°

�_4G� GDI�9b4�?DI/��I$4�?�I$4'��I4�RG�<>9;46<!�'�'�RB3<��RG79%9"� G�� G�I:� <���I �©;Å J <��<!A�B¹B>I�=Æ� GDIf�RI]D_I��&� 9 B34C�'� B�A�<!A/9��b9"�«�

�T<>4GA�I���B%���/2'��I$?6<��

Ï � ¯ ��9b4� L!¬ 3 ©;Å J I�� ©;Å J � Ç �©;Å J U<J � � � �©

3�©;Å J I���©;Å J � Ç ©;Å J U<J � � � �©

� � ¬

<>4�?�� G�I�� I�� �M�T<>4@A�I��T<!���P9 I$?èB32���� B32'� 9b4DI��"�>7

� � � � � � �@��� � � � & � � � � � � � � �� ��� � �'��� �

�_44��<>4��|I$<!�0�"��9¢B���'|B34^h6B34���IHE�<!�0� BM�!I/� G�B�?'�¸*°B��j� GDIX� � <!��I¿� �/<���I¿�!B�?�I��>=¹� I��R<����5�b9b4�CHG/<��¸�5�b<¹�>I$?

<���<gD�B��a�RB%� I�9;4�I/B>B%�"B�9b4DC�� GDI�� �#� ��I��*s� B%� � 9;�!I ® ��B ® � L�I­I>7ÊCD7È\dB��R?DB34�I/�z<��¡7SL�N�N��QPQ��9�� <xC3<�9H<�L�N�N��%UP7

�_4�� G�9�����I��&� 9bB34�9¢I*?DI����&�R9"A�I��{9�B��RI�� <����5�b9;4DC:��I/� GDB�?5�/=�?�9����T25���S��B%��� 9"A5�bIM� I�� <��<�5�b9b4DC����PGDI$?�2'�bI��/=�<>4�?

� GDI$4��5� I����&�R9"A�Id<!C>I$4DI/�R9;�4h6B34���I�EH<!�.� B�<�� C>B��R9"� G5� *°B��?���4/<���9���� �#� ��I��<�T7

����� � � �2,.1!3 -�*<'�=!14� ( �&5�� ���� � � � � �KµD� �s� ��´5�j²a�mµ � ´mµD� �K�s�)F � �_4�� G/9��}�5� BQ��I$?/2�� I�=\B34�I�� <��<�5� I��X*,� B%� ��©j9%9"� G<�RI/�5�b<���I��!I$4��

9%9"� G@�'� B�A/<!A�9��b9"�«�8�5� B���B���� 9 B34�<��T��B$� GDI:9�I$9 C3GH���d9b4 �7©_78�9b26<>4/? EHG�I$4+I)L�N�N��%U2'� I^� G�9;�%<!�'�'�RB3<��RG7��B

�!B�?�9"*s��� GDI4��'>I/9¢I$?�9�����B���� <>4'��I�9�I$9 C3G������ I�� 2'��� 9b4DCW*,�RB%� � GDI*5Q� 5�7

�_4ÞC>I$4DI/�R<��>=�<�� I�� <����m�b9b4DC$� ��I/��9;�|9b4'��I/����I$?FA�I/�«9¢ITI$4¦�{9�BK5Q� 5F� ��I/�5�T7<�H2���9�GDI$4�� GDI�9�I$9 C3G���I>e%U

?DB�I��§4DB��=?DI/��I$4�? B34»Ç5©;Å J I½9¡7ÊI>7"=�9�GDI$4�� <����m�b9b4DCÞ?�9���� �R9"A/2�� 9bB34�I �%U§9���25��I$?mU.=S� GDI@�RI�� <����5�b9;4DC�� ��I/�

L�O

Page 67: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

� GDB325�b?GA�I�9b4'��I/����I$?­qs �u.q>¥Hxd<è5Q� 5@����I/��7�5Q��I��T923m�T<����"��=�<>4 B��'� 9���<��z5Q� 5�@��B34'� 9�������B�*]�d5#��5@� ��I/��IK��U �� I�� <��<�5�b9b4DCW� ��I/� � 5Q� 58� ��I/�­IK`�UP7mÎ�G�9;�¢C>I$4�I/�R<!��I��_��B�� I�?�9�� � 9;4'�&��� <����5� I���B�*jÇm©;Å J � G�<>4���I/��*­B��0��9b4DC

� I�� <��<�5�b9b4DC�p)¡&v{x&l%5Q� 58� ��I/�5�:IK`�UH<>4/?¦IK��UP7

��� � � ) �8��·¹�s² � ´ � � ´5µ����K�K�)F � Î�GDI�*­B%��� Bg9%9b4DC����PGDI��!If�T<>4@� I/�5�b<���I�� GDI4�R9����5� I��R<>4/?DB%� � <����5�;9b4DCD7

� @%I/� <>9b4 � � ¯�� LJ � � ���© � ��B��/9bI��dB�*¿ª � ���© *­B��|I$<��RG �'=¸9%GDI/�RI J � � ���© 9���� GDIW� I$4DB��.��<��b9"~TI$?79�I$9 C3GH�|B�*

J � ���© 7�8:I/� �� ¯ ��� J � #%#%# ��� R 7

� 6�A'� <>9b4 ���9b9;?=?'�R<�9M�S*s� B%� ��©?9%9"� G8�'�RB�A/<!A/9��b9�� 9 I��È�'�RB���B��0� 9 B34�<��m��B LJ � � ���© � � � ¬�� ¯ L!¬/°/°/°�¬! Þ7

� @%I���I/�M� G�I�9¢I$9 C3G�������BCLx7

�]�H9;�HI$<�� 9��"���RGDB¹9%4$� G/<!��� GDI�� I�� 9b?�2/<��m� <��<�5�b9b4DC�?DB%��9;4�<!��I��¿� GDI*5�@5�9b4FG�<¹B�9;4DC�����<���� I/��h�B34H��I�EH<!�0� B

Bx<!�R9b<>4'��I�<>4�?@*½<�B>B��P<!A5� IW��B%���/2�� <!� 9 B34C� 9��!I�=�<>4�?K9"�%?DB�I��4DB���� ITI�����BFG�<¹B>I�?�9;� <>?�Bx<>4H� <xC>I��d9b46B�� GDI/�

<�� ��I��&���T7}`��B%����<!�R9���B34FB�*?� GDI��«9¢B��'� B#��I$?�2�� I���9��HC39�B>I$469b465�I��&� 9bB34@��7 e�7

��� � ��� �T¶H´���� �����¹� ^´�C �,� ���Q·�´5µ�� �s�K�)F � 5�9b4'��I:� GDI<� B#�T<��?h6B34���I=EH<!�0�bB$��I/� GDB�?5�|?DI����&�R9"A�I$?

9b4�5�I��&� 9 B34 �G�'� B¹B�9b?DI<�R<����5� I��|B�*4I���¬ Ç ©;Å J U�9%9"� GÞ?/9�� � �R9"A�2�� 9 B34EI§��U.=z9"��<!�'��I$<!�0��� G/<!�f� GDI���I8�!I/� GDB�?'�

<��RG/9 I/B>IW� I�� <����5�;9b4DC�I&1�I��&�|<>2���B%��<!� 9��T<������>7�h�B�� IW�'� I��T9�� I��"��=m� I/��I�� ¬ Ç ©;Å J U.= �'¯ L!¬/°/°/°¹¬! � A�I�<�� I/�B�*

?��R<¹9M�HB�A'� <>9b4�I$?$*,� B%� 2'�R9b4DC�I$9"� GDI/��<W� I]D�I��&� 9 B34��!I/� GDB�?FB���� GDI4J<�� � 9b4DC%����I/� GDB�?VI½<!*s��I/��A/2'�R4�9b4DC�U�9b4

5�I��&� 9 B34 ��7ÈÎ�G�I$4�� GDI���I/��B�*i� � � I$<���� ���©;Å J ¯�� IKª � ���© ¬ Ç ©;Å J U.¬ ��¯ L!¬/°/°/°g¬! � "d9���<�?DI�� 9"�R<!Am� IM� I��R<����5� I>7

Î�GDI�9�I$9 C3G�����<���� BQ�T9b<!��I$?¦9%9"� G�� G�I§4DI/9 � � �RI$<��<��<!� I§I���2�<��È��B¦Lx7 ��B���I�� G�<!� � 9���4DB���4�I���I���� <!�R9����

I���2�<��T��B Þ7ÈÎ�G/9��S�'� B#��I$?�2�� I|<¹B>B39b?'��9�I$9 C3G���I�� � 9��§<!� 9 B34�7

L�N

Page 68: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

��� � � � �2,.1!3 -�*<'�=�� 9 �����;%&- �`��<� GDBg9%4EAQ��B32��§I$<!�0�b9 I/��<!� C325�!I$4H���§<>4�?��b<!��I/��I&YD<����5�bI��/=X�RI�� <����5�b9;4DCK<!�§I/B>I/���E� � <xC>I�9��!4�I$9"� GDI/�

4DI���I����R<!���Y4DB���I����T9 I$4��T7��«��9��4� G�2'��?�I�� 9"�R<!A5�bI���BC�'�RI����&�R9"A�I�<@�0�RGDI$?�25� I<*­B��^� GDI�� I�� <����m�b9b4DC�� ��I/�+��B

� < '>I:�5�b<���I>7�Î�9�B�� 2'�PG@���PGDI$?�2'�bI��%<!� I�<�Bx<>9��b<!A5� I��H?DI/��I/�0�§9b4�9�� � 9;�_B>I/�.� 2'�%?'��4�<��§9��x7X�_46<�?DI/��I/�0��9;4�9�� � 9��

���PGDI$?�2'� IzB34�IX��B34�?�25�&���T� I�� <����m�b9b4DC�<!�i� 9;�!I ® � ¬0e ® � ¬/°/°/°;=�9%GDI/�RI ® � ?�I/��I$4/?'��B34�?�9 ���T2'�"�«�%B�*D<��/<!�0� 9��T2'�b<!�

�'� B�Am� I��¦<>4�?���<���� I���2�9"�RIM��B%�!I�I&YQ��I/�R9���I$4H� <!� 9 B34�7j�_4§<*?���4/<���9������PGDI$?�2'�bI�=\B34DI�C39"B>I��¢<^��I���2DI$4'��I%B�*

� G�� I��RGDB%�b?'� � J ¬ � � ¬/°/°/°�=�<>4�?8�!B34�9"��B��.�}� GDI���B�I����T9 I$4��¢B�*¸Bx<!�R9b<!� 9bB34�B�*¸� GDI�9�I$9 C3GH��� ��� �© 7?R GDI$4 ��� �© � �[©

B#�/�T2��0�/=DB34DI*9b4HB>B�'>I��M� I�� <����m�b9b4DCD7d` �{�Q�/9��T<��¸��I���2DI$4'��I�B�* �[©È�T<>4@A�I �[© ¯ � ���.®�� 7

����� ��� �$'&��7 *�9�� 5 ')(+� � , 7 - 5 � - =8587 *Y( �&1RÞIf�RI���B%�<�!I$4�?�� G�I�*°B%���bB¹9%9b4�C�<��bC>B��R9"� G'� *­B��_h�B34H��I�EH<!�0� B<��B%����2�� <!� 9 B34A9b46?���4/<���9���� �#� ��I��<�T7

�Í´5�K��� �GF5�IC��,� Haµ

Lx7E�GDI���'$� GDI49�I$9 C3G���?�9�� � �P9"A/2�� 9 B34¸�j��I/��*­B��0� B34DI�B�*?� GDI�*­B%��� Bg9%9b4DCW�{9�B��PGDB39���I���<!��� 9��!I ® �

Êfz! mp!|Wq>w¹Á?�]*�� GDI�9�I$9 C3GH��I­B���I�� � 9���<!��I$?�9¢I$9bC3GH�.U�?/9�� � �R9"A�2�� 9 B34�9��z4DB��È��B�Bf��'>I/9�I$?T=�9{7ÊI>7"= ��� �© IGJ�U

�[© =�C>B���B�� ��I/�7e�7S6�� GDI/��9%9;��IdC>B<��B�� ��I/� ��7

ÊWx�v{x&l[|:qs 'q,u[vKq>w¹ÁX�]* ® �¯ �Q® �_*­B��M��B%��I�9b4���ITC>I/� � =�C>B<��B�� ��I/�7e�7¿6�� GDI/��9%9�� IC>B���B�� ��I/� ��7

e�75�I/� ®�¯�® � Lx7¦�_4HB>B�'>IA<>4 5#��5¦� ��I/� IK��I��&� 9 B34�e�7�L¹UP7 5�B%�!I/� 9���I���B34DI���<�� 4DITI$? <F� BQ�T<��ShÞE

�5� BQ��I$?/2�� I<I{5�I��&� 9 B34 �%U¿��B§<��/��B%���5�;9�� G$� I��T2��.� 9"B>I4� <����m�b9b4DC!<>4�?$9�I$9 C3G�� 9b4DCD7¿\B���B8� ��I/�¦Lx7

��75�I/� ®È¯�® � Lx7?�i4�B>B�'>I<>4F5Q� 5�@»����I/�7I{5�I��&� 9 B34$e�7 �%UP7 ����IM�RI�� 9b?�2�<����R<����5�b9b4�C�9%GDI$4DI/B>I/�X��B%�0� 9"A5� I>7

Î�B§<¹B>B39b?G9�I$9 C3G��M�T<����T2'�b<!� 9bB34T=�2'��I4� B#�T<��Th�B34H��I!EH<!�0� B�� I��R<����5�b9b4�C:�!I/� G�B�?'�$7X\B<��B�����I/�¦Lx7

` 4DB�� 9���I$<!A5� I*?�921�I/� I$4'��I�A�I/�{9�ITI$4AB32��%25��I|B�*a�bBQ�T<��¸h�B34���I�EH<!�0� B��5� BQ��I$?/2�� I���<>4�?�� G/<!��B�*gB�� G�I/�0�

e!�

Page 69: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

IKJ��2���~TI�� I/�<>4�?G���2�45���RGVL�N�N��QPT��I/��~$2�9;4�9�I/��<��¡7�L�N�N%��P ��9"� �%<>4/?�5�GDI/�/G/<!�R?�L�N�N%��U�9;��� G�<!�M9�I*?�I���B32��5� I

� GDIW� B#�T<��jhÞE�B32'� �/2����9b4H��B��{9�B$�/<!�����/��I�� � 9;��<!� 9b4DC�� GDI�4�I/9 9�I$9 C3G����_*­B���� GDI:ª?©�<>4�?�B�A'� <>9b4�9b4�C<� GDI

?��R<¹9M�B�*}Ç ©;Å J 74Î�GDI/� I�<!� IW�{9�BA<>?�Bx<>4H� <xC>I��|B�*�?DB39b4DC$� 2'�PG <=?�I���B32��5�b9b4�C��fI½9,U%B�A'� <>9b4�9b4�C�<>4 I&Y#�5�b9��T9"�

9�I$9 C3GH� 9;4DC¦�T<>4���I��;��2'�§G�B¹9 ?/921�I/�RI$4H�<¨ ©;Å J <>4�?�¨ © <!� I�<>4�? GDB¹9�9¢I������ GDI 5Q� 5­9�B���'Q��P�<>4�? I½9;9,U�9"�

�'� BgB�9;?DI��¢<W�!I$<>45�X��B!9��<�'� B¹B>II����T9 I$45�&�<B�9;<f� GDI|2'� I�B�*¸�RI�� 9b?�2�<��m� <����5�;9b4DC�<>4�?F?DI��;<��>I$?$� I�� <��<�5�b9b4DCD7

5�9b45��I:� G�I�� B#�T<��Èh E�'� B#��I$?�2�� I��|<!� I��!I/� I��"�K2'��I$?���B6<��PG�9 I/B>I=<èC>B�B�?#!©;Å J =�<>4���B�� G�I/�^�!I$<>4'�4� G�<!�

� I$<>?'����B<� G�9��HI$4�?@� GDB32'�b?8A�I4��B34'� 9;?DI/� I$?$9%GDI$4�I/B>I/����B%��� 9"A5�bI>7

� �� ����� � � ����� � ���G�E�b�»���Í�

�_4=<��;��5#��5�@��'�RBQ��I$?�2'� I��/=#� GDI|?�9����&�RI/��I_� I/�5� I���I$4�� <!� 9 B34FB�*j¨5©;Å > IKªd©{U.=�AQ��<W� <����5�bI�B�*aª � ���© 9%9"� G8� GDI

9�I$9 C3GH� J � ���©;Å > =�?DITC>I$4DI/�P<!��I���B>I/���¦�P<!�/9b?'�"�Y<��W� G�IF4�2'�^A�I/��B�*_� I�� <����m�b9b4DC%��9b4'�&� I$<�� I��^A�I/�«9¢ITI$4 ® <>4�?

® � \7$`_��<G��B34'��I���2DI$4'��I�=�I�� � 9;��<!� 9b4DC6<C��2/<>4H� 9"�«�ÞB�*�9b4H��I/�RI�� �/=d� 2'�RG�<���MXP�����gIYKjIKªd©{U U.=d�T<>4¦A�I<B>I/���

9b4�<��/�T2'�R<!��I>7

Îg< '>I�cdYD<����5� I$L:*°B���9;4'� � <>4'��I>7:`_�*5#��5�@Í�'�RBQ��ITI$?'��9�9"� G ® =T� GDI�4�2'�^A�I/�B�*�?�9�� � 9;4'�&� 9"B>I:Ç �$I½9¡7ÊI>7"=

� U�Br<��;2DI��d?DI��&� I$<���I��4��B34DB���B34�9��T<������>74Î�G�9��_�R<!�/9b?5�"�G� I$<>?'����Bè<è?DITC>I$4DI/�R<!��I���B%� ��I/�P9 B��|?�9�� � �R9�A/2�� 9 B34èB�*

� 7:Î:BA<����bI/B�9b<!��IW� G�9;���'� B�A5� I��G=�9�I<�T<>4Y<!�'�m�"�6<$Bx<!�R9b<>4��B�*¿� GDI!@%<xB!({�S�b<��('�9�I����b9�~$<!� 9 B34bI{EH<���I��;�b<è<>4�?

@�B�A�I/���4L�N�N��QP��|B34DC�I/��<��¡7�L�N�N���P�8�9b2FI/�%<��¡7�L�N�N���UP7

5�2��5��B%� I�=¹9%9"� Gf� GDIX�H<¹�>I�� 9b<>4W��9����R9b4DC�?�<!� <���I/� � 9b4DC�B�*'cdYD<����5� IML!=!9�I¢G�<¹B>I�<!�a� 9��!I ® � GDI�B�A5� I/��B>I$?

9b4�*­B��0��<!� 9bB34 � © <>4/?7��2'�"� 9��5� I*?'�R<�9M��I � � ��� ¬�ª � ���© U.= � ¯ L!¬/°/°/°g¬! �=¸�'� B���I/�0�"��9�I$9 C3G���I$?FA�� J � ���© 74�«*X� GDI

4�2'�fA�I/�*B�*%?�9�� � 9;4'�&� 9"B>I<Bx<��b2DI���B�*�� GDI � � ��� 9��^��B�B7���§<����>=a9¢I$�T<>4+*,�R<xC%��I$4H��I$<��RG½� � � I$<�� I � � ��� ¬�ª � ���© U

AQ�K?��P<�9%9b4�C � � � J � ¬/°/°/°¹¬ � � � � *s� B%� � G�I<��B%���5� I/��I&(�?�<!� <@��B%� ��I/�R9 B���?�9;� � �R9"A/2'� 9 B34<Ï?I �C� ª � ���© ¬�� © UP7<R GDI$4

� GDI$��B%� ��I/�R9 B��!?�9;� � �R9"A/2'� 9 B34ÞB�* � 9��W��B34�� 9b4�2DB32'�/=È9�I�9%9��;��G�<¹B>I � ?/9�� � 9b4'�&� 9�B>I � Br<��;2DI���<!*,��I/�§@�<xB!(

e#L

Page 70: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

�S�b<���'Q9�I����b9"~$<!� 9 B34�7}Î�GDI�9�I$9 C3G���<�����B#�T9b<!��I$?@9%9"� GFI$<��RG½I � � � � � ¬�ª � ���© U�9��6J � ���© 7_IKÎ�G�9;��9��S� GDIf��B345��I���2�I$4'��I

B�*#� GDIÈ*½<��&�¸� G�<!�:<!*s��I/��<�*°I/9­� ��I/�5��B�*�hÞESh E�� �R<>4'� 9�� 9 B34�9%9"� G�� I�����I��&�i��B�9%G�9��PG�� GDId� <!� C>I/��?�9���� �R9"A/2�� 9bB34

¨'©�9��%9;4HBx<!�R9b<>4��/=5� GDIW� <����m� I*9��_� � 9����¸�'� B���I/�0�"�89¢I$9bC3GH��I$?79%9"� G@� I�� ��I��&����B8¨5©i7|5�ITI^hK<��/c�<��PGDI/�R4 I/�d<��¡7

I)L�N�N�O%UP7SÎ:B<�RI/� <>9b4G��B34'� � <>4��M��B�� <���4�25�fA�I/��B�*d� � � I$<��<� �=�B34DI4�T<>4AI$9�� GDI/�M��I/� �<¯ L!=�B��M� I��R<����5� I�

� � � I$<�����*,�RB%� � GDI � � � � I$<��<��<��/��B��P?�9b4DCW��B�� GDI$9"����B��0� I�� ��B34�?�9b4�Cf9�I$9 C3GH�T7¢@%<xB!({�S�b<���'Q9�I����b9"~$<!� 9 B34�<��

?DI����&�P9"A�I$?è<!A�BgB>I4� I�� 25�"����9b4è<<�R<����5�b9b4�C�?/9�� � �R9"A�2�� 9 B34�� G�<!�%9����/�bB%��I/����B�� G�I�� <!� C>I/�?/9�� � �R9"A�2�� 9 B348� 9b4'��I

9"��2'� I����!B�� I�9b4'*°B��0�§<!� 9 B34�7

�]*g<!��� 9;�!I ® 9�I49H<>4H�ÈÏaI �.� � © U.=59¢If�T<>462'��I�� GDI�@�<xB!({�S�b<���'Q9¢I��;�b9"~TI$?AI���� 9���<!��I�=

ÏjI �.� � © U ¯W R� ?�J J � ���© ÏaI � � ª � ���© ¬�� © U

W R� ?�J J � ���©

¬

9b4'����I$<>?èB�*g25� 9b4DCW� GDI49�I$9 C3G���I$?AG�9�����B>C��R<�� B�*?� GDIf� <��<�5� I$? � � ��� 7

Î�BW��B%�<�/2���I�� GDI��b9 '>I��b9bG�B�B�?�*½2�4'�&� 9 B34 �_I � � � © U.=#9¢I��T<>4835�0���¢?��R<¹9 � 2/4�9"*­B��0�<�"��I½9"*¸� GDI��/<!�R<���I/��I/�

� �/<���I�9��MA�B32/4�?DI$?T=DB�� GDI/�09%9���I*B34DI*4DITI$?C��B$��B%�fA/9;4DI4� GDI ��<!�M�5�R9 B���9%9�� G@��B%�!I�?�<!� <HU%<>4�?6<!�5�5�"�$� GDI

5Q� 5$��BF?��R<¹9Í��2'�"� 9"�m� If��B��/9 I��WI � � ��� ¬�ª � ���© U�9%9�� GG9�I$9 C3GH� J � ��� 7_Î�G�I$4G� GDI�@%<xB!({�S�b<��('�9�I����b9"~TI$?KI�� � 9;��<!��I

B�*?� GDI4�b9 '>I��;9bGDB�B�?�*­2/4'�&� 9 B34è9��/�

�MI �.� � © U �

W R� ?�J J � ��� ÏaI �.� ª � ���© ¬�� © U

W R� ?�J J � ���

¬

9%GDI/�RISÏaI �-� ª � ���> ¬�� > UH9���� GDI4��B%���5� I/��I&(�?/<!� <<��B%� ��I/�R9 B���?/9�� � �R9"A�2�� 9 B34�B�* � 9�9"� G ��<!���'�P9 B��$7

��I/��~$2�9b4/9�I/�*<��¡7FI)L�N�N%��U�4DB�� 9;��I�� G�<!�49�GDI$4V��B%�!I�*°B��0�0B�*���B34�?�9"� 9 B34/<��z9b4�?DI/��I$4�?DI$45��I�9;���'� I���I$4��

I­I>7ÊCD7"=�9b4�<���9���� 9;4DC�?�<!� <4�'� B�A5� I��Í9%G�I$4�<���<!�R<��!I/��I/� � 9���9b4�B>B%�"B>I$?<��B34/?�9"� 9 B34�<���B34W9%G/9��RG:� GDI���9;��� 9b4DC

?�<!� <K<!� I�9;4�?DI/��I$4�?DI$4��|B�*�I$<��RG B�� GDI/�[U.=�B34DI8��<��­��B%�!I/� 9��!I�� �«¥�q;u�x& �£�p�£�x ��� GDB%� I�I$<!�0���KB�Am��I/��Bx<!� 9 B34

� � ��<>4/?§I$<!�0����9����/2�� <!� 9bB34'�T7 �DB���9b4'� � <>45��I�=�9b4�cdYD<����5� I:LÈÏ?IsÇ ©;Å J ¬�� ©;Å J � � ¬�ª © ¬�� © U ¯ ÏaIsÇ © ¬�� © � � U.°#J�I$4'��I

<�����B�*�� GDI$ª?©�<>4�? � © �T<>4­A�I ��?�9���I$4DC3<xC>I$?�7 �Þ5D9���9��b<!�.�"�69b4+cdYD<����5� I8eQ=d� GDI8ª?©�8 J <>4�? � ©�8 J �T<>4­A�I

e�e

Page 71: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

��?�9�� I$4DC3<xC>I$?�7 � Î�G�I6<>?�Bx<>4H� <xC>I B�*�?DB39b4�CV� G�9���9���B�AQB�9bB32'�/� 9"��� <¹B>I����!I��!B���� <>4�?���<�������ITI$? 2��

��B%���/2'� <!� 9 B34�7

J�B¹9�I/B>I/��=S9%GDI$4 ?�9���I$4�C3<xC>I��!I$4H��9;��9;���5� I��!I$4���I$?T=�@�<xB!({�S�b<���'Q9¢I��;�b9"~$<!� 9 B34 9;��4�BF�bB34DC>I/��?�9"� I��&�����

<!�'�5�;9��T<!A5� I>7�` �RI��!I$?��69�����B$� I/�'�RI���I$4H��� GDI!9b4'*°B��0�§<!� 9 B34C��B34�� <>9b4DI$?Þ9;4@� GDI!?�9;��I$4DC3<xC>I$?���B%����B34DI$4����

<��d<���9"YQ� 2��RI*?�9���� �R9"A/2�� 9bB34�B�*}� GDI � IsB�9;<§@%<xB!({�S�b<��('�9�I����b9�~$<!� 9 B34mU�<>4�?G� G�I$4@�'�RBQ��ITI$? 9b4C��B%�fA/9;4�<!� 9 B34

9%9"� G$�RI�� <����5�b9;4DCD7 ��2'��I/�R9��T<���I&Y#��I/�P9��!I$4���B34@� G�9����!I/� G�B�?A9;��2�4�?DI/��9b4�B>I�� � 9 C3<!� 9bB34�7

� ��� � � � � �

����� � 9:5 '�5)1!��( 7 *�9 � *�� �I#"%;*�-<*��&7 *�%&1 � 5 ���I-

�_4�9"� 9b<��������'�RB���B%� I$?GAQ� ��<>9"�|<>4�? 13<�1�ITIGI)L�N%�!e%U.=¸� G�9��_�/�b<����B�*}�!B�?DI�����G�<��d<!� � �R<��&��I$?��*25�RG <!� ��I$4H� 9bB34

*s� B%� I���B34DB%��I/� �R9��/�_� I���I$<!�0�PGDI/�0�9b4G�/<�����*°I/9 ?DI��T<>?DI��$7¿�]�M�5� B¹B�9b?DI���<�� GDITB�� I/� 9��T<��j*­B32�4�?�<!� 9 B34G*°B���� GDI

�/G�9;� B%��B��/G�9;�T<���<!�RC32'�!I$4�����I­C>I$4DI/�P<����"���T<���� I$?���x&z! �x[u[qKp� Gvs¾#x�n!l[z!=�9%G�9��PG�9��¢4�<��!I$?§<!*s��I/�¿� GDI%I���B34DB%�§9�� �

1D7 h�7Æ�|I/��4�I��M9%G�B§<!� � <���'>I$?�� G�I�?DB%�§9b4�<>4����/<!�R<>?�9bC%� B�*zI���B34DB%��9;�/�9;4¦L�N��!���.U�<xC3<>9b4'���M� GDI:��B%� �{9H<!�

��<>9b45� � � I$<�� <!�'�'� B3<��PGb��B I���B34DB%��9��/��=�� GDIKI���2�9;�b9"A'�R9;2'�-��I/� GDB�?5�T7 5�ITI��d2�<>4�?'�CI)L�N�O�eQ=:L�N�O�O%U�*­B��

� I/B�9 I/9M�z<>4�?!?/9����T2'���R9 B34'�T7iJ�I/� I�9�I�B34'�"�:� B�B�'�<!��<4����I��T9;<���?'��4�<��§9��H?�9���I���2�9��b9�A'�R9b2'� ��B�?DI���9b4�J�I$4�?��0�

<>4�? @%9;�RG�<!�R?5�:I)L�N�N!�HUP7f`����!B%���<����a��B%����B34DI$4����B�� GDI/��� G�<>47� GDI:� I�� I/Bx<>4H���b<xC>C>I$?7Bx<!�R9b<!A5� I���=�� 25�RG <��

�'�R9;��I���<>4/?FB�� GDI/��I$4HB�9"� B345�!I$4H� <���I&Y�B>C>I$4DB32'��Br<!�R9;<!A5� I��/=�<!� I|I&Y��/�b2�?DI$?8*­B���� GDI�� < '>I|B�*a�R9����5�b9;�T9"�{�>7aRÞI

9����;2'� � �R<!��I|<>4A9��<�'� B¹B>I���I$4H�%9;4FI�� � 9���<!� 9 B34GA��F2'� 9b4�C�@�<xB!({�S�b<���'Q9¢I��;�b9"~$<!� 9 B34�7

8�I/� 6 �© ¯ I�6 J © ¬�6 � © UXA�I�A/9"Bx<!�R9b<!��I|4DB��.��<��T�R<>4�?�B%� Br<!�R9;<!A5� I��S9%9"� G

M8I�6 � � ©;Å J � � 6/©{U ¯ � 6 � © P �� � I�6/©;Å J � 6/©«U ¯� ¬

*­B�� ®^¯ �#¬/°/°/°¹¬�� � L!=d9%GDI/� I � 9;�f� GDIF9b?DI$4�� 9"�«�V��<!� �R92Y�7@Î�GDI�B�A5��I/��B>I$? ?�<!� 25� *­B��^� G/9��f�!B�?DI���<!� I

��© ¯ ��9b4 � 6 J © ¬�6 � © "%=È*­B�� ®W¯ L!¬/°/°/°g¬���7 �DB��<� 9;���5�b9��T9��{�Þ9b4­�'� I���I$4�� <!� 9 B34T=È� G�IF9;4�9"� 9b<��X� � <!��I���6 J �=<>4�?

e��

Page 72: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

6 � �^9¢I/�RI�� < '>I$4���B$A�I��F<>4�?Þ<����R2'�!I$?B'�4�B¹9%4�7W6�*�9b4���I/� I�� �|9���� GDI<�b9 '>I��b9bGDB�B�?G*­2�45�&� 9 B34KB�����B%����I/�R9 B��

?�9���� �R9"A/2�� 9bB34§B�*�I J ¬ � UP7

8�I/� �'© ¯ ��<�Y � 6 J © ¬�6 � © "%=Æ� I/��� © A�I��H9"*&� © ¯ 6%� © =�<>4/?C� I/� � ¯ I J ¬ � UP7��«*d9�I:9��R9"��I4Ç © ¯ I �'© ¬�� © U.=

� GDI�?/9�� � �R9"A�2�� 9 B34§9;4HB>B%�"B>I$?69b4���I���2DI$4H� 9;<��¸� <����5�;9b4DC�9��� ©;Å J IsÇ ©;Å J � ª © U ¯ Ï?IsÇ ©;Å J � � ¬�ª © ¬�� © ¬�� ©;Å J U.P/<>4�?

� G�<!�|9b4�B>B%�"B>I$? 9b4C9¢I$9 C3G��d2���?�<!� 9b4DC=9��XJ�©;Å J <bÏ?I���©;Å J ��� ¬�ª?©)¬�� © U.°T�I/� <>9�� I$?F��B%���/2�� <!� 9bB34'�d<!� I�C39"B>I$4

9b4$� GDI^`M�'��I$4�?/92Y�7

�DB���I$<��RG�3'Y�I$? � =HJ�I$4�?����!<>4�?§@%9��PG�<!�R?@I)L�N�N!�HU�2'��I�� G�I5Q� 5:��B�I/Bx<��b2�<!��I�� G�I_�b9 '>I��;9bGDB�B�? �_I �.� ���aU

A/<���I$?�B34�I���2�<!� 9 B34GI>O%U�B�*��|B34DC|I/�z<��¡7�I)L�N�N���UP7 �¢2�� � 9b4DCd< ��<!�?�'�R9 B���B34 � =!9�I��T<>4�� � I$<!�d� GDI��b9 '>I��b9bGDB�B�?

��B%���/2'� <!� 9 B346<���<<�H<��>I�� 9b<>47��B%���/2'� <!� 9 B346<>4�?62'��I�� GDI�5#��5$��I/� GDB�?G��B�� 9���2'�b<!��I�9�I$9 C3G���I$?@� <��<�5� I��

B�*�I � ¬�ªÈUiD�B39b4H�����>7�@%<xB!({�S�b<���'Q9�I����b9"~$<!� 9 B34C�T<>4GA�I�<!�'�5�;9 I$?8��B§9����'�RB¹B>I�� GDI�I��8�T9 I$4'�&�>7

• • • • • • • • • • ••

• •

••

• • • ••

• • •

• •• •

• • • • • • • •

Den

sity

0.0 0.2 0.4 0.6 0.8 1.0

0.0

1.0

2.0

3.0

� �GF � C�� ���Î%GDI<��B%� ��I/�R9 B���?�9�� � �P9"A/2�� 9 B34KB�* J <!*s��I/�^� GDI8�!�6B�A5� I/��Br<!� 9bB34'�49%9�� GY2�4�9"*­B��0�

�'�R9bB��$7�8�9b4DI � � I�� 25�"�?*,� B%�'@�<xB!({�S�b<���'Q9¢I��;�b9"~$<!� 9 B34TP�?DB���� � � I�� 2'�"�?*s� B%� � GDI���� <>4�?�<!�R?§5Q� 5�7

RÞI@� 9;�*2'�;<!��I$?��!�Y?�<!� <ÞB�A5� I/��Br<!� 9bB34'��� J ¬/°/°/°¹¬����$*s� B%� � GDI@�!B�?DI��S9%9�� G J ¯ � ¯ �#° �Q=�<>4�?

e��

Page 73: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

9b4�9�� 9b<���Br<��b2�I�6 J�J ¯ 6 ��J ¯ �#°5`���� 25��9b4DCW� G�<!��9�I�'�4DB¹9 J ¯ � ¯ ='9�I�2'��I$?�� G�I*5Q��58��I/� GDB�?G9%9"� G

¯ L/�#¬������8��B§B�A'� <>9b4@� GDIf�b9 '>I��b9bGDB�B�?8*½2�4'�&� 9 B34G*°B�� =�<��_� G�B¹9%4A9;4 ��9 C32�� I�Lx7S�]����B�B�'@O�7�L�����I���B34/?'�

B34Y<65�9;�b9���B34�\��R<!�/G�9;�/��9¢B���'#� � <!� 9 B34V9%9"� GÞ@fL/�������C��9��&� B��5� BQ��I��0��B���=�<>4�?F� GDI ��� � <!�4� GDI!I$4�? B�*¿� GDI

5Q� 5=9��M��7�L ��7¿Î�GDI4���!B�B�� GG�T2��0B>I�9��S� GDI4�RI�� 2'�"�S*s� B%� � GDI�@�<xB!({�¿�;<���'Q9¢I����;9"~$<!� 9 B34�7

��� � �L-�*<' � � �$9:5 ' � 5)-�% ( *�5 'Î�GDI��!BgB�9;4DC�<�B>I/�R<xC>I:� �#� ��I���= � © ¯ W��� ?�J 5 �sÇ ©�8 � ��� © ¬�9��HB�*s��I$4G��ITI$4A9;4=?/9 C39"� <��Æ��B%�<��2�4�9��T<!� 9 B34�7ÈÎ�GDI

9b4���2��¿�R9 C34�<��ÆÇ © � < '>I��MBr<��b2�I�*,� B%� <�'�4DBg9%4���I/��B�*�?�9��0�&� I/��I�� � <!��I���<>4/? � © 1��½IK�#¬�� � UP7}�¿�=B�A5��I/��B�9b4DC

� GDI^A5�b2����RI$?G� 9 C34�<��;� � J ¬/°/°/°�¬�� � =�9"�9���B�*�9;4H��I/� I����M��B8� I���B34'� � �R25�&�_� GDI^Ç5©�<>4�?G��B=I�� � 9���<!��I:� GDIW� �#� ��I��

��B�I����T9 I$4���� 5 � 7Xh6B�� If� I/*°I/�RI$4'��I��M�T<>4@A�I�*­B32�4�?è9b4è8�9b2è<>4�?�E�GDI$4VI)L�N�N��%UP7

RÞIf��B�B�'è<�� 9���2'�b<!��I$?èI&YD<����5�bI�*,� B%� E�GDI$4A<>4�?A8�9ÈI)L�N�N��%U�9;4$9%G�9��PG$� GDIf� �#� ��I�� I���2�<!� 9 B34A9;�

� © ¯ Ç © � �#° O�Ç ©�8 J � �#° ��Ç ©�8 � �� © °

Î�GDI�9;4��/2���� 9 C34/<�����Ç5©�9�I/� I§9b9;?Þ2�4�9"*­B��0�.B34 � �#¬�L!¬ � "37�Î�GDI<�R9 C34�<��2(§��B!(�4DB39;��I��R<!� 9 BG9H<��f��B34�� � B%���bI$?Y<!�

L��K?'��=?9%G�9��PGÏC39"B>I��!<F� � <>4/?�<!�R? ?DI/B�9b<!� 9 B34+��7 �7*°B��W� GDIè4DB39�� I>7FÎ%GDI 5 � 9b4¦� G/9��W�T<���I��T<>4½A�I=I$<�� 9����

9b4���ITC��R<!��I$?èB32���9�9"� G �%B��.��<��Æ�'�R9 B���<>4�?è<��;�Æ� GDI�� <����5�b9;4DC�<>4/?$9¢I$9 C3G�� 9b4DC<�T<��;�T2'�b<!� 9 B34'�S�T<>4�A�I�*­B32�4�?

9b4è8�9b2è<>4�?�EHG�I$4VI)L�N�N��%UP7¿` ?�9"�RI��&��5Q��5�9�9"� GDB32���2'� 9;4DC!<<� B#�T<��Th E �'� B#��I$?�2�� I|<!�'�5�;9 I��T7

`��B�� <��gB�*}e!�����R9 C34�<��¸��I���2DI$4'��I��_9�I/� I:�R9��*25�b<!��I$?T=�I$<��PGC9�9"� GCe!���8��I���2DI$4H� 9;<���B�A5��I/�0Br<!� 9 B345��*s� B%�

� GDI:� �Q����I��è7M6d2���9;4H��I/� I�����9�<��9b4@��I�� � 9b4DC<� G�I^� 9��<�5� I*5Q� 589%9"� G6?�921�I/� I$4H�M� I�� <����5�;9b4DC<���PGDI$?�2'� I���<>4�?

9%9"� G^� GDI¿�{9�B�� I�� <����m�b9b4DCM��I/� GDB�?5�¿I½9¡7ÊI>7"=�� 9����5�bIÈ�R<>4�?DB%�Í� <����m�b9b4DC^IK�[U¸B>I/�.� 2'�a� I�� 9b?/2�<��H� <��<�5�b9b4DC:Is�[U UP7

6d4DI^� GDB32'� <>4/?@� � � I$<����WI ¯ L/�����HU_9�I/� I:�T<!�0�R9 I$?69;4G� GDI�5Q� 5$�'� B#��I$?�2��RI>7�RÞI�I�� � 9���<!��I$?C� GDI�9b4��/2��

� 9 C34/<��HÇm©�AQ�fh7` �Ï2'� 9b4DC�� GDIS9�I$9 C3GH�z<!�?� 9��!I ® � ��7aÎg<!A5� I�LS� GDBg9M�?� GDI�4�25�fA�I/�gB�*m��9��0�/�b<���� 923��T<!� 9 B34�B�*

� 9 C34/<�����9;4:e!����� 9���2'�b<!� 9bB34'�/=rI$<��RGW9%9"� GWe!������I���2DI$4�� 9b<����R9 C34�<����T7aJ%I/�RI�=%� I�� <����m�b9b4DC�*s� I���2DI$4'�&� ® � ��I$<>4'�

e��

Page 74: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

�'� B#��I$?�2��RI���IK�.U�B��$Is�&U�9�I/� I�<!�5�5�b9 I$?Y<!� ® �!¬0e ® �!¬ � ® �!¬/°/°/°�IK��B ® � ¯ e!���K9;���5�b9 I��|4�B@� I�� <��<�5�b9b4DC�UP7 ��B��?���4�<���9����0�RGDI$?�25�b9b4DC�=�� I��R<����5�b9b4�C��5� BQ��I$?/2�� I�9���<!�5�5�b9 I$?W9%G�I$4DI/B>I/�}� GDI%I&1�I��&� 9"B>I�� <����m� I�� 9"~TI^I½?�I&3�4DI$?

<�� � I)L � ��� � IGJ�U U�9;��� I������ G/<>4 ��7S�i46B32���I&YD<����5� I�=m� G�9���?'��4�<��§9������RG�I$?�2'� I4� I$<>?5����B8����BCL���� 9;�!I��

B�*?� I�� <����m�b9b4DC%�H9b4$�'�RBQ��I����R9b4DC<e!����� 9bC34�<����T7

Îg<!A5� IELF� GDB¹9_�$� G�<!��� I�� <����m�b9b4DC�I$9"� GDI/����B�B B�*,��I$4 I ® �7����<����,U§B�����B�BE�R<!� I�I ® �CA�9 C�U���I$4/?'�$��B

�'� B�?�2'��Iè<��b<!�RC>IA4�2'�^A�I/��B�*���9;���/�b<���� 9"3m�T<!� 9 B34'�T7­R�G�I$4½� I�� <����5�;9b4DC7��B�BYB�*s��I$4T=¢I>7ÊCD7 ® � ¯ LèB����Q=

� GDI/� I*<!� If��<!��'>I$?@*,�RI���2DI$45�&��� G�<!�_� GDIfh�B34H��I!EH<!�0� B<��I/� GDB�?69���4DI/B>I/��B34G� GDI4�P9 C3GH��� �P<���'�=m� I��R2'�"� 9b4DC

9b4è?�9;� <�� � � B32'�HI���� 9���<!� 9 B34'�$7È�i4G� GDI4� I$<���B34/<!A5� I��R<>4DC>I�B�* ® �<IsA�I/�{9�ITI$4Ce!��<>4�?@�!�HU.=5�RI�� 9b?�2�<��Æ� <��<�5�b9b4DC

�!I/� GDB�?G��ITI�������B�A�I����b9 C3G����"��A�I/� ��I/�M� G�<>4G� GDI4�R9����5� I��R<>4�?�B%��� <����5�;9b4DCD7

�I/��I/�0��9b4/9�� � 9��|@%I�� <����5�b9;4DC!5��RGDI$?�25� I ® � ?���4/<���9��

L � e!� �!� L/��� e!��� ���PGDI$?�2'� I

I/���RB�� � � � � � � � � � � � � � �

��(«e L�L � � L � L � L � � L/� L � � � L�L L�e

�g(«� �HN �HN �H� ��� �#L ��� ��� �HN e�O e�O � � ��N ��O

�g(«O �'L � � �!� ��e �!e ��� �%� ��O ��N ��O L�e L�e ��� �%�

Ng(0L�L e�� e!� e%� �!� ��O ��O ��e �HO � � ��� ��� ��� e�N �'L

L�eg(0L�� L/� N L � � O � L¹� e!� ��� ��e ��� ��� L�� O

L��g(«e�� L�L L/� L&� L�L O O L&� L�� ��� ��� O�� O�� � L�L

L��g(«�!� � L/� O N � � � � L � � � L L

� �!� �#L ��� ��� e�� � � � � � � � � e e

e��

Page 75: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

� ´�� �,�@��XÎ%GDI�4�2'�^A�I/�0�gB�*��§9����/�b<����R923/I$?:� 9 C34�<�����Is� GDI�35�0� �?��B%�b2'��4�U:9;4!<���B�� <��DB�*Æe!���f� 9��W(

2'�b<!� 9bB34'�/=�I$<��RG@9%9"� GGe!���<��I���2DI$4�� 9b<��T� 9 C34/<����/=D<!� I�?DI��!B345� � �R<!��I$?�7}`������ GDI4��B%�b25��4'�HI&Y���I/�'�

��B%�b2'�§4¦L�<!�RI^� I��R2'�"����*s� B%� ?�921�I/� I$4H����B%�fA�9b4�<!� 9 B34'�%B�*�5Q� 5$� � �R<!��ITC39bI��T7d5Q�#�^A�B%�;� � � �=<>4�?

�)� �!B34G� B¹9 �:� I/�'� I���I$4���u[q,|_kmr�x^l)p� m¥%n!| u&p�|Mk�rtqs �£�<>4�?�l�x[u[qK¥!�'p!riu&p!|_kmr¤qs �£^� I�� ��I��&� 9"B>I��"�>7

����� � , 7 =8� ( � 7 ,.9���*<'�= *�' � -<% ( (+��7Îj�R<���'�9b4DC¦��2'�"� 9��5� I8� <!� C>I/���F9b4E�/�b2�� ��I/�=9��!B�*d9b4H��I/�RI�� �<��BÏI$4�C39b4DITI/�0��<>4/?���B%���/2���I/�8���T9 I$4�� 9�� ���T7»Î�GDI

�'� B�Am� I�� G/<��_�RI���I$9"B>I$?F�*2'�PG <!� ��I$4H� 9 B34V� I���I$4H�����6<>4�?F��<>4��@��B%�b2�� 9bB34'��G/<�B>IWA�ITI$4@�'�RB���B%� I$?T=�<���B34DC

9%G�9;�RGF� GDI���I/� GDB�?YB�*�\B��P?DB34ÞI/��<��¡77I)L�N�N��%U�<>4�?�`_B�9"� ~TB32���I)L�N�N��%U�9��4�!B%���4�/� B%��I��"�7� I��b<!��I$?¦��BG� GDI

�!I/� GDB�? ?DI����&�R9�A�I$?�9b4­� G�9���<!��� 9��/� I>7�`���G�<��WA�ITI$4­�!I$4�� 9 B34DI$? I$<!�0�;9 I/��=d� GDI$9"��<�� C>B��R9�� G'��I����5� Bg�V� GDI

� <����m�b9b4DCY?�9;� � �R9"A/2'� 9 B34 I>�%UP7ÍJ�I/� IC9¢I 2'��IC� GDI�I&YD<����5�bI�9b4»`MB�9"� ~TB32���I)L�N�N��%U���B+� G�B¹9 � G�<!��25� 9b4DC

� <����m�b9b4DC�?�9�� � �P9"A/2�� 9 B34FI �%U��T<>4G�'� B�?�2'��I�A�I/� ��I/�M� �R<��('�9b4�C<� I�� 2'�����T7

Î�GDI_� �R<���'�9b4DC:�'� B�Am� I��)9;4�`_B�9"� ~TB32'��I)L�N�N��%US�T<>48A�IM*­B��0��2'�b<!��I$?=<��H<^� � <!��I�� ��<���I���B�?DI��59%9"� G�� GDI

� � <!��I^Br<!�R9;<!A5� I�Ç5© ¯ IsÇ � J �© ¬ Ç � � �© U.='9�GDI/� I4Ç � J �© 9���� GDIf�bBQ�T<!� 9 B346B�*?� GDI4� <!� C>I/�B34A<���� �R<>9 C3G��M�b9b4DI|<>4�?@Ç � � �©

9��S� GDIf� <!� C>I/�MB>I�� B#�T9"�«�>7XÎ�GDI �¹©�<!� I4� B#�T<!� 9 B34èB�A5� I/��Br<!� 9bB34'�T7}Î�G�I/��I/B>B%�"B>I*9b4$� GDI4*­B%��� Bg9%9b4DCW9H<����

Ç � J �©;Å J ¯ Ç � J �© � Ç � � �© �Le J:I ® � L¹U.¬

Ç � � �©;Å J ¯ Ç � � �© � J:I ® � L¹U.¬

�¹©;Å J ¯ Ç � J �©;Å J � ��I ® � L¹U.¬

9%GDI/�RI J:I ® U�1 �½IK�#¬�6 ��U<>4�? ��I ® U 1 �½IK�#¬ � �¹U<!� I§9b4/?DI/��I$4�?DI$4��T7fRÞI<*½2���� GDI/��<��0� 2'�!I�� G�<!��9�I�B34'���

G�<¹B>I��'� B�A/<!A/9;�b9"�{�7Ï��C��BE��< '>I�� GDI�� B#�T<!� 9 B34�B�A5��I/��Bx<!� 9 B34 ��©�7 Î%GDIC�P<!��IKB�*f*­<���� I7� 9bC34�<����/�b2�� ��I/�è9��

�� =j9�9"� G � A�I$9b4DC$� GDI�9%9b?�� G B�*S� � ?DI/��I��&� 9 B34­� ITC39 B34�7�Î�GDI/�RI/*°B�� I�=j� GDI§<��&� 2�<���B�A5��I/��Bx<!� 9 B34D� © 9���<

B>I��&��B��%B�*?� I$4DC�� G �©z<��!B34�C�9%G�9��PG=<!�_�!B%� �HB34DI�9��S� GDI�� �P2DIdB�A5��I/��Bx<!� 9 B34�7ÈÎ�G�Id?/9�� � �R9"A�2�� 9 B34�B�* �©z9��

��I/�R4DB32'�;�b9{ItÏ��/U � �gB39�����B34jI � � UP7XÎ�GDI�*­<���� I�� 9bC34�<����H<!� I|2�4�9"*­B��0�<����?/9�� � �R9"A�2���I$?�9b48� GDI|?DI/��I��&� 9 B34$�RITC39 B34�7

e%�

Page 76: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

�_4$� G�9����T<���I�=m� GDI4� <����5�;9b4DC�?�9�� � �R9�A/2�� 9 B348¨'©;Å J IsÇ5©;Å J � ��©;Å J ¬�ª?©«U�9b4VI �%US�T<>4�A�I|I$<�� 9������ GDBg9%4���B<A�I

<8��92Y#� 2�� I�B�* �©;Å J 4DB��0��<��g?�9�� � �P9"A/2�� 9 B345�/=#9%9"� G@��I$<>4'��<>4/?@Bx<!�R9b<>4'��I��_A�I$9b4DC<*½2�4'�&� 9bB34'��B�* ��©;Å J <>4�?

Ç © 7Èh�B�� I�?DI/� <>9;���H<!� IdC39"B>I$4A9b4�� GDI�<!�'��I$4�?�9"Y�7�@�I�� <��<�5�b9b4DC�9��S��B34�?�2'�&��I$?¦o.xK¡/n!l�xHI$<��RG@Ç ©;Å J 9���?��R<¹9%4T=

<>4�?G��B345�T2���� I$4���I�� � 9���<!� 9bB34èB�* M P �� ± IsÇ ©;Å J U�9;��?DB34DI�2'�R9b4DC!@%<xB!({�S�b<��('�9�I����b9"~$<!� 9bB34­I>�%UP7

`4DB�� GDI/�4� �R9;��'F� G�<!��9�I��T<>4V�5�;<��79%9"� GF� G�9��dI&YD<����5�bI�9�����B69b4���ITC��R<!��I§B32'��� GDI�� � <!��I<Bx<!�R9b<!Am� IWÇ5©

<>4�?�25��I�h6B34���IEH<!�.� B���B�9�����2���I�<>4�9b4�?�9;�T<!��B��dBr<!�R9;<!A5� IS� G�<!�È��I����;�?9%G�9��PG���B%����B34DI$4H��B�* ��©�9��d� G�I�� �R2DI

� 9 C34/<��¡7fR 9"� GF� GDI�� �R2�IW�R9 C34�<��z9b?DI$4�� 923/I$?¸=�9��d9;��� �R9"B�9b<��i��B6I�� � 9���<!��I�� GDI�� �P2DI�� BQ�T<!� 9bB34ÞB�*}� GDI�� <!�RC>I/�T7

Î�G�9;�4w0n!rsr2p[k5u.qs �£^kml n�w[x�¥!�#l�x �'�RB�?�25��I���<>4èI/B>I$4GA�I/� ��I/�M�RI�� 2'�"�T7

��9 C32�� Ife<� GDB¹9_��� GDI��5� B�����B�*a� �P<���'�9b4DC�I/���RB��0�4I­I�� � 9���<!��I$?C� B#�T<!� 9 B34 � � �R2�If� B#�T<!� 9 B34mU¢B�*È�!�<� 9���2#(

�b<!��I$?@�R2�4'�/=�9�9"� G � � ¯ L!° �#=+6 � ¯ �#°�L!=HÏ�� ¯ �#° N�<>4�? � ¯ �#°�Lx7 �g9�B>I*G�2�4�?�� I$?���� � I$<��<�^I ¯ �!���HUS9�I/� I

2'��I$?¸=�9%9"� G�� I��R<����5�b9b4�C�?�B34DI�<!�%I/B>I/���G����I/��7XÎ�G�I���B��G3/C32��RI�� I�� 2'�"��I$?G*,� B%� 2'� 9b4DC�� G�I�B��'� 9���<��i� <��W(

�5�b9;4DC�?�9;� � �R9"A/2'� 9 B34­I �%Ud<>4/?F� GDI<��9b?�?5� I43/C32'� IW*,� B%� 25� 9b4DC8� GDI<��B%���b<!�m� 9b4DC��'� B#��I$?�2�� I>7WÎ�GDIWA�B�� ��B%�

3/C32��RI4� GDB¹9_��� GDI4� I��R2'�"�S*,�RB%� 2'� 9b4�C�<�� I����HB��'� 9��§<��¸� <����5�;9b4DC�?�9�� � �R9�A/2�� 9 B34��©;Å J ¯ 6#IsÇm©;Å J � Ç5©«U�<���9b4

`_B�9"� ~TB32'��I)L�N�N��%UP7S�GDI���B��$3/C32��RIG�<���L �^�R2/4'�}9�9"� GF<!A5��B%�b2���I�Bx<��b2DIB�*i� �R<��('�9b4�C�I/�0� B��0��I&Y#��ITI$?/9b4DC�L/�

<!��� I$<�� �B34'��I�=� GDIW��9b?�?'�bI�3/C32�� I�G�<��WL��Q=�<>4�?7� GDI^A�B�� ��B%� G�<���e!��7�5�9���9;�b<!�0�"��=�� GDI:��B��F3/C32��RI*G�<����

�R2�45�HI&Y#��ITI$?�9;4DC�� GDI^e!�<�;9���9"�/=#� GDI4��9;?�?'� I_3/C32�� I�G/<�����='� GDI4A�B�� ��B%� G/<��MO�7

Î�GDI=<!A�B¹B>I8�/<!�R<��!I/��I/����B%�^A/9b4�<!� 9 B34 9��^���b9 C3G����"�Þ?�921�I/� I$4��4*,�RB%� � G�<!��B�*M`_B�9�� ~TB32��$I)L�N�N��%U.=}9%9"� G

����<��;� I/���/�b2�� ��I/�?DI$4'� 9"�«��A�2����b<!� C>I/�M��� <!��I�I���2�<!� 9 B34GBx<!�R9b<>45��I>7}R 9"� G�� GDI$9"�_��B34#3/C32��R<!� 9bB34T=�� GDI4� I��R2'�"���

<!� If�R9���9��b<!�SA/2'�¿� G�I�?�921�I/� I$4'��I��SA�I/�{9�ITI$46?/921�I/�RI$4H���'�RBQ��I$?�2'� I���<!� If���§<���� I/�$7

� �f)� � � � �

�_4½� G�9����/<!��I/��9�I��'� B���B%��IA< C>I$4DI/�P<���*s�R<��!I/9�B���'­*­B��Cn! ��«r¤q, Æx�h�B34H��IÞEH<!�0� B���B%���/2'� <!� 9 B34'��*­B��

e�O

Page 77: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

time

erro

r

0 20 40 60 80 100

-40

-20

020

40

time

erro

r

0 20 40 60 80 100

-40

-20

020

40

time

erro

r

0 20 40 60 80 100

-40

-20

020

40

� �GF � C�� ) �7�G�I8� �P<���'�9b4DCF� I�� 2'�����^*s� B%� 2'�R9b4DC7� G�� ITI�921�I/� I$4��W� I���2DI$4�� 9b<���h�B34H��I EH<!�0� B

�!I/� GDB�?'�T7^RÞI§2'��I$? ¯ �!���6<>4�?F� I�� <����m� I$? <!�|I/B>I/���7����I/��7:Î%GDIW�H(�<�YD9��|9���� GDI!?�9;� � <>4'��I

A�I/�{9�ITI$4�� GDIKI�� � 9���<!��I$?�<>4�?»� �R2DI7��B%� 9"� 9 B345��B�*f� G�IC� <!� C>I/�T7 Î:B��¸�¦� I�� 2'�"����*s� B%� 25� 9b4DC

!©;Å J �'� I��0�&�R9"A�I$?$A��VI �%U.PT��9b?/?'� I��È� I��R2'�"����*s� B%� 25� 9b4DC�� GDI:��B%���b<!�5�R9b4DC��'� B#��I$?�2��RI�P'A�B�� ��B%���

� I�� 25�"���S*,�RB%� 2'� 9b4�C�I>�%UP7e�N

Page 78: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

?���4�<���9���� �Q����I��<�T7@�]�*9;�f�/� I$<!�W� G�<!��<����!B%� ��<����È� G�I=<¹Br<>9��;<!A5� I���I���2DI$4�� 9b<��¿h6B34���I6EH<!�0� BG�5� BQ��I$?/2�� I��

�T<>4�A�I�2�4�923/I$?!2/4�?DI/�È� G�9;�È*s�R<��!I/9�B���'�7dÎ�G/9��zC>I$4DI/�R<�����I/� � 9b4�Cf�'� BgB�9;?DI���<f��B%���!B34§C��RB32�4�?<*­B��¢2/4�?DI/� (

� � <>4�?/9b4DC!<>4�?A9��<�'� B¹B�9b4DC:Bx<!�R9 B32'���R9���9��b<!�S�!I/� GDB�?'��?�I/B>I�� B���I$?�*°B��_� ��I��T923m���!B�?DI����T7?�]�%<�����B��'�RB¹B�9b?DI��

<�C>I$4DI/�R<���C32�9b?/<>4'��I�B34FGDBg9 � 2'�PG��5� BQ��I$?/2�� I��¿� GDB32'�b?�A�I2'� I$?=9;4��'�P<��&� 9���I<>4/?=G�B¹9 ?�921�I/� I$4�� � � �P9���'#� �

?DI/B>I�� B���I$?+*­B��<� ��I��T923m���'� B�A5� I����^�T<>4½A�I$��B%�fA/9;4DI$?+��BY<��RG�9 I/B>IG�§<�Y�9���2'��I����T9 I$45�&�>7V�i4½��<!��� 9��T2#(

�b<!��=T9�I!?�9����T25����I$?7��I/B>I/�R<��&'>I/� 9���� 2�I��9;4K9;���5� I��!I$4�� 9b4DC8��I���2DI$4H� 9;<��ah�B34���I=EH<!�0� B$�!I/� GDB�?'�/=�4/<��!I��"��=

�RG�B39���I��HB�*i� GDI�� I��T2'�0� 9"B>I�� <����m�b9b4DC�?/9�� � �R9"A�2�� 9 B34 !©;Å J =D<>?�Bx<>4H� <xC>I���<>4�?F?�9�� <>?�Bx<>4�� <xC>I��HB�*i� I�� <��<�5�b9b4DC

<>4�?�� G�I$9"�����RG�I$?�2'�b9b4�C�=�I��8�T9 I$4H��2'��I|B�*?h6B34���I�EH<!�.� B<� <����5�bI��/=�<>4�?è@%<xB!({�S�b<��('�9�I����b9�~$<!� 9 B34�7

��I�� 9b?DI��4� GDI�B�AQB�9 B325��<!�'�5�b9;�T<!� 9 B34 B�*S� GDI<��I���2DI$4H� 9;<��Èh E¦9b4F� GDI���� <!��I�� �/<���I��!B�?�I����/=¸� GDI/�RI§<!� I

��<>4��FB�� GDI/���'� B�Am� I��<�S� G�<!�M�T<>4@A�I�*­B��0��2'�b<!��I$?A<���<�?���4�<���9���� �#� ��I�� <>4/?G��B%�"B>I$?A25� 9b4DCW��I��RG/4�9���2DI��

?DI����&�P9"A�I$?è9b4$� G�9���<!�0� 9��/� I>7 �DB���I&Y�<��<�5� I�='� GDI�5Q��5��5� BQ��I$?/2�� I4�T<>4GA�I�A/2/9��"�H9b4H��B�<<h E�h E ���RGDI���I4��B

�'� B�?�2'��Id<��!B�� IdI����T9bI$4H��� �R<>45� 9"� 9 B34��'� B���B%� <��Æ�RG/<>9b4�7ÈÎ�GDI�J<�� � 9b4DC%� � �RI]D_I��&� 9 B34$�5� BQ��I$?/2�� Id?DI����&�R9"A�I$?

9b4 5�I��&� 9 B34�O�7�L7�T<>4�A�I�25��I$? 9b4���B%�^A/9b4�<!� 9 B34�7 Î�G�9����{�Q��I6B�*fh�B34H��IÏEH<!�.� B­�!I/� G�B�?'�FIK��B%�!I/� 9;�!I��

�T<���� I$? � ��B34#3�C32��R<!� 9 B34#(§A�9b<���I$?:h6B34���I�EH<!�0� B �HU�G�<¹B>I�A�ITI$4f��I�� ��I$?�I&1�I��&� 9"B>I�*­B��d� 9���2'�b<!� 9b4�C�A/9 B���B%�"�#�!I/�0�

I½8�I$<��PGT=jL�N�N��%UP7�5�ITI��]��9%9b4DC�I/��<��¡7MI)L�N�N���U.=Æ�|B34DC!I/��<��¡7MI)L�N�N���U.=5RÞB34DC=<>4�?è8:9;<>4DCGI)L�N�N%��U�<>4�?FB�� G�I/�0�

*­B��<�!B�� IFI&Y�<��<�5� I��T7VRÞI�GDB���I�� G�<!��� G�I8�RI�� 2'�"���:� I/��B�����I$? GDI/� I��T<>4E� � 9;�*2'�;<!��I$��B�� Iè9b4H��I/�RI�� ��<>4�?

I&1�B�����*s� B%� B�� GDI/�_� I���I$<!�0�PGDI/�0��B34G� G�9��S�{�Q��I�B�*a�'� B�A5� I����T7

� � !�� � & � �

����� ����� / ' � , 7 * ,.')( � * � (+7 * �;% ( *�5 ' ��5���( �&� � , � (+*<'�= ��/ ' �&��36�$' ���$';9:� � ��,.*�'Î�G�9;�<���RG�I��!I�9���35�0� �§?/9����T2'��� I$?½A���J<�� � 9b4DC%�7I)L�N%���#=5�I��&� 9 B34�e�7 �%U§<���B34DIC9�<¹�E��B�?DB 9�����B���� <>4'��I

� <����m�b9b4DCD7�Î�9bI/�R4DI/�¦I)L�N�N#L¹U�C>I$4DI/�R<��;9"~TI��M� G�I�?�9;���T2'��� 9bB34A2�4�?DI/�_� GDI�GDI$<>?�9b4DC ��9b4�?DI/��I$4�?DI$45��If�RG�<>9;4'�/= �

�!�

Page 79: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

<>4�?è9��S�T<����bI$? � h�I/� � B���B%�b9"~TI$?69b4�?DI/��I$4�?DI$45��I_� <����m�b9b4DC �fA��=8:9b2�I)L�N�N��%UP7SÎ�GDIdC>I$4DI/�R<��T���PGDI��!I4�T<>4�A�I

� � <!��I$?�<���*­B%��� B¹9_�T7

5�2��5��B%� I�� G�<!�S¨ÈIsÇÆU�9�� '�4DB¹9�4�2�����B�<�4DB��0�§<��b9"~$9b4DC4��B345� � <>4H�/=�<>4/?<9�I<!� Id<!A5� I_��B�?'�R<�9 9b4�?DI/��I$4#(

?DI$4���� <����5� I���*s� B%� �IsÇÆUP7:` hK<!��'>B¹BF�RG/<>9b4 � B J ¬QB � ¬/°/°/° "<�T<>4FA�I:��B34'��� �R2'�&��I$?F9%9"� G7� GDI:� �R<>4'�R9"� 9 B34

*½2�4'�&� 9 B34

�½IsÇj¬���U ¯

���� ��� �I��'UQ��9b4 � L!¬�� ������ � � � "%¬ 9"* � �¯ Çi¬

L � !���? � �I���UQ�§9b4 � L!¬ � � � �� � � � "¹Ë ��¬ 9"* � ¯ Çi¬

9%GDI/�RI JWIsÇÆU ¯ ¨ÈIsÇTU � �IsÇÆU9����T<���� I$?7� GDI8qs|Mk�n!l[v§p� mw.x�l p�vKq>n�I­B���q,|_k#n!l&v§p! mw[x�ÀXx&q"£!¾�v;UP7��_4H� 2/9"� 9"B>I��"��=�� GDI

� �R<>4'�R9"� 9 B34�*s� B%�B � ¯ Ç8��B�B � Å J ¯ �!9���<��/��B%�<�5�b9�� G�I$?�A���C>I$4DI/�P<!� 9b4DC*<>4�9b4�?DI/��I$4�?�I$4H�È� <��<�5� I�*s� B%�

�I # U.=�<>4�?�� GDI$4 �)� G�9b4�4�9;4DC �d9"��?DB¹9�4�A/<���I$?!B34§<4��B%���/<!�R9;��B34!B�*�� GDIM��B���� I�����B34/?�9b4DCd9�����B���� <>4'��I��P<!� 9 B%�

J:I��'U%<>4�? JWIsÇÆUP7��«���T<>47A�I^� GDBg9%4G� G/<!��¨ 9��%<>4 9b4�Bx<!�R9b<>4��%?�9�� � �P9"A/2�� 9 B34FB�*}� GDI:��B345� � �R2'�&��I$?7h�<!�('>B¹B

�RG/<>9b4�7 ��B���I�� G�<!��� GDIè<!A�B¹B>IG���PGDI��!IA9���B34'���Ï<�� ��I��T9b<��¢I&Y�<����m� I�=È� G�<!�<�!B��RI$� I/�R9 B32'�Wh�I/� � B���B%�b9��)(

J<�� � 9b4DC%��<�� C>B��R9"� G5�<�}�!B%���¿��B%�<��B34'�"�<��< '>Id?DI/��I$4�?DI$4��}� B#�T<��m��B¹B>I���9b4'����I$<>?�B�*:9b4/?DI/��I$4�?DI$4��zC%� B�A/<��

D 2'���5�T7d`4èI$9 C>I$4#(�<>4/<��"�Q�R9��HB�*?� G�9����RG/<>9b4A9��S�'�RB¹B�9b?DI$?è9b4è8�9b2VI)L�N�N��%UP7

5�2��5��B%� IX9�IM�T<>4�4DB��¢?�9"� I��&���"�:� <����5�bI¿*s� B%� ÆIsÇÆUaA/2'��G/<�B>I�<��RI/B>I/�0� 9"A5�bIShÞEShÞE+�'� B#��I$?�2��RI�IK��B%� �

� 9b4�C%� I&({� ��I/�½h EShÞE ���RGDI���I8� <!� 9;�)3/I��^� G�9��:��B34�?�9"� 9 B34�U.=j9%9�� GV� �R<>4'�R9"� 9 B34V*½2�4'�&� 9bB34�WIsÇi¬��'U.=d� G�<!��G�<��

�IsÇTU�<���9"����9;4HBx<!�R9b<>4��z?�9�� � �R9�A/2�� 9 B34�7¸Î%GDI$4W9¢I�G�<�B>I ÆIsÇÆU�WIsÇi¬��'U ¯ �I��'U�WI��m¬ ÇTUP7}J%I$45��I�=�9"*59¢IM��B34/?�2'�&�

<@h�I/� � B���B%�b9��^� ��I/�+9%9"� G WIsÇi¬��'U�<��^� GDI��'� B���B%� <��È�PG�<>9b4Ï<>4/?¦¨ÈIsÇÆU�<��^� GDI�� <!� C>I/��?�9���� �R9"A/2�� 9bB34T=T� GDI

� I]D�I��&� 9 B34��'�RB�A/<!A/9��b9��{���T<>4@A�I4��B%�<�/2���I$?A<��

Ï�� ¯ �§9b4��aL!¬ ¨ÈI���U�WI���¬ ÇÆU¨ÈIsÇTU�:IsÇj¬���U�� ¯ �§9b4��aL!¬ ¨ÈI��'U��IsÇTU

¨ÈIsÇTU��I��'U�� ¯ ��9b4��dL!¬ J:I��'UJ:IsÇÆU�� °

J�I$4'��I�=¸� GDI��'�RBQ��I$?�2'� I�?�I����&�R9"A�I$?K9b4Y5�I��&� 9 B34 ��7 �A9���� � 9��;�iBx<��b9b?T=ÆA/2'���T<>4Y4�B$�bB34DC>I/��A�I:�T<����bI$? ��9b4�?DI&(

��I$4�?DI$4'��I��PG�<>9b4���<!�5�'� B3<��RG�7

�#L

Page 80: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

��� � � 5 143 % (+, (+*�5 ' ��/ ' � 5 - � ���?*�'�( ��� � � ,.143;-��4> �2�$9�( * 5 ' ���<� A�DB�����B%���/2�� 9b4�C�� GDI�9¢I$9bC3GH���/=59¢I�G/<�B>If��B���B%���/2���I

ÏaI���© � � ¬�ª?©�8 J ¬�� ©�8 J U ¯ 5?I���© � J 6 J � ©�8 J � U � L ��� I���© � � 6 � � ©�8 J � U � � 5aI���© � � 6 � � ©�8 J � U � L ��� I���© � J 6 J � ©�8 J � U � ¬

<>4�?�*­B��%9��<�/2�� 9b4DCW��9��0� 9b4DC�?�<!� <#='9�I�4DITI$?

ÏaI �[© ¯ L � � ¬�ª?©�8 J ¬�� ©�8 J ¬���©{U ¯ 5?I�� © � J 6 J � ©�8 J � U � L ��� I�� © � � 6 � � ©�8 J � U �ÏaI���© � � ¬�ªa©�8 J ¬�� ©�8 J UÏaI � © � �[© ¯ L!¬���© ¬ � ¬�ª?©�8 J ¬�� ©�8 J U ¯ 5?I � © �� � 6 � � ©�8 J � UL ��� I���© �� � 6 � � ©�8 J � UÏaI � © � �[© ¯ eQ¬���© ¬ � ¬�ª?©�8 J ¬�� ©�8 J U ¯ 5?I ��© �� J 6 J � ©�8 J � UL ��� I���© �� J 6 J � ©�8 J � U

5�2��5��B%� I<� G�<!�:� GDI��5�R9 B��*?/9�� � �R9"A�2�� 9 B34F*­B�� � 9��MÏ � I J ¬ � U.=d� GDI$4�C39"B>I$4½��B%���5� I/��I=B�A5��I/�0Br<!� 9 B345�/=d� GDI

��B%� ��I/�R9 B��

ÏaI �-� 6 J ¬/°/°/°�¬�6 � U <­Ï ��I � U�I&Y#� � � ��© ? �

I�6 J © � J 6 J � ©�8 J � U � � I�6 � © � � 6 � � ©�8 J � U �e � °

R 9"� GDB32��d� B%���gB�*�C>I$4DI/�R<��b9"�«��=%9�I¿� < '>I?Ï��%I � UT��B�A�IH2�4�9"*­B��0�#B34 � �#¬�L � � 7?Î�GDI$4:� GDIS��B%� ��I/�R9 B��z?�9���� �R9"A/2�� 9bB34

9���� 9;���5�b923�I$?�<��

ÏaI �-� 6 J ¬/°/°/°�¬�6 � U <�I&Y#��� I J � � J U �

e � � J� I � � � � U �

e � �� � ¬ ��� J ¬ � � L!¬

9%GDI/�RI

� J ¯ I��© ? �

6 �J � ©�8 J � U8 J� � ¬

� J ¯ � � J��© ? �

6 J © 6 J � ©�8 J �

� � ¯ I��© ? �

6 �� � ©�8 J � U8 J� � ¬

� � ¯ � ����© ? �

6 � © 6 � � ©�8 J � °

Î�BF�R<����5� I8*,�RB%�-� G�I�� �R2/4'�T<!��I$? 4DB��0��<��H?�9;� � �R9"A/2'� 9 B34 B 1 5?IsÇÆUf9%9"� G B � ��=È9�GDI/� I$¨ÈIsÇÆU*9��

� � <>4�?/<!�R? 4DB��.��<���?DI$4'�R9"�{��=S9�I�2'��IC� GDIG*­B%��� Bg9%9b4DCV� � �P<!��ITC��>7 R�G�I$4 � �#=�9¢I7� 9����5���­��B34�?�25�&��<

��e

Page 81: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

� 9��<�5� I�4DB��0��<��d�P<>4�?DB%� 4�2'�fA�I/��C>I$4�I/�R<!� 9 B34T=g<>4�?Y?DB�� I]D�I��&� 9 B34�2�4H� 9;�j9�I§G�<¹B>I�<G�R<����5� I�� <!� 9���*,��9b4DC

B � �r7 �DB�� � � �#=3I�� ��I��T9b<����"��9%G�I$4 ��9��jA/9 C�=!9�IH25��IHI&YQ��B34DI$4�� 9b<����R<>4�?DB%�ÍBx<!�R9b<!A5� IX9%9"� G^� GDIS� I]D�I��&� 9 B34

�!I/� GDB�?�7

5�2��5��B%� I¿� G/<!�}� GDI�I&Y#��B34DI$4�� 9b<��/?�9;� � �R9"A/2'� 9 B34 ��� � 8���� � 9;�È��BfA�I�2'� I$?�<��¢<>4§I$4HB>I��bB���*½2�4'�&� 9 B34T=H� GDI$4

9�I�4DITI$?���B�3�4�?$� GDI4��9b4/9��*25����B345� � <>4H�_AG��B<� G�<!�

5?IsÇ � ��UL ��� I��U � � � � � 8���� � ¬ Ç��b�

Î�G�9;�HC39"B>I���2'��� G�I4A�I�� ����B%�b2�� 9 B34$*­B�� � �

��¯ I&Y#� � I � �� � e � � �/U � e "� e!¨ � ��I)L ��� I�/U U °

Î�GDI�<��/��I/�'� <>4'��I8�R<!��I�*­B���25� 9b4DC$� G�9���I&YQ��B34DI$4�� 9b<���?/9�� � �R9"A�2�� 9 B34 9���� GDI$4»L � � 7$Î�B�<��PG�9 I/B>I$��9b4�9;�*2'�

� I]D�I��&� 9 B34��R<!��I�=m9¢If*­2��0� GDI/�S3�4�?$� G�<!�M� GDI�A�I�� ���RGDB39;��I4*°B�� �� 9��

��� ¯ I� � � � � ��U � eQ°

R 9"� G�� G�9����PGDB39���I�B�* � �d<>4�? � =�9¢I*9����5� I���I$4H�S� GDI4�RI]D_I��&� 9 B34@�!I/� GDB�?èB�*jB>B34 �%I$25��<>4�4VI)L�N��#L¹UP7SÎ�GDI

� I]D�I��&� 9 B34C�R<!��I^*­B���� G�9��_���RGDI���I�?DI��&�RI$<���I��d<�� �*9b4'�&� I$<���I���=�<>4�?@� G�9��_�R<!��I�9���B>I/���C����<����T*­B����!B�?DI/�R<!��I

��B��b<!�RC>I ��I­I>7ÊCD7"=m*­B�� � ¯ �#¬�L!¬0eQ=�� GDI�� I]D�I��&� 9 B34G�R<!��I���<!� If��7 e���=���7�L�eQ=�<>4�?G��7 �H��UP7

����� � 5 143 % (+, (+*�5 ' � � 5 7 � , 7 =8� ( �07 , 9�� *�'�= > �2�$9�( * 5 ' �����)A8�I/�2��©;Å J ¯ I�� ©;Å J � J � ¬/°/°/°¹¬�� ©;Å J � R �� ± � USA�I�� GDI�B�A5��I/��B>I$?G�R9 C34�<�����<!�_� 9��!I ® � Lx7}Î�GDI$4

3dI���©;Å J � Çm©;Å J U ¯ I)L � Ï���U�� L�R �� ± � 8��� I � � U<R �� ±

�©;Å J�� �� Ï�� � L

�©;Å J

R �� ±�� ?�J

L� R �� ± 8 J 5 �!I�� ©;Å J � � � � Çm©;Å J U � � 8��� I � � U<R �� ± 8 J

I �©;Å J � L¹U �¯ � Ï�� R �� ±�

� ?�J5 � I�� ©;Å J � � � � Ç ©;Å J U � I)L � Ï��gU � � � 8��� � R �� ± 8 J �©;Å J��

���

Page 82: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

*­B�� ®d¯ �#¬�L!¬0eQ¬/°/°/°s=�9�GDI/� I05 ��� I/*°I/�.����B<� GDI�4DB��.��<���?DI$4'�R9"�{��9%9"� G$Bx<!�R9b<>4'��I � �r7 ��2��0� GDI/�0�!B�� I5 � I�� ©;Å J � � � � Ç ©;Å J UA6#IsÇ ©;Å J � ª © U ¯ L

¨ 6 � I&YQ�� � I�� ©;Å J � � � � Ç � J �©;Å J U �e � �

�IsÇ � J �©;Å J � Ç � J �© � Ç � � �© U �

e�I�6 � e%U �� �

¯ �� e!¨ �'©;Å JI&Y#�

� � IsÇ � J �©;Å J � � ©;Å J U �e � �©;Å J

� �

9%GDI/�RI

� �©;Å J ¯ � � 6 �6 � � � � �

¬ � ©;Å J ¯ � � ©;Å J � � �� �

�Ç � J �© � Ç � � �©

I�6 � e%U � � � �©;Å J<>4�?

� ¯ e �'©;Å J� e!¨ � 6I&Y#�

�� L

e � � �©;Å J � � �� �

�Ç � J �© � Ç � � �©

I�6 � e%U � � � �©;Å J� �©;Å J � �

J�I$4'��I�= 3dI�� ©;Å J � Ç ©;Å J UA6�IsÇ ©;Å J � ª © U�9���<���9"YQ� 2��RI|B�* ©;Å J 4DB��.��<���?�9�� � �P9"A/2�� 9 B34�7

� � � � � ��� � � `_B�9"� ~TB32'��=¸��7dI)L�N�N��%U.=a` � ��B#�RG/<�� � 9��W� 9���2'�b<!� 9 B347�H<��>I�� 9;<>4Y<!�'�'� B3<��PG7��BG�*2'��� 9"� <!� C>I/��� �R<���'�9b4DC�=jÄ����

��l)n¹w.x0x0¥!qs �£!u�n!  ��p%¥%p!l.± ,�n! mp!lWp! m¥�Ì^p!¢¹q"£Hp�vKq>n! '=a� � )Æ='�'L[(§���D7

��I/��~$2�9b4/9>=HE7"=���I�� �/= �*7t\�7"=_\d9�� 'Q�/=XR#7¹@�7"=�<>4�? 8�<!�R9"~/~$<#=%E7_I)L�N�N��%U.= � ����4�<���9��@��B34�?�9"� 9 B34/<���9b4�?DI&(

��I$4�?�I$4'��If�!B�?DI�����<>4�?@hK<!��'>B¹B7�RG�<>9;4@h�B34H��I§EH<!�0�bB��!I/� GDB�?'�/= ���#ºi��|<x/l&º0,mv§p%vKq;u&v§º4��u.u&n�wgº�=���B

<!�5��I$<!�V7

EH<!�0�;9b4T=5�7 �a= �gB%��� B34T= �*7¸\�7�<>4/?K5Q��B!1�I/��=T��7�5�7?I)L�N�N�e%U.=X� ` h�B34H��I�EH<!�0� B=<!�'�'� B3<��PG7��B=4DB34/4DB��0��<��

<>4/?64DB34'�;9b4DI$<!��� � <!��I&({� �/<���I��!B�?DI��b9;4DC�= ��#n!�#l[ mp!r?n)¡^vs¾#x���|<x/l.q>w0p�  ,mv§p�vKq,u[vKq>w0p!r¸��u.u&n�w�qKp%vKqKn! 5=� �

�HN��g(«�!����7

EH<!����I/�¹=�E7 �!7�<>4/?V�|B3G�4T=�@�7}I)L�N�N���U.=V�06d4­\|9"A'A5��� <����m�b9b4DC8*°B��:� � <!��I�� ��<���I��!B�?DI����/= � Mq>n!|�x/vKl[q ��p!=

i�'=m���'L[(«������7

���

Page 83: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

EH<���I��;�b<#=m\�7jI)L�N�N%��U.=T�R5Q� <!� 9�� � 9;�T<��:9b4�*­I/� I$4'��I|<>4�?@h6B34���I�EH<!�.� B!<�� C>B��R9"� G'���/= �¦¼mx&u[vK= � =me��HNg( �����D7

EH<���I��;�b<#=m\�7/<>4�?6@�B�A�I/���/=�E7 �g7jI)L�N�N��%U.=T��@%<xB!({�S�b<��('�9�I����b9"~$<!� 9bB34AB�*d� <����5�;9b4DCW���RGDI���I��/= � Mq>n!|�x/vKl[q ��p

� =mO#L[(«N��D7

EHGDI$4¸=�@�7�<>4�?�8:9>=#Î|7�I)L�N�N��%U � �S�b9b4/?§@�I�����B��R<!� 9 B34�B�*:8:9b4�I$<!�0�"�W��ITC��P<>?DI$?$�9;���&� I/��I�5�9 C34�<����ÈA��8\|9"A'A5�

5D<����5� I/��= ��Ä���� ��¼�l p! �u/p%w�vKq>n! �u�n!  ,5q"£� mp�r��Sl n�w.x&u0u[q, �£%= � � =me��'L/��(«e��'L ��7

EHG�2��0�PG�9����>=}\�7 `*7MI)L�N�O�N%U.= �R5Q��B#�RG�<���� 9���h6B�?DI����W*­B��<J�I/��I/� B>C>I$4DITB32'��� ��` 5�I���2�I$4'��I��/= �DM�#r,r�x/vKqs �n)¡

/¦p�vs¾#x/|�p%vKqKw.p!r Mq>n!r"n�£�z � �'=��!Ng(«N��D7

��<>9"�¹=3@�7oE7x<>4�?!13<�1�ITI�=%��7 hÏ7'I)L�N%�!e%U.=�� h6I/� GDB�?'�gB�*�I�� � 9;��<!� 9 B34:*°B��?��<!��'>I/���z?/9���I���2�9��b9"A5�R9b2'��= � ��w.n! mn �

|�x�vKl.q>w0p ��� ='�HN%�¹(«�#L&�D7

\I��"*½<>4�?T=�`*7 c�7r<>4�?�5���9"� GT=�`�7 ��7 h�7#I)L�N�N!�HU.="�R5�<����5�b9;4DC!(§A/<���I$?4`_�'�'�RB3<��RGDI��j��B|E�<����T2'�b<!� 9b4�CMh�<!�RC39b4�<��

�I$4'�R9"� 9 I��/= � ��n!�#l[ mp!rjn)¡^vs¾#x���|�x&l[qKw.p! B,mv§p�vKq,u[vKq>w0p!r¸��u0u&n¹w/q>p�vKqKn� �± � = ��N�O��H�%�%N�7

J<�� � 9b4DC%�/=�R 7 �!7iI)L�N%���HU.=T� h6B34���I�EH<!�.� B�� <����5�b9;4DC:��I/� GDB�?5��2'� 9b4DCWhK<!��'>B¹B��RG/<>9b4'��<>4�?$� GDI$9���<!�'�5�b92(

�T<!� 9bB34'�/= � Mq>n!|�x/vKl[q ��p!= � �m=mN%�¹(0L/�%N�7

\B��R?�B34T= �*7�1D7"=�5�<�����B34T=j��7�1D7g<>4�?�5#�§9"� GT=i`*7 ��7 hÏ7�I)L�N�N��%U.=V� ` 4DB¹B>I���<!�'�'�RB3<��RGV��BK4DB34'�b9;4DI$<!�[Zr4DB34

\|<>2'��� 9b<>4½�H<��>I��R9b<>4E� � <!��IèI�� � 9��§<!� 9 B34T= �¦Ä�� � �Sl n�w.x.x�¥!qs �£�uGn�  ��p%¥%p�l@p! m¥ ,mq2£% mp!r �Sl n�w.x&u0u.qs �£%=

� ��� =jL/�H�¹(0L�L ��7

\B��R?�B34T= �*7�1D7"=�5�<�����B34T=X��7�1D7¢<>4�?EcÈ9%9;4DC�=HE7 h�7MI)L�N�N��%U.= � �H<��>I��R9b<>4b� � <!��I�I�� � 9���<!� 9bB34E*°B���� �P<���'�(

9;4DCÏ<>4�? C32�9b?�<>4'��IA25� 9b4DC�� GDICA�B�B���� � �P<!��3m�"��I/�¹= �E��Ä ��� ��n!�#l[ mp!r�n ¡��S�#qK¥%p� mw.x&±��dn� 5vKl)n�r�p! �¥

Êfz! mp!|Wq>w&u[=i� ¸=iL&� ���!(0L&��� ��7

���

Page 84: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

J<!��B>I/��=a`*7}I)L�N�O�N%U.=��¸n!l�x�w.p�u[vKqs �£�± ,mvKl[�'w�vK�#l�x�¼jq,|�x ,Æx&l[q>x&u /Vn�¥Hx/r u�p� m¥7vs¾#x �Wp!rt|<p! ��}qsr¤v{x/l:=�EH<��W(

A5�R9b?DC>I>7 ������EH<��^A'�R9b?�C>I �4�9"B>I/�.� 9"�{� ��RI����T7

J�I$4�?�����=M��7 ��7H<>4�? @�9��RG/<!�R?T=%1!( ��7;I)L�N�N#L¹U.=L��8:9 '>I��b9bGDB�B�? I/Br<��b2/<!� 9 B34b*­B��F?���4/<���9��G�b<!��I$4��8Br<!�R9;<!A5� I��

��B�?DI��;�T7 �S�i4��dn�|Mk��#v§p%vKqKn! �p!r ��w.n! mn!|Wq>w&u¿p! �¥ ��w0n� mn!|�x/vKl[qKw/u0±�EHG�7�Lx7j`��<��<>4¸= J�7 h�7"= ��I������ I/��= ��7 `*7

<>4/? ��<>2T=¹8�7 ��7aI­I$?5�.UP7}�B��R?�� I��PGH�/�¿�4�b2�9¢I/�

J��2���~TI�� I/��=¸h�7�<>4�?����2�45���RGT=TJ*7¹@�7ÈI)L�N�N��%U.= � h6B34���I�EH<!�0� Bè<!�'�5� B�YD9���<!� 9 B345�M*­B��|C>I$4DI/�R<��d� � <!��I<���/<���I

��B�?DI��;�T7 ����x[u�x�p!l w&¾���x«k#n�l[v�� � =5c}Î�J^=�� �2'�R9��RG�7

�)� <!�R?T=}h�7z<>4�?½�S�b< '>I�=}`*7�I)L�N�N��%U.= �RE�B34���B32���� �R<��('�9;4DCFA��¦����BQ�PG�<�� � 9��8�'� B���<xC3<!� 9 B34 B�*_��B34�?/9"� 9 B34�<��

?�I$4'� 9"�«���#=�9b4 �dn�|Mk��#v{x/l��iq;u[qKn!  � � � ��� ����=��7¿��2�YQ��B34 <>4�? @�7HEH9"��B%���b<EI­I$?'�.U.=%5Q�5�R9b4DC>I/���

��I/9� �B��('�7

�]��9%9b4DC�='h�7"=�EHB�YÆ= �*7/<>4�?C�|B34DC�=5`*7aI)L�N�N���U.=6�R5�I���2DI$4�� 9b<��:9��<�/2�� <!� 9 B34$*­B��_��2'�"� 9��bBQ�T2'���;9b4 'x<xC>I�<>4�<��2(

�#� 9���= � �Sl n�w.x.x�¥!qs �£!u�n)¡fvs¾#x�Ì^p�vKq>n! mp!rT�Mw.p%¥Hx/|:z�n)¡ ,�w�q>x/ mw.x&±��:,#��=��i�'=iL�L���O��!(0L�L���O�O�7

��9"� <xC3<¹9�<#=5\�7�I)L�N�N��%U.=�� h6B34���I|E�<!�0� B�3m�"��I/�¢<>4�?<���!B�B�� GDI/�X*°B���4�B34#(]\d<>2'���R9b<>4�4DB34'�;9b4DI$<!�¢5Q� <!��IM���/<���I

��B�?DI��;�/= � �#n��Ql[ mp!r?n)¡ �dn!|_km��v§p�vKq>n! mp�r?p! �¥ �Sl)p.km¾HqKw.p!r:,mv§p�vKq,u[vKq>w&u.= � =iL[(«e���7

�|B34DC�=?`*7"=g8�9b2T=�1�7o5�7"=�<>4�?¦RÞB34DC�=aR#7 J*7¿I)L�N�N���U.=V�R5�I���2DI$4�� 9b<���9;���/2�� <!� 9 B345�|<>4�?V�H<��>I�� 9;<>4­��9;��� 9b4DC

?/<!� <��'� B�A5�bI��<�/= � ��º¸��|�x&l/º ,�v§p�vKq;u&v§ºM��u0u&n¹w¹º;= ��¸=me%�!Og(«e�O�O�7

��� B>C3G¸=X`*7"=¿�¿� Bg9%4T=Xh�7"=Sh�9b<>4¸=H5�7"=�5¹D_B%�;<>4�?DI/��=}��7�<>4/?�J�<>25����� I/��=X� I)L�N�N���U.= �!�}� B���I$9b4Eh�B�?�I��b9b4DC

��� 9b4�C<J�9b?/?DI$4$h�<!��'>BgB@h�B�?DI��;�/= � �#n��Ql[ mp!r?n)¡ /Vn�r"x0w/�#r2p�l MqKn�r2n�£%z ) � � =iL��!��L[(0L����#Lx7

8�I$<��PGT=m`*7¹@�7aI)L�N�N��%U.= /¦n!r"x0w/�#r"p!l /¦n�¥Hx&rsr¤q, �£�Á �Sl[q, �w/q kmr"x&u^p! m¥��Sk%kmr¤qKw.p�vKqKn� �uP7S`�?/?�9���B348RÞI���� I/�è8:B34DC!(

�§<>4T��5�9b4�C3<!��B��RI>7

���

Page 85: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

8:9;2T=/1D7o5�7aI)L�N�N��%U.= � h�I/� � B���B%�b9"~TI$?�9b4�?DI/��I$4�?DI$4��S� <����5�b9;4DCW9%9"� GG��B%����<!�R9���B34'����B<� I]D�I��&� 9 B34@� <��<�5�b9b4DC

<>4/?A9�����B���� <>4'��I4�R<����5�b9b4�C�= �-,�v§p�vKq;u&vKqKw/u^p! m¥ �dn!|_km��vKqs �£�= � =iL�L �g(0L�L�N�7

8:9;2T=�1D7o5�7:<>4�?�EHG�I$4T=�@�7XI)L�N�N��%U.=V� �S�b9b4�?Þ?DI���B34�B>B%�b2�� 9bB34VB�9b<���I���2DI$4H� 9;<���9����/2'� <!� 9 B34'�/= � ��n!�#l. �p!r¿n)¡

vs¾�x���|�x&l[qKw.p! B,mv§p�vKq,u[vKq>w0p!r¸��u.u&n�w�qKp%vKqKn! 5=�� � =m���%�¹(«�%�!��7

8:9;2T=�1D7o5�7"=�EHG�I$4T=�@�7"=%<>4�?bRÞB34DC�=�R 7 J�74I)L�N�N��%U.=���@�I]D�I��&� 9 B34���B34H� � B%��*­B�����I���2DI$4�� 9b<��9�����B���� <>4'��I

�R<����5�b9b4�C�= ��¼�x�w/¾H 'q>w0p!r ��x«k�n!l[vK=Æ�I/�/<!�����!I$4��%B�*�5Q� <!� 9���� 9��/�/=�5Q� <>4�*­B��R? �4�9"B>I/�.� 9"�{�>7

8:9;2T=\1D7o5�7"= ��I$2�9�<��b?¸=%`�7 ��7"=�<>4/?!8:<�9M� I$4'��I�=�E7 c%7'I)L�N�N%��U.="� hK<!��'>B¹B�� � �P2'�&� 2�� I��z9b4:A/9bB%� B>C39��T<��#��I���2DI$4'��I

<��;9 C34'�!I$4����/= �­¼�x�w/¾H 'q>w0p!r �Mx«k#n!l&vK=�5Q� <>4�*­B��R? �4�9"B>I/�0� 9��{�>7

8:9;2T=�1�7o5�7"=#RÞB34DC�=�R 7 J�7"=�<>4�?$�|B34�C�='`�7TI)L�N�N���U.=;�RE�BgBr<!�P9b<>4'��I4� � �R25�&� 2�� IB�*¸� GDIf\d9"A'Am�X� <��<�5� I/�¿9%9"� G

<!�5�5�b9��T<!� 9 B345�^��B¦� GDIG��B%���/<!�R9;��B34'��B�*dI�� � 9���<!��B��.��<>4�? <>2DC%�!I$4�� <!� 9 B34»���RGDI���I��/= � Mq>n!|�x/vKl[q ��p!=

i�'=me%�¹(§�%��7

hK<��/c�<��RG�I/�R4T=H5�7 ��7"=�ES�"��?DI�=dh�7 `�7"=�<>4/?�8�9b2T=z1D7o5�7SI)L�N�N�O%U.= �R5�I���2DI$4�� 9b<��}�)����B���� <>4'��IA5�<����m�b9b4DC�*­B��

��B34��/<!�P<��!I/� �R9��:�H<��>I���h�B�?DI��;�/��Î�GDI ��I&Y#��\I$4�I/�R<!� 9 B34T= � �dp! mpH¥!qKp�  ��n!�#l. �p!r?n ¡0,mv§p%vKq;u&vKqKw/u.=�9b4

�5� I����T7

B>B34 ��I$2'��<>4�4¸=\1D7'I)L�N��#L¹U.="�)��<!�P9 B32'�d��I��PG�4�9���2DI��z2'��I$?�9b4W��B34�4�I��&� 9 B34�9%9"� G:�R<>4/?DB%� ?�9 C39"���/= �4Ì^p�vKq>n! mp!r

M�#l�x�p!��n)¡ ,mv§p� m¥%p!l ¥�u��Sk%kmr¤q§x�¥�/Vp%vs¾#x&|<p�vKq>w&u�,Tx&l[q>x&u.=i� )Æ= ���g( ��O�7

��9�� �/=fhÏ7 ��7%<>4�? 5DGDI/�/G�<!�R?¸= �*7:I)L�N�N%��U.= � ��9��"��I/�R9b4�CEB�9b<b� 9���2'�b<!� 9 B34T� <>2�Y�9��;9b<!���b��<!��� 9��/� IF3m����I/�0�/= �

k�l x]kml.qs 5v§Á4ÀdÀdÀ}º� '� �_ºsn�y�º,p%wgº�� ���5�Qu/x&l.u��5u0¾#x]k5¾Qp!l ¥x7

�d2�<>4/?��/=¢@�7 c�7�I)L�N�O�e%U.= � c}��B34DB%��I/� �R9��è?�9�� I���2�9;�b9"A'�R9;2'� �!B�?DI�����Is9%9"� G ?�9;���T2'��� 9bB34'�.U.= � ��w0n� mn!|�x/vKl[qKw

�Mx&¢¹q§x&À»�'=iL[(«N���7

�%�

Page 86: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

� I)L�N�O�O%U.=�¼i¾#x �Mw0n! mn�|<x�vKl.q>w&uWn)¡_Êfq;u�x��/�#qsrtq§o&l[qs�Q|W= ��I/9 �B���'��}�H<�� 9��i�¿�;<���'Q9¢I����{7

@%<!A�9b4DI/��=H8�7¹@�7�I)L�N�O�N%U.= � ` Îg2���B��R9b<���B34bJ9b?�?DI$4�hK<!��'>BgBbh6B�?DI����§<>4�?�5�I��bI��&��I$?b`_�'�5�b9��T<!� 9bB34'�!9b4

5#��ITI��PGA@�I���B>C34/9"� 9 B34T= � ��l)n¹w.x0x0¥!qs �£!u�n)¡fvs¾#x�Ä���� �S=�� �m=�e��%�¹(«e�O���7

@%2'A/9b4T=���7 �7jI)L�N�O%��U.= /��#r¤vKq kmr�x�Ä.|Mk��#v§p%vKqKn! �¡&n!l�Ìfn! 5l x&u§k#n! 'u/xWqs  ,5�#l.¢�x&z�u.= ��I/9 �B���'��}R 9�� I/�>7

5Q�/9bITC>I��bG�<��"��I/��='��7�1�7/<>4�?68�<>2��R9"� ~TI$4¸=�5�7¹8�7jI)L�N�N!�HU.=T�R5�I���2DI$4�� 9b<�� �_��?�<!� 9b4DC�B�*�E�B34�?�9�� 9 B34�<�� �È�RB�A/<!A/9��2(

9�� 9 I���B34G�d9"� I��&��I$?F\��R<!�/G/9��T<��:5Q� �R2'�&� 2'� I��/= ��Ì:x/vKÀ}n!l��g=�) � =m�%�!Ng(«�!�%��7

Îg<>4�4DI/��=}h�7d`�7z<>4�?+RÞB34DC�=XR#7 J*7MI)L�N�O%��U.= � Î�GDI��T<����T2'�b<!� 9bB34�B�*M��B%����I/�R9 B��!?�9�� � �P9"A/2�� 9 B345��A���?�<!� <

<>2�C%�!I$4H� <!� 9bB34FIs9�9"� G�?�9��0�T2'��� 9 B34�U.= ���#n��Ql[ mp!rTn)¡�vs¾#xM��|�x/l.q>w0p!  ,�v§p�vKq;u&vKqKw.p!r5��u.u&n�w�qKp%vKqKn!  �)Æ=���e�Og(

���!��7

Î�9 I/�P4DI/��=�8�7_I)L�N�N���U.=�� h�<!��'>BgBb�RG/<>9b4'�<*­B��§I&Y#�5� B��P9b4DC���B%� ��I/�R9 B���?�9�� � �R9�A/2�� 9 B34'�$Is9%9"� G ?�9����T2'�0� 9 B34mU.= �

�� ' mp!rtu^n)¡ ,mv§p%vKq;u&vKqKw/u ) )Æ=jL¹����L[(0L¹�!��e�7

��<�����2DI/~�=Hh�7><>4�?�5��RGDI/�R<xC>B�=�J�7 `*7'I)L�N�O��%U.=)� ����IHB�*5�H2�9��b?#(�2'��<>4�?:c�4DI/� C���({hK9b4�9���9�~$<!� 9 B34 �}� BQ��I$?/2�� I��

��B E�B%���/2���IA8:B¹9�({c¢4DI/� C�� 5Q� �R25�&� 2�� I��!B�*�� GDI@�H<���'QA�B34DIAB�*�c�4'>I/�/G�<��b9b4¸= � Mq>n.k#n!r¤z!|<x/l0u[=�) � =

L&� �%�¹(0L&������7

RÞI�� ��I)L�N�N�e%U.=O� h�9"YQ� 2��RIf�!B�?DI����/='h�B34H��I�EH<!�0�bB�=m�H<��>I�� 9;<>4�2���?�<!� 9;4DC�<>4�?A?���4�<���9;�4�!B�?DI����/= ���dn�| �

k��#v{x/l ,�w�q>x/ mw.xWp! �¥�,�v§p�vKq;u&vKqKw/u.= ) � = ��e��g( ������7

RÞI�� �/=�h�7|<>4�? J<!���R9���B34¸=�1D7<I)L�N�O�N%U.= �p!zHx&u.q>p! b¡/n!l�x�w0p!u[vKqs �£p! m¥ ¥!z! mp�|:q>wb|�n¹¥Hx&rtu.=���I/9 �B���'��

5#�'�R9b4DC>I/��(§�zI/�0�b<xCD7

RÞB34DC�=ÆR 7 J�7�<>4�?68�9b<>4DC�= ��7dI)L�N�N%��U.=6� �_��4�<���9;�4�)����B���� <>4'��IfRÞI$9bC3GH� 9b4�C�9b4Gh�B34���I!EH<!�.� B§<>4�?F6���(

� 9;��9"~$<!� 9 B34T= � ��l)n¹w.x0x0¥!qs �£!u�n)¡fvs¾#x�Ì^p�vKq>n! mp!rT�Mw.p%¥Hx&|Wz�n)¡ ,�w�q>x/ mw.x/='��B§<!�'��I$<!�$7

��O

Page 87: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

Hybrid Diagnosis with Unknown Behavioral ModesMichael W. Hofbaur 1 and Brian C. Williams 2

in: Proceedings of the 13th International Workshop on

Principles of Diagnosis (DX02), May 2002

Abstract. A novel capability of discrete model-based diagnosismethods is the ability to handleunknown modeswhere no assump-tion is made about the behavior of one or several components of thesystem. This paper incorporates this novel capability of model-baseddiagnosis into a hybrid estimation scheme by calculating partial fil-ters. The filters are based on causal and structural analysis of thespecified components and their interconnection within the hybrid au-tomaton model. Incorporating unknown modes provides a robust es-timation scheme that can cope, unlike other hybrid estimation andmulti-model estimation schemes, with unmodeled situations and par-tial information.

1 Introduction

Modern technology is increasingly leading to complex artifacts withhigh demands on performance and availability. As a consequence,fault-tolerant control and an underlying monitoring and diagno-sis capability plays an important role in achieving these require-ments. Monitoring and diagnosis systems that build upon the discretemodel-based reasoning paradigm[8] can cope well with complexityin modern artifacts. As an example, the Livingstone system[22] suc-cessfully monitored and diagnosed the DS-1 space probe in flight,a system with approximately480 modes of operation. However, awidespread application of discrete model-based systems is hinderedby their difficulty to reason about the continuous dynamics of an ar-tifact in a comprehensive manner. Continuous behaviors are difficultto capture by the pure qualitative models that are used by the rea-soning engines. Nevertheless, additional reasoning in terms of thecontinuous dynamics is vital for detecting functional failures, as wellas low-level incipient (i.e slowly developing) faults and subtle com-ponent degradation.

Hybrid systems theory provides a modeling paradigm that inte-grates both, continuous state evolution and discrete mode changesin a comprehensive manner. Recent work in hybrid estimation[14,16, 24, 9] attempts to overcome the shortcomings of discrete model-based diagnosis cited above and provides schemes that integratemodel-based approaches with techniques from fault detection andisolation (FDI)[23, 4] and multi-model adaptive filtering[13, 11, 10].The hybrid estimation schemes, as well as their FDI and multi-modelfiltering ancestors, work well whenever the underlying model(s) are’close’ mathematical descriptions of the physical artifact. They canfail severely whenever unforeseen situations occur. Therefore, it isessential to provide models that capture the entire spectrum of possi-ble behaviors/modes whenever we use the hybrid estimate for closedloop control, for instance. Model-based diagnosis, in contrast, does

1 Department of Automatic Control, Graz University of Technology, A-8010Graz, Austria, email:hofbaur @irt.tu-graz.ac.at

2 MIT Space Systems and AI Laboratories, 77 Massachusetts Ave., Rm. 37-381, Cambridge, MA 02139 USA, email:williams @mit.edu

not impose such a strong modeling assumption. Its concept of theunknown modeallows diagnosis of systems where no assumption ismade about the behavior of one or several components of the sys-tem. In this way, it captures unspecified and unforeseen behaviorsof the system under investigation. This paper provides an approachto incorporate the concept of an unknown mode into our hybrid es-timation scheme[9]. As a result we obtain an estimation capabilitythat can detect unforeseen situations. Furthermore, it allows us tocontinue estimation on a degraded basis. We achieve this by causalanalysis[17, 20], structural analysis[7] and decomposition of the sys-tem.

This paper starts with a brief introduction to our hybrid systemsmodeling and estimation scheme. Upon this foundation, we extendhybrid estimation to incorporate the unknown mode and demonstratethe underlying structural analysis and decomposition task. Finally, anexperimental evaluation with computer simulated data for a Martianlive support system demonstrates the advantages of this extended hy-brid estimation scheme.

2 Hybrid Systems

The hybrid automaton model used throughout this paper is based on[9] and can be seen as a model that merges hidden Markov models(HMM) with continuous discrete-time dynamical system models (wepresent the model on the level of detail sufficient for this work andrefer the reader to the reference cited above for more detail).

2.1 Concurrent Hybrid Automata

Definition 1 A discrete-time probabilistic hybrid automaton (PHA)A is described as a tuple〈x,w, F, T,Xd, Ts〉:

• x denotes the hybridstate variablesof the automaton3, composedof x = {xd} ∪ xc. The discrete variablexd denotes themodeof the automaton and has finite domainXd. Thecontinuous statevariablesxc capture the dynamic evolution of the automaton.xdenotes thehybrid stateof the automaton, whilexc denotes thecontinuous state.

• The set ofI/O variablesw = ud ∪ uc ∪ yc of the automatonis composed of disjoint sets of discrete input variablesud (calledcommand variables), continuousinput variablesuc, and continu-ousoutput variablesyc.

• F : Xd → FDE ∪ FAE specifies thecontinuous evolutionof theautomaton in terms ofdiscrete-time difference equationsFDE andalgebraic equationsFAE for each modexd ∈ Xd. Ts denotes thesampling period of the discrete-time difference equations.

3 When clear from context, we use lowercase bold symbols, such asv, todenote asetof variables{v1, . . . , vl}, as well as avector [v1, . . . , vl]

T

with componentsvi.

Page 88: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

• The finite set,T , of transitionsspecifies the probabilistic discreteevolution of the automaton.

Complex systems are modeled as a composition of concurrentlyoperating PHA that represent the individual system components. Aconcurrent probabilistic hybrid automata (cPHA)specifies this com-position as well as its interconnection to the outside world:

Definition 2 A concurrent probabilistic hybrid automaton (cPHA)CA is described as a tuple〈A,u,yc,vs,vo, Nx, Ny〉:

• A = {A1,A2, . . . ,Al} denotes the finite set of PHAs that repre-sent the componentsAi of the cPHA (we denote the componentsof a PHAAi by xdi,xci,udi,uci,yci, Fi,Xdi).

• Theinput variablesu = ud ∪uc of the automaton consists of thesets of discrete input variablesud = ud1 ∪ . . . ∪ udl (commandvariables) and continuous input variablesuc ⊆ uc1 ∪ . . . ∪ ucl.

• Theoutput variablesyc ⊆ yc1 ∪ . . . ∪ ycl specify the observedoutput variables of the cPHA.

• The observation process is subject to additive, zero mean Gaussiansensor noise. Ny : Xd → IRm×m specifies the mode dependent4

disturbancevo in terms of the covariance matrixR = diag(ri).• Nx specifies additive, zero mean Gaussiandisturbancesthat act

upon the continuous state variablesxc = xc1 ∪ . . . ∪ xcl. Nx :Xd → IRn×n specifies the mode dependent disturbancevs interms of the covariance matrixQ.

Definition 3 The hybrid statex(k) of a cPHA at time-stepk spec-ifies the mode assignmentxd,(k) of the mode variablesxd ={xd1, . . . , xdl} and the continuous state assignmentxc,(k) of thecontinuous state variablesxc = xc1 ∪ . . . ∪ xcl.

Interconnection among the cPHA componentsAi is achieved viashared continuous I/O variableswc ∈ uci∪yci only. Fig. 1 illustratesa simple example composed of 3 PHAs.

A1 A2

uc1

ud1

ud2CA

wc1

A3

yc2

yc1

Figure 1. Example cPHA composed of three PHAs

A cPHA specifies a mode dependent discrete-time model for aplant with command inputsud, continuous inputsuc, continuousoutputsyc, modexd, continuous state variablesxc and additive, zeromean Gaussian disturbancesvs, vo. The discrete-time evolution ofxc andyc is described by the nonlinear system of difference equa-tions (sampling periodTs)

xc,(k) = f(k)(xc,(k−1),uc,(k−1)) + vs,(k−1)

yc,(k) = g(k)(xc,(k),uc,(k)) + vo,(k).(1)

The functionsf(k) andg(k) are obtained by symbolically solving5

the set of equationsF1(xd1,(k)) ∪ . . . ∪ Fl(xdl,(k)) given the modexd,(k) = [xd1,(k), . . . , xdl,(k)]

T .

4 E.g. sensors can experience different magnitudes of disturbances for differ-ent modes.

5 Our symbolic solver restricts the algebraic equations and nonlinear func-tions to ones that can be solved explicitly and utilizes a Grobner Basisapproach[3] to derive a set of equations of form (1).

Consider the illustrative cPHA in Fig. 1 with

A1 = 〈{xd1}, {ud1, uc1, wc1}, F1, T1, {m11, m12}...〉A2 = 〈{xd2, xc1}, {ud2, wc1, yc1}, F2, T2, {m21, m22}...〉A3 = 〈{xd3, xc2, xc3}, {ud2, uc1, yc1, yc2}, F3, T3, {m31}...〉.

F1, F2 and F3 provide for a cPHA mode xd,(k) =[m11, m21, m31]

T the equations

F1(m11) = {uc1 = 5.0 wc1}F2(m21) = {xc1,(k) = 0.8 xc1,(k−1) + wc1,(k−1),

yc1 = xc1}F3(m31) = {xc2,(k) = xc3,(k−1) + yc1,(k−1),

xc3,(k) = 0.4 xc2,(k−1) + 0.5 uc1,(k−1),

yc2 = 2.0 xc2 + xc3}.

(2)

This leads to the discrete-time model:

xc1,(k) = 0.8 xc1,(k−1) + 0.2 uc1,(k−1) + vs1,(k−1)

xc2,(k) = xc1,(k−1) + xc3,(k−1) + vs2,(k−1)

xc3,(k) = 0.4 xc2,(k−1) + 0.5 uc1,(k−1) + vs3,(k−1)

yc1,(k) = xc1,(k) + vo1,(k)

yc2,(k) = 2.0 xc2,(k) + xc3,(k) + vo2,(k)

(3)

2.2 Estimation of Hybrid Systems

To detect the onset of subtle failures, it is essential that a monitoringand diagnosis system is able to accurately extract the hybrid state ofa system from a signal that may be hidden among disturbances, suchas measurement noise. This is the role of a hybrid observer. Moreprecisely:

Hybrid Estimation Problem: Given a cPHACA, a sequencesof observations{yc,(0),yc,(1), . . . ,yc,(k)} and control inputs{u(0),u(1), . . . ,u(k)}, estimate the most likely hybrid statex(k) at time-stepk.

A hybrid state estimatex(k) consists of acontinuous state esti-mate, together with the associatedmode. We denote this by the tuple

x(k) := 〈xd,(k), xc,(k),P(k)〉,

wherexc,(k) specifies the mean andP(k) the covariance for the con-tinuous state variablesxc. The likelihood of an estimatex(k) is de-noted by thehybrid belief-stateh(k)[x].

We perform hybrid estimation as extended version of HMM-stylebelief-state update that accounts for the influence of the continuousdynamics upon the system’s discrete modes. A major difference be-tween hybrid estimation and an HMM-style belief-state update, aswell as multi-model estimation, is, however, that hybrid estimationtracks a set of trajectories, whereas standard belief-state update andmulti-model estimation aggregate trajectories which share the samemode. This difference is reflected in the first of the following tworecursive functions which define our hybrid estimation scheme:

h(•k)[xi] = PT (mi|xj,(k−1),ud,(k−1))h(k−1)[xj ] (4)

h(k)[xi] =h(•k)[xi]PO(yc,(k)|xi,(k),uc,(k))∑j h(•k)[xj ]PO(yc,(k)|xj,(k),uc,(k))

(5)

h(•k)[xi] denotes an intermediate hybrid belief-state, based on tran-sition probabilities only. Hybrid estimation determines for each

Page 89: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

xj,(k−1) at the previous time-stepk − 1 the possible transitions,thus specifying candidate successor states to be tracked. Consecu-tive filtering provides the new hybrid statexi,(k) and adjusts the hy-brid belief-stateh(k)[xi] based on the hybrid probabilistic observa-tion functionPO(yc,(k)|xi,(k),uc,(k)). The estimatexj,(k) with thehighest belief-stateh(k)[xj ] = maxi(h(k)[xi]) is taken as the hybridestimate at time-stepk.

Tracking all possible trajectories of the system is almost alwaysintractable because the number of trajectories becomes too large afteronly a few time-steps. In [9] we present an approximative anytimeanyspace algorithm that copes with the exponential growth, as well asthe large number of modes in a typical concurrent hybrid automatonmodel.

Hybrid estimation and other multi-model estimation schemes havein common that they require models that are ’close’ mathematical de-scriptions of the system. They can fail severely whenever unforeseen,i.e. unmodeled, situations occur. As a consequence, we have to pro-vide models for all operational modes as well as an exhaustive setof models for possible failure modes. Providing all possible failuremodels can be problematic even under the assumption of an exhaus-tive failure mode effect analysis (FMEA). For instance, consider anincipient fault in a servo valve that causes the valve to drift off itsnominal opening value. The drift (positive, negative, slow, fast...) issubject to the fault. It is surely difficult to provide a mathematicalmodel with the correct parameter values that captures all possibledrift situations. Nor is it helpful to introduce a sufficiently large setof modes that captures possible situations of the drift fault as thiswould introduce additional complexity for hybrid estimation by in-creasing the number of modes unnecessarily.

This requirement of hybrid mode estimation is in contrast to dis-crete model-based diagnosis schemes, such as GDE (e.g. [5, 6, 19]).Model-based diagnosis deduces the possible mode of the systembased on nominal models, and few specified fault models only. Theonset of possible fault scenarios are covered by the so calledun-known modewhich does not impose any constraints on the system’svariables.

The next section provides an approach that systematically incor-porates the concept of the unknown mode into our hybrid estimationscheme.

3 Estimation with Unknown Modes

The estimation scheme [9] requires a fully specified mode assign-mentxdi,(k) for each candidate trajectory that is tracked in the courseof hybrid estimation. Only a fully specified mode allows us to deducethe mathematical model (1) for the overall system. This model is thebasis for the dynamic filter (e.g. extended Kalman filter) that is usedin the course of hybrid estimation.

uc1

yc1

yc2PO

xc1

xc2

xc3

MIMO Filter

Figure 2. MIMO filter (e.g. extended Kalman filter) for the cPHA example

For our illustrative 3 component example introduced abovethis would mean that hybrid estimation calculates a multi-input

multi-output (MIMO) filter (see Fig. 2) for modexdi,(k) =[m11, m21, m31]

T based on the mathematical model (3). This filterprovides the hybrid state estimatexi,(k) as well as the value for thehybrid probabilistic observation functionPO(yc,(k)|xi,(k),uc,(k))for the hybrid estimator (see Appendix A for the extended Kalmanfilter estimation details).

Let us assume the modexdi,(k) = [?, m21, m31]T which speci-

fies that component 1 (A1) is in unknown mode. A component in un-known mode imposes no constraints (equations) among its variables(uc1 and the internal variablewc1, in our case). As a consequence,we cannot deduce an overall mathematical model of the form (1) andfail to provide the basis for the hybrid estimation scheme, the MIMOfilter for modexdi,(k) = [?, m21, m31]

T .

1 2

uc1

ud1

ud2

wc1

3

yc2

yc1

vs1 vs3

vo1

vo2

A A

CA

A

vs2

Figure 3. Example cPHA with explicit noise inputs

However, a close look on the PHA interconnection (Fig. 3 - thefigure extends Fig. 1 by including the implicit noise inputs, as wellas indicating the causality for the internal I/O variables) reveals thatwe can still estimate component 3 by its observed outputyc2 and theobservationyc1 as a substitute for the value of its input. This intuitiveapproach utilizes a decomposition of the cPHA as shown in Fig. 4.

1

vs1

2

3

vs3

yc2

yc1

vo1

vo2

vo1yc1

uc1A A

A

vs2

uc1

Figure 4. Decomposed cPHA

The decomposition allows us to treat the concurrent parts of thesystem independently and calculate afilter cluster consisting of 2independent filters. However, when calculating the individual filtersfor the cluster, we have to take into account that we use themea-surementof the input to the third component (yc1) in replacement toits true value. This can be interpreted as having additional additivenoise at the component’s input as indicated in Fig. 4. The followingmodification of the covariance matrixQ3 for the state variables ofA3 takes this into account:

Q3 = b3r1bT3 + Q3, (6)

wherer1 denotes the variance of disturbancevo1 andb3 = [0, 1]T

Page 90: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

uc1

yc1

PO2

yc2

PO1

xc2

xc3

xc1Filter 1

Filter 2

Filter Cluster

Figure 5. Decomposed filter

denotes the input vector6 of A3 with respect toyc1.A filter cluster consisting of extended Kalman filters and the

MIMO extended Kalman filter are interchangeable as they providethe same expected value for the continuous state (E(xc)) wheneverthe mode of the automaton is fully specified. However, the decom-posed filter has the advantage that the probabilistic observation func-tion PO of the overall system is given by

PO =∏

j

POj , (7)

wherePOj denotes the probabilistic observation function of thej’thfilter in the filter cluster.

This factorization of the probabilistic observation function allowsus to calculate an upper bound forPO whenever one or more com-ponents of the system are in unknown mode. We simply take theproduct over the remaining filters in the cluster. This is equivalentwith considering the upper bounds of the inequalitiesPOj ≤ 1 foreach unknown filterj. In our example with unknown componentA1

this would mean:PO ≤ PO2,

wherePO2 denotes the observation function for the filter that esti-mates the continuous state of componentA3.

The following subsection provides a graph-based approach forfiler cluster deduction that grounds the informally introduced decom-position on a more versatile basis.

3.1 System Decomposition and Filter ClusterCalculation

Starting point for the decomposition of the system for a cPHA modexd is the set of equations

F1(xd1,(k)) ∪ . . . ∪ Fl(xdl,(k)) =: F(xd), (8)

whereFj(xdj,(k)) returns the appropriate set of equations for a com-ponentAi wheneverxdj,(k) ∈ Xdj or the empty set whenever thecomponent is in unknown mode, i.e.xdj,(k) =?. Although we stillhave to solve the set of equations to arrive at the mathematicalmodel of form (1) we can interpret the set of equations (8) as the

6 In the general case, we have to calculatebj for a cPHA componentAjand observed inputsuyc by linearization, more specifically:bj,(k) =

∂fj/∂uyc|xcj,(k−1),ucj,(k−1), wherefj denotes the right-hand side of

the difference equation for componentAj , uyc refers to the observedvariables that are used as inputs to the component (i.e.uyc ⊂ yc) andxcj,(k−1) as well asucj,(k−1) represent the state estimate and the contin-uous input for componentAj at the previous time-step, respectively.

raw modelfor the system given modexd. The following decom-position performs a structural analysis of the raw model-based oncausal analysis[17, 20], structural observability analysis[7] and graphdecomposition[1].

A cPHA model does not impose a fixed causal structure that spec-ifies directionality of automaton interconnections. Causality is im-plicitly specified by the set of equations. This increases the expres-siveness of the modeling framework but requires us to perform acausal analysis of the raw model (8) as a first step. The deduc-tion of the causal dependencies is done by applying the bipartite-matching based algorithm presented in [17]. The resulting directedgraph records the causal dependencies among the variables of thesystem (Fig. 6 shows the graph for the the illustrative 3 PHA ex-ample). Each vertex of the graph represents one equationei ∈ F

uc1 xc1wc1 yc1 xc2 xc3 yc2

Figure 6. Causal graph for the cPHA example

or an exogenous variable specification (e.g.uc1) and is labeled byits dependent variable which also specifies the outgoing edge (in thefollowing, we will use the variable name to refer to the correspond-ing vertex in the graph). Vertices without incoming edges specify theexogenousvariables.

Definition 4 A causal graphof a cPHACA at a modexd is a di-rected graph that records the causal dependencies among the vari-ablesv ∈

⋃i xci ∪ uci ∪ yci of CA. We denote the causal graph

by CG(CA,xd) and sometimes omit arguments where no confusionseems likely.

Goal of our analysis is to obtain a set of independent subsystemsthat utilize observed variables as virtual inputs. Therefore, we slicethe graph at observed variable vertices with outgoing edges, insert anew vertex to represent a virtual input and re-map the sliced outgo-ing edges to this vertex. Fig. 7 demonstrates this re-mapping for thecausal graph of Fig. 6. The observed variables areyc1 andyc2. Onlythe vertex with dependent variableyc1 has an outgoing edge, thus weslice the graph atyc1 → xc2 and re-map the edge to the virtual inputuyc1.

uc1 xc1wc1 yc1

xc2 xc3 yc2uyc1

Figure 7. Remapped causal graph for the cPHA example

A dynamic filter (e.g. extended Kalman filter) can only estimatethe observable part of the model. Therefore, it is essential to perform

Page 91: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

an observability analysis prior calculating the filter so that non ob-servable parts of the model are excluded. We perform this analysison a structural basis7.

Definition 5 We call a variablev of a cPHACA at modexd struc-turally observable (SO)whenever it is directly observed, i.e.v ∈ yc,or there exists at least one path in the causal graphCG(CA,xd) thatconnects the variablez to an output variableyc ∈ yc of CA.

A filter estimates the state variablesxc of a dynamic system basedon observationsyc and the inputsuc that act upon the state variablesxc. The required knowledge about the inputsuc indicates that thestructural observability criteria is not yet sufficient to determine thesubmodel for estimation. We have to make sure, that no unknown ex-ogenous input influences a variable. To illustrate this, consider againthe 3 PHA example with modexd = [?, m21, m31]

T . Component1 in unknown mode omits the equation that relates the variablesuc1

andwc1. This leads to a causal graphCG (Fig. 8), wherewc1 is la-beled as exogenous (no incoming edges). This unknown exogenousinput influences the state variablexc1 and, as a consequence, pre-vents us from estimating it!

uc1 xc1wc1 yc1

xc2 xc3 yc2uyc1

Figure 8. Remapped causal graph for the cPHA example with unknowncomponentA1

We extend our structural analysis of the causal graph by the fol-lowing criteria:

Definition 6 We call a variablev of a cPHACA at modexd struc-turally determined (SD)whenever it is an input variable of the au-tomaton, i.e.v ∈ uc, or there does not exist a path in the causalgraphCG(CA,xd) that connects an exogenous variableue /∈ uc

with v.

Furthermore, it is helpful to eliminate loops in the causal graphprior checking variables against both structural criteria. For this pur-pose, we calculate thestrongly connected componentsof the causalgraph[1].

Definition 7 A strongly connected component (SCC)of the causalgraphCG is a maximal setSCC of variables in which there is a pathfrom any one variable in the set to another variable in the set.

Fig. 9 shows the remapped causal graph for the 3 PHA example aftergrouping variables into strongly connected components.

The strong interconnection among variables in an SCC impliesthat:

1. Structural observability of variables in an SCC follows directlyfrom structural observability of at least one variable in the SCC.

7 Throughout the paper we assume that loss of observability is caused bya structural defect of the model. Otherwise, it is necessary to perform anadditional numerical observability test [18] as structural observability onlyprovides anecessarycondition for observability.

uc1 xc1wc1 yc1

xc2, xc3 yc2uyc1

Figure 9. Causal SCC graph for cPHA example

2. A variable in an SCC is structurally determined, if and only if allvariables in the SCC are structurally determined.

As a consequence, we can apply our structural analysis to stronglyconnected components directly and operate on the SCC graph, i.ea causal graph without loops. The analysis of a strongly connectedcomponent with respect to structural observability and structural de-termination (SOD) can be outlined as follows:

function determine-SOD-of-SCC(SCC,uc, k)whenSOD-undetermined?(SCC)

if exogenous?(SCC)then vi← independent-var(SCC)

if vi ∈ uc then SD(SCC)← TrueelseSD(SCC)← False

else V ← uplink-SCCs(SCC)loop for SCCi in V

do determine-SOD-of-SCC(SCCi,uc, k)SO(SCC)← TrueSD(SCC)← all-uplink-SCCs-are-SD?(V)cluster-index(SCC)← k ∪ cluster-indices(V)

SOD-determined(SCC)← Truereturn Nil

Our structural analysis algorithm determines structural observabil-ity and determination (SOD) of a variable by traversing the SCCgraph backwards from the observed variables towards the inputs.In the course of this analysis we label non-exogenous strongly con-nected components with an index that refers to their cluster mem-bership. This indexing scheme allows us to cluster the variables intonon-overlapping clusters with respect to the observed variables. Thedirect relation between a variable, its determining equation, and thecPHA component that specified this equation leads to the compo-nent clusters sought. The structural analysis can be summarized asfollows:

function component-clustering(CA,xd)returns a set of cPHA component clustersyc← observed-vars(CA)CG ← remap-causal-graph(CG(CA,xd),yc)uc← virtual-inputs(CG) ∪ input-vars(CA)CGSCC ← strongly-connected-component-graph(CG)k← 0loop for SCCi in output-SCCs(CGSCC ,yc)

do determine-SOD-of-SCC(SCCi,uc, k)k← k + 1

graph-clusters← get-SOD-SSC-clusters(CGSCC )return automaton-clusters(CA, graph-clusters)

Page 92: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

uc1 xc1wc1 yc1

xc2, xc3 yc2uyc1

sod-1 sod-1 sod-1

sod-2 sod-2

cluster 1

cluster 2

A1{ },A2

A3{ }

Figure 10. Labeled and partitioned causal SCC graph for the 3 cPHAexample

Each component cluster defines the observable and determinedraw model for a subsystem of the cPHA. This raw model can besolved symbolically and provides the nonlinear system of differenceequations (a model similar to (1), but with the additional virtual in-puts) that is the basis for the corresponding filter in the filter cluster.In this way we exclude the unobservable and/or undetermined partsof the overall system from estimation.

Whenever a state variablexcj becomes unobservable and/or un-determined (e.g. due to a mode change) during hybrid estimation,we hold the value for the mean at its last known estimatexcj andincrease its varianceσ2

j = pjj by a constant factor at each hybridestimation step. This reflects a continuously decreasing confidencein the estimatexcj and allows us to restart estimation whenever thevariable becomes observable and determined again8.

4 Example - BIO-Plex

Our application is the BIO-Plex Test Complex at NASA JohnsonSpace Center, a five chamber facility for evaluating biological andphysiochemical Martian life support technologies. It is an artificial,biosphere-type, closed environment, which must robustly provide allthe air, water, and most of the food for a crew of four without in-terruption. Plants are grown in plant growth chambers, where theyprovide food for the crew, and convert the exhaledCO2 into O2. Inorder to maintain a closed-loop system, it is necessary to control theresource exchange between the chambers without endangering thecrew. For the scope of this paper, we restrict our evaluation to thesub-system dealing withCO2 control in the plant growth chamber(PGC), shown in Fig. 11.

The system is composed of several components, such as redundantflow regulators (FR1, FR2) that provide continuousCO2 supply, re-dundant pulse injection valves (PIV1, PIV2) that provide a means forincreasing theCO2 concentration rapidly, a lighting system (LS) andthe plant growth chamber (PGC), itself. The control system main-tains a plant growth optimalCO2 concentration of1200 ppm duringthe day phase of the system (20 hours/day).

Hybrid estimation schemes are key to tracking system operationalmodes, as well as, detecting subtle failures and performing diag-noses. For example, we simulate a failure of the second flow reg-ulator. The regulator becomes off-line and drifts slowly towards itspositive limit. This fault situation is difficult to capture by an explicitfault model as we do not know, in advance, whether the regulator

8 Whenever a state variablexcj is directly observed we also can utilize analternative approach suggested in [15] that restarts the estimator with theobserved value, thus improving the observer convergence time.

Airlock Plant Growth Chamber

Cre

w C

ham

ber

CO2

tank

lighting system

chamber control

flow regulator 2

pulse injection valves

CO2

flow regulator 1

Figure 11. BIO-Plex plant growth chamber

drifts towards its postitive or negative limit, nor do we know the mag-nitude of the drift. A fault of this type, which develops slowly andwhose symptom is hidden among the noise in the system is a typicalcandidate for our unknown-mode detection capability. However, wealso provide explicit failure models that describe typical situations.For example, the PGC has 4 plant trays with one illumination bankfor each tray. A black out of one illumination bank can be interpretedas a25% loss in light intensity. This situation can be modeled explic-itly by a dynamical model that takes this reduced light intensity intoaccount.

In the following we describe the outcome of a simulated experi-ment where the flow regulator fault with drifting symptom is injectedat time pointk = 700 and an additional light fault, that harms oneof the four illumination banks, is injected atk = 900. The faults are’repaired’ atk = 1100 andk = 1300 for the flow regulator fault andthe lighting fault, respectively. This experiment illustrates unknownmode detection and recovery from it, nominal failure mode detection,and the multiple fault detection capability of our approach.

FR1

FR2

PIV1

PIV2

LS

PGC

ud3

uc1

ud1

ud2

yc1

yc2

yc3

wc1

wc2

wc3

A1

A2

A3

A4

A5

A6

Figure 12. BIO-Plex cPHA model

The simulated data is gathered from the execution of a refined sub-set of NASA’s JSC’s CONFIG model for the BIO-Plex system[12].Hybrid estimation utilizes a cPHA model that consists of 6 com-ponents as shown in Fig. 12. To illustrate the complexity of thehybrid estimation problem we should note, that the concurrent au-tomaton has approximately56 ≈ 15000 modes. Each mode de-scribes the dynamic evolution of the chamber system by a third or-der system of difference equations. For example, the nominal op-erational condition for plant growth is characterized by the mode

Page 93: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

xd = [mr2, mr2, mv1, mv1, ml2, mp2], wheremr2 characterizesan partially open flow regulator,mv1 a closed pulse injection valve,ml2 100% light on, andmp2 plant growth mode at1200 ppm, re-spectively. This mode specifies the raw model:

F1(mr2) = {xc1,(k) = 0.5 uc1,(k−1), yc1 = xc1}F2(mr2) = {xc2,(k) = 0.5 uc1,(k−1), yc2 = xc2}F3(mv1) = {wc2 = 0.0}F4(mv1) = {wc3 = 0.0}F5(ml2) = {wc1 = 1204.0}F6(mp2) = {xc3,(k) = xc3,(k−1) + 20.163·

[−1.516 · 10−4f1(wc1,(k−1))f2(xc3,(k−1))+

yc1,(k−1) + yc2,(k−1) + wc1,(k−1) + wc2,(k−1)],

yc3 = xc3},(9)

wheref1 andf2 denotes

f1(wc1) :=− 7.615 + 0.111 wc1 − 2.149 · 10−5 w2c1

f2(xc3) := 72.0− 78.89 e−xc3/400.0.(10)

xc1,(k) andxc2,(k) denote the gas flow ([g/min]) of flow regulator 1and 2, respectively andxc3,(k) denotes theCO2 gas concentration([ppm]) in the plant growth chamber.wc1,(k) andwc2,(k) denote thegas flow ([g/min]) of the pulse injection valves andwc3,(k) denotesthe photosynthetic photon flux ([µ-mol/m2s]) of the lights above theplant trays. The nonlinear expression

−1.516 · 10−4f1(wc1,(k−1))f2(xc3,(k−1))

approximates theCO2 gas production [g/min] due to photo-synthesis according to theCO2 gas concentration and chamberillumination[12]. This raw model defines a third order system ofdiscrete-time difference equations with sampling periodTs = 1[min]:

xc1,(k) = 0.5 uc1,(k−1) + vs1,(k−1)

xc2,(k) = 0.5 uc1,(k−1) + vs2,(k−1)

xc3,(k) = xc3,(k−1) + 20.163[−1.041+

1.141e−xc3,(k)/400.0 + xc1,(k−1) + xc2,(k−1)] + vs3,(k−1)

yc1,(k) = xc1,(k) + vo1,(k)

yc2,(k) = xc2,(k) + vo2,(k)

yc2,(k) = xc3,(k) + vo3,(k),(11)

uc1 xc1

wc3

yc1

xc2 yc2

xc3 yc3

1204.0

wc2

0.0

wc1

Figure 13. Causal graph of the BIO-Plex cPHA raw model (9)

The causal graph (Fig. 13) of the raw model (9) leads to the de-composition of the system as shown in Fig. 14 (our implementationof the causal analysis and decomposition algorithms treats constantvalues, such as the value 1204.0 for the photosynthetic photon flux,as known exogenous inputs with constant value). The decompositionof the model leads to a filter cluster with 3 extended Kalman filters -one for each flow regulator and one for the remaining system (pulseinjection valves, lighting system and plant growth chamber). Thisenables us to estimate the mode and continuous state of the flow reg-ulators independent of the remaining system. As a consequence, anunknown mode in a flow regulator does not cause any implicationson the estimation of the remaining system.

uc1 xc1

wc1

yc1

uyc1

cluster 1 {FR1}

xc2 yc2

cluster 2 {FR2}

xc3 yc3

cluster 3 {PIV1, PIV2, LS, PGC}

uyc2

wc2

wc3

1204.0

0.0

Figure 14. Partitioned causal SCC graph of the BIO-Plex cPHA model

Fig. 15 shows the continuous input (control signal)uc1, observedflow rates for flow regulator 1 and 2 and theCO2 concentration forthe experiment. Both flow regulators provide half of the requestedgas injection rate up tok = 700. At this time point, the second flowregulator starts to slowly drift towards its positive limit which it willreach at approximatelyk = 800. The camber control system re-acts immediately and lowers the control signal in order to keep theCO2 concentration at the requested 1200 ppm concentration. Thistransient behavior causes a slight bump in theCO2 concentrationas shown in Fig. 15-b. Our hybrid mode estimation system detectsthis unmodeled fault atk = 727 and declares flow regulator 2 to bein an unknown mode (we indicate the unknown mode by the modenumber 0 in Fig. 16). The flow regulator modestuck-open(mr5) be-

650 700 750 800 850

0

1

2

3

4

5

6Flow Regulator 2 Estimation Detail

time [minutes]

mod

e nu

mbe

r

727 769

Figure 16. Mode estimate detail for flow regulator 2

comes more and more likely as the regulator drifts towards its openposition. Hybrid mode estimation prefers this mode as symptom ex-

Page 94: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

600 700 800 900 1000 1100 1200 1300 14000

0.2

0.4

0.6

0.8

1

time [minutes]

CO

2 ga

s in

flow

rat

e [g

/min

]

control input

inflow rate FR2

inflow rate FR1

727

(a) Control inputuc and measuredCO2 input flow rates

600 700 800 900 1000 1100 1200 1300 14001120

1140

1160

1180

1200

1220

1240

time [minutes]

CO

2 co

ncen

trat

ion

[ppm

]

727

(b) CO2 level in PGC (measurement - gray/green, estimate -black)

Figure 15. Observed data and continuous estimation of theCO2 concentration in plant growth chamber

planation fromk = 769 onwards, although flow regulator 2 goesinto saturation a little bit later atk = 800.

The light fault atk = 900 is detected almost instantly atk = 904(ml4). This good discrimination among the pre-specified modes(failure and nominal) is further demonstrated at the terminationpoints of the faults. Repairs of the flow regulator 2 and the lightingsystem are detected immediately atk = 1101 andk = 1301, re-spectively. Fig. 17 shows the mode estimation result for the lightingsystem and flow regulator 2 over the entire experiment horizon.

600 700 800 900 1000 1100 1200 1300 1400

0

1

2

3

4

5

6Flow Regulator 2

time [minutes]

mod

e nu

mbe

r

600 700 800 900 1000 1100 1200 1300 1400

0

1

2

3

4

5

6Lighting System

time [minutes]

mod

e nu

mbe

r

Figure 17. Mode estimates for flow regulator 2 and lighting system

5 Implementation and Discussion

The implementation of our hybrid estimation scheme extends previ-ous work on hybrid estimation [9] and is written in Common LISP.

The hybrid estimator uses a cPHA description and performs decom-position and estimation, as outlined above. Decomposition is doneon-line according to the mode hypotheses that are tested in the courseof hybrid estimation. In general, it can be assumed that the the modein the system evolves on a lower rate than the hybrid estimationrate, which operates on the sampling periodTs. Therefore, we cacherecent decompositions and their corresponding filters for re-use asa compromise between a-priori calculation (space complexity) andpure on-line deduction (time complexity).

Optimized model-based estimation schemes, such asLivingstone[22], utilize conflicts to focus the underlying searchoperation. A conflict is a (partial) mode assignment that makes ahypothesis very unlikely. This requires a more general treatmentof unknown modes compared to the filter decomposition taskintroduced above. The decompositional model-based learningsystem Moriarty[21] introduced continuous variants of conflicts,so-calleddissents. We are currently reformulating these dissents forhybrid systems and investigate their incorporation to improve theunderlying search scheme. This will lead to an overall frameworkthat unifies our previous work on Livingstone, Moriarty and hybridestimation.

REFERENCES

[1] A. Aho, J. Hopcroft, and J. Ullman,Data Structures and Algorithms,Addison-Wesley, 1983.

[2] B. Anderson and J. Moore,Optimal Filtering, Information and SystemSciences Series, Prentice Hall, 1979.

[3] Grobner Bases and Applications, eds., B. Buchberger and F. Winkler,Cambridge Univ. Press, 1998.

[4] J. Chen and R. Patton,Robust Model-Based Fault Diagnosis for Dy-namic Systems, Kluwer, 1999.

[5] J. de Kleer and B. Williams, ‘Diagnosing multiple faults’,Artificial In-telligence, 32(1), 97–130, (1987).

[6] J. de Kleer and B. Williams, ‘Diagnosis with behavioral modes’, inPro-ceedings of IJCAI-89, pp. 1324–1330, (1989).

[7] A. Gehin, M. Assas, and M. Staroswiecki, ‘Structural analysis of sys-

Page 95: Rao-Blackwellised Particle Filtering for Dynamic …motionplanning/reading/me577Particle...Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks Arnaud Doucet Engineering

tem reconfigurability’, inPreprints of the 4th IFAC SAFEPROCESSSymposium, volume 1, pp. 292–297, (2000).

[8] Readings in Model-Based Diagnosis, eds., W. Hamscher, L. Console,and J. de Kleer, Morgan Kaufmann, San Mateo, CA, 1992.

[9] M. Hofbaur and B.C. Williams, ‘Mode estimation of probabilistic hy-brid systems’, inHybrid Systems: Computation and Control, HSCC2002, eds., C.J. Tomline and M.R. Greenstreet, volume 2289 ofLec-ture Notes in Computer Science, 253–266, Springer Verlag, (2002).

[10] P. Li and V. Kadirkamanathan, ‘Particle filtering based likelyhood ra-tio approach to fault diagnosis in nonlinear stochastic systems’,IEEETransactions on Systems, Man, and Cybernetics - Part C, 31(3), 337–343, (2001).

[11] X.R. Li and Y. Bar-Shalom, ‘Multiple-model estimation with vari-able structure’,IEEE Transactions on Automatic Control, 41, 478–493,(1996).

[12] J. T. Malin, L. Fleming, and T. R. Hatfield, ‘Interactive simulation-based testing of product gas transfer integrated monitoring and controlsoftware for the lunar mars life support phase III test’, inSAE 28th In-ternational Conference on Environmental Systems, Danvers MA, (July,1998).

[13] P. Maybeck and R.D. Stevens, ‘Reconfigurable flight control via multi-ple model adaptive control methods’,IEEE Transactions on Aerospaceand Electronic Systems, 27(3), 470–480, (1991).

[14] S. McIlraith, ‘Diagnosing hybrid systems: a bayseian model selectionapproach’, inProceedings of the 11th International Workshop on Prin-ciples of Diagnosis (DX00), pp. 140–146, (June 2000).

[15] P.J. Mosterman and G. Biswas, ‘Building hybrid observers for complexdynamic systems using model abstractions’, inHybrid Systems: Com-putation and Control (HSCC’99), eds., F. Vaandrager and J. Schuppen,volume 1569 ofLNCS, 178–192, Springer Verlag, (1999).

[16] S. Narasimhan and G. Biswas, ‘Efficient diagnosis of hybrid systemsusing models of the supervisory controller’, inProceedings of the 12thInternational Workshop on Principles of Diagnosis (DX01), pp. 127–134, (March 2001).

[17] P. Nayak,Automated Modelling of Physical Systems, Lecture Notes inArtificial Intelligence, Springer, 1995.

[18] E. Sontag,Mathematical Control Theory: Deterministic Finite Dimen-sional Systems, Springer, New York, Berlin, Heidelberg, 2 edn., 1998.

[19] P. Struss and O. Dressler, ‘Physical negation: Integrating fault modelsinto the general diagnostic engine’, inProceedings of the InternationalJoint Conference on Artificial Intelligence (IJCAI’89), pp. 1318–1323,(1989).

[20] L. Trave-Massuyes and R. Pons, ‘Causal ordering for multiple modesystems’, inProceedings of the 11th International Workshop on Quali-tative Reasoning (QR97), pp. 203–214, (1997).

[21] B. Williams and B. Millar, ‘Decompositional, model-based learningand its analogy to diagnosis’, inProceedings of the 15th National Con-ference on Artificial Intelligence (AAAI-98), (1998).

[22] B. Williams and P. Nayak, ‘A model-based approach to reactive self-configuring systems’, inProceedings of the 13th National Conferenceon Artificial Intelligence (AAAI-96), (1996).

[23] A. S. Willsky, ‘A survey of design methods for failure detection in dy-namic systems’,Automatica, 12(6), 601–611, (1974).

[24] F. Zhao, X. Koutsoukos, H. Haussecker, J. Reich, and P. Cheung, ‘Dis-tributed monitoring of hybrid systems: A model-directed approach’, inProceedings of the International Joint Conference on Artificial Intelli-gence (IJCAI’01), pp. 557–564, (2001).

Acknowledgments

In part supported by NASA under contract NAG2-1388.

A Extended Kalman Filter

The disturbances and imprecise knowledge about the initial statexc,(0) make it necessary to estimate the state by its meanxc,(k)

and covariance matrixP(k). We use an extended Kalman filter[2]for this purpose, which updates its current state, like an HMM ob-server, in two steps. The first step uses the model to predict meanfor the statexc,(•k) and its covarianceP(•k), based on the previous

estimate〈xc,(k−1),P(k−1)〉, and the control inputuc,(k−1):

xc,(•k) = f(xc,(k−1),uc,(k−1)) (12)

A(k−1) =∂f

∂x

∣∣∣∣xc,(k−1),uc,(k−1)

(13)

P(•k) = A(k−1)P(k−1)AT(k−1) + Q. (14)

This one-step ahead prediction leads to a prediction residualr(k)

with covariance matrixS(k)

r(k) = yc,(k) − g(xc,(•k),uc,(k)) (15)

C(k) =∂g

∂x

∣∣∣∣xc,(•k),uc,(k)

(16)

S(k) = C(k)P(•k)CT(k) + R. (17)

The second filter step calculates the Kalman filter gainK(k), andrefines the prediction as follows:

K(k) = P(•k)CT(k)S

−1(k) (18)

xc,(k) = xc,(•k) + K(k)r(k) (19)

P(k) =[I−K(k)C(k)

]P(•k). (20)

The output of the extended Kalman filter, as used in our hybrid esti-mation system, is a sequence of mean/covariance pairs〈xc,(k),P(k)〉for xc,(k) as well as the hybrid probabilistic observation function

PO(y(k)|x(k),uc,(k)) = e−rT

(k)S−1(k)r(k)/2

. (21)