A New Formulation of the Rao-Blackwellized Particle Filter

(1)

Technical report from Automatic Control at Linköpings universitet

A New Formulation of the Rao-Blackwellized Particle Filter

Gustaf Hendeby

,

Rickard Karlsson

,

Fredrik Gustafsson

Division of Automatic Control

E-mail:

hendeby@isy.liu.se

,

rickard@isy.liu.se

,

fredrik@isy.liu.se

13th August 2007

Report no.:

LiTH-ISY-R-2813

Accepted for publication in Proceedings of the IEEE Statistical Signal Processing Workshop, Madison,

WI, 2007

Address:

Department of Electrical Engineering Linköpings universitet

SE-581 83 Linköping, Sweden

WWW:http://www.control.isy.liu.se

AUTOMATIC CONTROL REGLERTEKNIK LINKÖPINGS UNIVERSITET

Technical reports from the Automatic Control group in Linköping are available from

(2)

(3)

Abstract

For performance gain and efficiency it is important to utilize model structure in particle filtering. Applying Bayes’ rule, present linear Gaussian substructure can be efficiently handled by a bank of Kalman filters. This is the standard formulation of the Rao-Blackwellized particle filter (RBPF), by some authors denoted the marginalized particle filter (MPF), and usually presented in a way that makes it hard to implement in an object oriented fashion. This paper discusses how the solution can be rewritten in order to increase the understanding as well as simplify the implementation and reuse of standard filtering components, such as Kalman filter banks and particle filters. Calculations show that the new algorithm is equivalent to the classical formulation, and the new algorithm is exemplified in a target tracking simulation study.

Keywords: Particle filter, Rao-Blackwellization, Kalman filter, Object oriented design, Marginalized particle filter

(4)

(5)

1. INTRODUCTION

The particle filter (PF), [1,2], is the fundamental solution to many recursive Bayesian filtering problems, incorporating both nonlinearity and non-Gaussianity in an optimal way. For linear and Gaussian systems the optimal solution is given by the Kalman filter (KF), [3,4]. Furthermore, the Rao-Blackwellized particle filter (RBPF), sometimes denoted the marginalized particle filter (MPF) or mixture Kalman filters, [5, 6, 7, 8,

9,10,11] improves the performance when a linear-Gaussian substructure is present. A recent survey of different particle filter related methods is given in [12].

TheRBPF divides the state vector xtinto two parts, one part xp_t, which is estimated using thePF, and another part xk

t, whereKFs are used. Basically, denoting the measurements and states up to time t with Yt= {yj}tj=0and Xt= {xj}tj=1, respectively, the joint probability density function (PDF) is given using Bayes’ rule as

p(Xp, xk_t|Yt) = p(xkt|X p , Yt) | {z } KF p(Xpt|Yt) | {z } PF . (1)

If the model is conditionally linear Gaussian, i.e., if the term p(xk

t|X p

t, Yt) is linear Gaussian, it can be optimally estimated using theKF. To obtain the second factor it is necessary to resort to using thePF.

2. RAO-BLACKWELLIZED PARTICLE FILTER (RBPF)

A fairly general state space setting where theRBPF can be applied, [10,13,11], is given by the model

xp_t+1= fp(xp_t) + Fp(x_tp)xk_t + Gp(xp_t)wp_t (2a) xkt+1= fk(x p t) + Fk(x p t)xkt + Gk(x p t)wtk (2b) yt= h(x p t) + H(x p t)x k t + et, (2c)

where the noises are assumed independent and Gaussian, w_tp∼ N (0, Qpt), w k t ∼ N (0, Q k t), and et∼ N (0, Rt). 2.1. RBPF — Standard Formulation

The standard approach to implementing theRBPFfor the model structure in (2) is given in for instance [10,13,11]. The al-gorithm is summarized in Alal-gorithm1. Here, theKFandPF

parts interact heavily and steps in the respective algorithms are mixed, it is therefore difficult to clearly see the problem structure and how to use standard components in the filtering. Also, the time update and measurement update of the vari-ous filters are neither completely separated nor clearly stated. This hinders reuse of standard components such as existing

KFandPFfilter code when implementing the filter.

Note that for the special case, when there is no linear term in (2c) and that the linear dynamics is independent of xp, i.e.,

Algorithm 1 Rao-BlackwellizedPF(Standard formulation)

1: Initialization: For i = 1, . . . , N , xp_0|−1 ∼ pxp₀(x p 0) and set {xk

0|−1, P0|−1} = {xk0, P0}. Let t = 0.

2: PFmeasurement update: For i = 1, . . . , N , evaluate and normalize the importance weights using to the likelihood

p(yt|Xpt, Yt−1) = N h(xp_t) + H(xp_t)ˆxk_t|t−1 H(xp_t)Pt|t−1(H(x p t)) T _{+ R} t . Resample the particles if needed.

3: PFtime update and Kalman filter update (a) KFmeasurement update

xp_t+1∼ N“fp(xpt) + F p (xpt)ˆx k t|t Fp(xpt)Pt|t(F p (xpt)) T + Gp(xpt)Q p t`G p (xpt) ´T” (c) KFtime update, ˆ xk_t+1|t= Fkˆxk_t|t+ fk(xp_t) + Lt`xp_t+1|t− fp(xpt) − Fp(x p t)ˆxkt|t ´ Pt+1|t= Fk(x p t)Pt|t(Fk(x p t)) T_{+ G}k_(xp t)Q k t`Gk(x p t) ´T − LtMtLTt Mt= Fk(xpt)Pt|t(Fk(x p t))T+ Gp(x p t)Q p t`Gk(x p t) ´T Lt= Fk(xp_t)Pt|t(Fp(x p t)) T_M−1 t .

4: Set t := t + 1 and repeat from step 2.

H = 0 and Fp(xpt), Fk(xkt), Gp(x p

t), and Gk(xkt) are con-stant matrices, the covariance recursion is the same for all filters. Hence, this can be utilized in order to save computa-tions.

2.2. RBPF — New Formulation

In [14] an object oriented general filtering software in C++ calledF++1_{is presented. When implementing new algorithms,}

such as the RBPF, it is important to reuse various software components. For instance the RBPF could in principle use previously developed PFandKFcomponents. However, us-ing Algorithm1this is neither very easy nor straightforward

(6)

Filters Kalman Particle Filters Kalman Particle Filter Kalman Filter Mixing Step

Measurement Update Time Update Model Interaction

See Section 2.2.2 See Section 2.2.1 See Section 2.2.3 Filter ˆ x x_ˆ y

Fig. 1. Illustration of the newRBPFformulation. The internal dependency between the filters in the measurement and time update are indicated with arrows.

to do.

In this section a newRBPFalgorithm is presented that is formulated in terms of well-knownKFandPFsteps followed by a simple compensation step. The structural insight given by this new algorithm formulation helps to give a better un-derstanding of theRBPFalgorithm and at the same time it al-lows for efficient software development in terms of standard signal processing components.

The key to the new algorithm formulation is to view the

RBPFas interacting filters working with different system mod-els. The linear part is captured in one model, which has the nonlinear components as input, and the nonlinear effects are captured in a nonlinear model with only the nonlinear parts. The noise in the nonlinear model depends on the estimates from the linear model. This is illustrated in Fig.1, and scribed in the subsequent sections. Of course, the above de-scription is a bit overly simplified since it is sometimes mo-tivated to put linear states in the nonlinear model. For a dis-cussion about that, see [13]. The sequel of this section will explain this in more detail. First the result is stated and then the derivation is given in Section2.2.4.

2.2.1. Kalman Filter Model

The model for the Kalman filters is obtained form model (2), using the conditional linear Gaussian property in (1) (i.e., xp_t is considered known). The model has the state vector ¯xk ₌

ξ

xk, where ξ basically is x

p_{. It turns out that keeping ξ} provides an easy and efficient way to compute many quanti-ties needed in the algorithm. The linear and Gaussian model (when xpt is given) becomes:

¯ xk_t+1= ξt+1 xk_t+1 =0 F p_(xp t) 0 Fk(xp_t) ξt xk_t +G p_(xp t) 0 0 Gk(xp_t) wp t w_tk +u p t uk_t (4a) = ¯Ftkx¯ k t + ¯G k tw¯ k t + ¯u k t (4b) yt= 0 H(x p t) ξt xkt + uth+ et= ¯Htkx¯kt + uht + et, (4c)

where the nonlinear effects in (2) enter as external input to the system, upt = fp(x p t), ukt = fk(x p t), and uht = h(x p t), and ¯ Qt= Qp_t 0 0 Qk t .

2.2.2. Particle Filter Model

The nonlinear effects that are not taken care of in the linear model in Section2.2.1are put into a model to be handled by aPF. The resulting model for the model structure used in this paper is

xp_t+1= ¯w_tp (5a)

yt= ¯et, (5b)

with the noise ¯ w_tp∼ N ( ˆξt+1, P_t+1|tξ ), (5c) ¯ et∼ N h(xpt) + H(x p t)ˆx k t+1|t, St, (5d) where St= H(xpt)P k t|t−1 H(x p t) T + Rt, (5e) and where the covariance Pkand Pξare given in detail in (8) (Section2.2.4). Here, ˆξ and ˆxkare used to denote the estimate of the two parts of ˆx. At a first glance this seems to be a very¯ simple model, however, there is a nontrivial connection to the linear model hidden in the noise description.

2.2.3. Model Interaction

The suggested formulation of theRBPFis now the one given in Algorithm2. In itKFandPFare run in parallel followed by a mixing/combining step, which is performed after that both the KFand the PF have been time updated to adjust the KF

For details regarding the covariance calculation refer to Sec-tion2.2.4.

Also, see Section2.2.5for an alternative interpretation of this as aKFmeasurement update in response to a virtual mea-surement.

2.2.4. The New RBPF Algorithm

In Algorithm2the new formulation is summarized. Note that theKFmeasurement and time update steps are the traditional update of the mean value and the covariance in aKF. For the

PFthey represent a standardPFwith weight computation and resampling, and simulation of the system. This is all standard components. Schematically, Algorithm2can be depicted as in Fig.1.

(7)

Algorithm 2 Rao-BlackwellizedPF(New formulation)

1: Initialization (same as standard formulation): For i = 1, . . . , N , xp,(i)_0|−1 ∼ pxp₀(x p 0) and set {ˆx¯ k,(i) 0|−1, ¯P (i) 0|−1} = (xp0 xk 0 ), (0 0_{0 ¯}_P 0) . Let t = 0. (

(i)_{is used to denote particle} number, and ˆξ and ˆxp indicate estimates of the parts of ˆ

¯ xk.)

2: Measurement update

(a) PFmeasurement update using (5): For i = 1, . . . , N , evaluate and normalize the importance weights using the likelihood p(yt|X p t, Yt−1) = N (ˆy (i) t , St) ˆ

y(i)_t = ¯H_tk(i)xˆ¯k(i)+ uh(i)_t

S_t(i)= ¯H_tk(i)P¯_t|t−1k(i) ( ¯H_t|t−1k(i) )T + Rt, with uh

t = h(x p(i) t ).

(b) KF measurement update using (4) (depends on the

PF): ˆ ¯

xk(i)_t|t = ¯xk(i)_t|t−1− ¯P_tk(i)( ¯H_t|t−1k(i) )T(S_t(i))−1(i)_t (i)t = yt− ˆy

(i) t , ¯

P_t|tk(i)= ¯P_t|t−1k(i)

− ¯P_t|t−1k(i) ( ¯H_t|t−1k(i) )T(S_t(i))−1H¯_t|t−1k(i) P¯_t|t−1k(i) . (c) Resample if necessary.

3: Time update

(a) KFtime update using (4): ˆ

¯

xk(i)_t+1|t= ¯F_tk(i)xˆ¯k(i)_t|t + ¯uk(i)_t ¯

P_t+1|tk(i) = ¯F_tk(i)P¯_t|tk(i)( ¯F_tk(i))T+ ¯Gk(i)_t Q¯k_t( ¯Gk(i)_t )T with ¯uk(i)_t = fp_(xp(i)

t )T fk(x p(i) t )T

T . (b) PFtime update using (5) (depends on theKF):

xp(i)_t+1∼ N ( ˆξ_t+1|t(i) , P_t+1|tξ(i) ) (c) Mixing step, update theKFusing (6).

ˆ

4: Increase time and repeat from step 2.

Note again that for some special cases the covariance re-cursion is the same for all filters. Hence, this can be utilized in order to save computations. The computational complex-ity of different partitions and the complexcomplex-ity betweenPFand

RBPFare analyzed extensively in [13]. Derivation of Algorithm2

Here, Algorithm2will be verified by showing that the result-ing equations are identical to those in the standard algorithm formulation.

Applying theKFto the linear system (4) yields ˆ_ξ_t+1|t ˆ xk t+1|t ! = F p_(xp t)ˆxkt|t+ u p t Fk_(xp t)ˆxkt|t+ u k t ! , (7a)

and the covariance matrix ¯Pk := (Pξ _Pξk

The covariance follows from expanding the covariance measurement update in theKFfor the extended linear system:

The result follows directly from conducting the matrix multi-plications block-wise.

(8)

Combine the time update step with the model interaction in (6) and the equations in the linear part in Algorithm1are obtained.

Furthermore, looking at the equations for the ξ estimates, it is clear that with the noise in (5) thePFis identical to that in Algorithm1. Thus, the new algorithm formulation is equiva-lent to the standard formulation.

Note here that appending the linear part with the dummy variable ξ does not impose any unnecessary computations since the ξ information is used in thePF and in the model inter-action step. Given that the KF is a well studied algorithm, with many efficient implementations, the implementation of the algorithm is likely to be both more efficient and numeri-cally well-behaved using theKFformulation, than if reimple-mented from scratch.

2.2.5. Alternative Interpretation

It should also be noted that it is possible to show an alternative interpretation of Algorithm2. Let ξ be xp_{, and then combine} steps 3(b) and 3(c) in Algorithm2 to one, where step 3(b) generates virtual measurements, yp(i)_{, and look at 3(c) as a} measurement update for the new measurement

yp= I 0 ¯x.

Furthermore, with this formulation, step 2(a) can be inter-preted as a weighting step of a filter bank. Now theRBPFis in fact a conditional Kalman filter bank, with random virtual measurements, rather than a modified particle filter.

3. EXAMPLE

To exemplify the newRBPFalgorithm formulation, the target tracking example from [13] is revisited.

3.1. Model

The problem of estimating the position and velocity of an air-craft is studied in a target tracking example using a simple 2D constant acceleration model, where the range and bearing to the aircraft are measured:

xt+1=         1 0 T 0 T2_/2 ₀ 0 1 0 T 0 T2_/2 0 0 1 0 T 0 0 0 0 1 0 T 0 0 0 0 1 0 0 0 0 0 0 1         xt+ wt (9a) yt= r ϕ = q p2 x+ p2y arctan (py/px) ! + et, (9b)

where the state vector is xt = (px py vx vy ax ay)T, i.e., position, velocity, and acceleration, with sample period T = 1, and where r and ϕ are range and bearing measurements.

The dashed line indicate the division into the parts present in (2).

Using the notation in model (2) and the formulation in Section2.2.1, the system can be written as:

Fp=1 0 0.5 0 0 1 0 0.5 , Gp= I2×2, Gk = I4×4, H = O2×4, uht = h(x p t) = q p2 x+ p2y arctan (py/px) ! .

The input signal are upt = fp(¯x) = x p

t and ukt = fk(¯x) = 0. The noise etis Gaussian with zero mean and covariance R = cov e = diag(100, 10−6_{). The process noises are assumed} Gaussian with zero mean and covariances

Qp= cov wp= diag(1, 1)

Qk = cov wk = diag(1, 1, 0.01, 0.01).

The nonlinear effects that are not taken care of in the lin-ear model are put into a model to be handled by aPFaccording to (5).

The resulting linear model is therefore,

¯ xk_t+1=         0 0 1 0 1₂ 0 0 0 0 1 0 1₂ 0 0 1 0 1 0 0 0 0 1 0 1 0 0 0 0 1 0 0 0 0 0 0 1         ¯ xk_t+u p t 0 + wt, (10a) y = uh_t + et, (10b)

and the nonlinear model follows immediately.

To verify the algorithm numerically theF++ software was used for Monte Carlo simulations using the above model struc-ture. In Fig.2the positionRMSEfrom 100 Monte Carlo sim-ulations for thePFand theRBPFis depicted using N = 2000 particles. The computational complexity forRBPF vsPFfor the described system is analyzed in detail in [13], and not part of this paper.

4. CONCLUSIONS

This paper presents the Rao-Blackwellized particle filter (RBPF) in a new structured way that is more suitable for implemen-tation. This enables easy integration with previously well-studied algorithms, such as PFandKF, together with an in-creased insight into the inner workings of the algorithm. This is particularly interesting in an object oriented framework, such as the software packageF++. The algorithm is verified in calculations, and exemplified in a Monte Carlo study.

5. REFERENCES

[1] N. J. Gordon, D. J. Salmond, and A. F. M. Smith, “Novel approach to nonlinear/non-Gaussian Bayesian state

(9)

esti-5 10 15 20 25 30 35 40 45 50 0 1 2 3 4 5 6 7 8 9 10 Time [s] Pos. RMSE [m] PF RBPF

Fig. 2. PositionRMSEfor the target tracking example using 100 Monte Carlo simulation with N = 2000 particles. The

PFestimates are compared to those from theRBPF.

mation,” IEE Proceedings-F Radar and Signal Process-ing, vol. 140, no. 2, pp. 107–113, Apr. 1993.

[2] A. Doucet, N. de Freitas, and N. Gordon, Eds., Sequen-tial Monte Carlo Methods in Practice, ser. Statistics for Engineering and Information Science. New York: Springer-Verlag, 2001.

[3] R. E. Kalman, “A new approach to linear filtering and prediction problems,” Tranactions of the American So-ciety of Mechanical Engineering — Journal Basic En-gieering, Series D, vol. 82, pp. 35–45, Mar. 1960. [4] T. Kailath, A. H. Sayed, and B. Hassibi, Linear

Estima-tion. Prentice-Hall, Inc, 2000.

[5] A. Doucet, S. Godsill, and C. Andrieu, “On sequential Monte Carlo sampling methods for Bayesian filtering,” Statistics and Computing, vol. 10, pp. 197–208, 2000. [6] G. Casella and C. P. Robert, “Rao-Blackwellisation of

sampling schemes,” Biometrika, vol. 83, no. 1, pp. 81– 94, 1996.

[7] A. Doucet, N. J. Gordon, and V. Krishnamurthy, “Parti-cle filters for state estimation of jump markov linear sys-tems,” IEEE Transactions on Signal Processing, vol. 49, no. 3, pp. 613–624, Mar. 2001.

[8] R. Chen and J. S. Liu, “Mixture Kalman filter,” Jour-nal of the Royal Statistical Society. Series B (Statistical Methodology), vol. 62, no. 3, pp. 493–508, 2000. [9] C. Andrieu and A. Doucet, “Particle filter for partially

observed Gaussian state space models,” Journal of the Royal Statistical Society. Series B (Statistical Methodol-ogy), vol. 64, no. 4, pp. 827–836, 2002.

[10] T. Schön, F. Gustafsson, and P.-J. Nordlund, “Marginal-ized particle filters for mixed linear / nonlinear state-space models,” IEEE Transactions on Signal Process-ing, vol. 53, no. 7, pp. 2279–2289, Jul. 2005.

[11] T. B. Schön, R. Karlsson, and F. Gustafsson, “The marginalized particle filter in practice,” in Proceedings of IEEE Aerospace Conference, Big Sky, MT, USA, Mar. 2006.

[12] O. Cappé, S. J. Godsill, and E. Moulines, “An overview of existing methods and recent advances in sequential Monte Carlo,” Proceedings of the IEEE, 2007, (To Ap-pear.).

[13] R. Karlsson, T. B. Schön, and F. Gustafsson, “Com-plexity analysis of the marginalized particle filter,” IEEE Transactions on Signal Processing, vol. 53, no. 11, pp. 4408–4411, Nov. 2005.

[14] G. Hendeby and R. Karlsson, “Target tracking perfor-mance evaluation — a general software environment for filtering,” in Proceedings of IEEE Aerospace Confer-ence, Big Sky, MT, USA, Mar. 2007.

(10)

(11)

Avdelning, Institution Division, Department

Division of Automatic Control Department of Electrical Engineering

Datum Date 2007-08-13 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport

URL för elektronisk version

http://www.control.isy.liu.se

ISBN — ISRN

—

Serietitel och serienummer Title of series, numbering

ISSN 1400-3902

LiTH-ISY-R-2813

Titel Title

A New Formulation of the Rao-Blackwellized Particle Filter

Författare Author

Gustaf Hendeby, Rickard Karlsson, Fredrik Gustafsson

Sammanfattning Abstract

For performance gain and efficiency it is important to utilize model structure in particle filtering. Applying Bayes’ rule, present linear Gaussian substructure can be efficiently handled by a bank of Kalman filters. This is the standard formulation of the Rao-Blackwellized particle filter (RBPF), by some authors denoted the marginalized particle filter (MPF), and usually presented in a way that makes it hard to implement in an object oriented fashion. This paper discusses how the solution can be rewritten in order to increase the understanding as well as simplify the implementation and reuse of standard filtering components, such as Kalman filter banks and particle filters. Calculations show that the new algorithm is equivalent to the classical formulation, and the new algorithm is exemplified in a target tracking simulation study.

Nyckelord

(12)