TRANSACTIONS 174
IEEE ON SIGNAL PROCESSING, VOL. 50, NO. 2, FEBRUARY 2002
A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking
M. Sanjeev Arulampalam, Simon Maskell, Neil Gordon, and Tim Clapp
AbstractIncreasingly, for many application areas, it is becoming important to include elements of nonlinearity and non-Gaussianity in order to model accurately the underlying dynamics of a physical system. Moreover, it is typically crucial to process data on-line as it arrives, both from the point of view of storage costs as well as for rapid adaptation to changing signal characteristics. In this paper, we review both optimal and suboptimal Bayesian algorithms for nonlinear/non-Gaussian tracking problems, with a focus on particle filters. Particle filters are sequential Monte Carlo methods based on point mass (or particle) representations of probability densities, which can be applied to any state-space model and which generalize the traditional Kalman filtering methods. Several variants of the particle filter such as SIR, ASIR, and RPF are introduced within a generic framework of the sequential importance sampling (SIS) algorithm. These are discussed and compared with the standard EKF through an illustrative example. Index TermsBayesian, nonlinear/non-Gaussian, filters, sequential Monte Carlo, tracking. particle
I. INTRODUCTION ANY problems in science require estimation of the state of a system that changes over time using a sequence of noisy measurements made on the system. In this paper, we will concentrate on the state-space approach to modeling dynamic systems, and the focus will be on the discrete-time formulation of the problem. Thus, difference equations are used to model the evolution of the system with time, and measurements are assumed to be available at discrete times. For dynamic state estimation, the discrete-time approach is widespread and convenient. The state-space approach to time-series modeling focuses attention on the state vector of a system. The state vector contains all relevant information required to describe the system under investigation. For example, in tracking problems, this information could be related to the kinematic characteristics of the target. Alternatively, in an econometrics problem, it could be
Manuscript received February 8, 2001; revised October 15, 2001. S. Arulampalam was supported by the Royal Academy of Engineering with an AngloAustralian Post-Doctoral Research Fellowship. S. Maskell was supported by the Royal Commission for the Exhibition of 1851 with an Industrial Fellowship. The associate editor coordinating the review of this paper and approving it for publication was Dr. Petar M. Djuric. M. S. Arulampalam is with the Defence Science and Technology Organisation, Adelaide, Australia (e-mail: [e-mail]). S. Maskell and N. Gordon are with the Pattern and Information Processing Group, QinetiQ, Ltd., Malvern, U.K., and Cambridge University Engineering Department, Cambridge, U.K. (e-mail: [e-mail]; [e-mail]). T. Clapp is with Astrium Ltd., Stevenage, U.K. (e-mail: [e-mail]). Publisher Item Identifier S 1053-587X(02)00569-X.
M
related to monetary flow, interest rates, inflation, etc. The measurement vector represents (noisy) observations that are related to the state vector. The measurement vector is generally (but not necessarily) of lower dimension than the state vector. The statespace approach is convenient for handling multivariate data and nonlinear/non-Gaussian processes, and it provides a significant advantage over traditional time-series techniques for these problems. A full description is provided in [41]. In addition, many varied examples illustrating the application of nonlinear/nonGaussian state space models are given in [26]. In order to analyze and make inference about a dynamic system, at least two models are required: First, a model describing the evolution of the state with time (the system model) and, second, a model relating the noisy measurements to the state (the measurement model). We will assume that these models are available in a probabilistic form. The probabilistic state-space formulation and the requirement for the updating of information on receipt of new measurements are ideally suited for the Bayesian approach. This provides a rigorous general framework for dynamic state estimation problems. In the Bayesian approach to dynamic state estimation, one attempts to construct the posterior probability density function (pdf) of the state based on all available information, including the set of received measurements. Since this pdf embodies all available statistical information, it may be said to be the complete solution to the estimation problem. In principle, an optimal (with respect to any criterion) estimate of the state may be obtained from the pdf. A measure of the accuracy of the estimate may also be obtained. For many problems, an estimate is required every time that a measurement is received. In this case, a recursive filter is a convenient solution. A recursive filtering approach means that received data can be processed sequentially rather than as a batch so that it is not necessary to store the complete data set nor to reprocess existing data if a new measurement becomes available.1 Such a filter consists of essentially two stages: prediction and update. The prediction stage uses the system model to predict the state pdf forward from one measurement time to the next. Since the state is usually subject to unknown disturbances (modeled as random noise), prediction generally translates, deforms, and spreads the state pdf. The update operation uses the latest measurement to modify the prediction pdf. This is achieved using Bayes theorem, which is the mechanism for updating knowledge about the target state in the light of extra information from new data.
1In this paper, we assume no out-of-sequence measurements; in the presence of out-of-sequence measurements, the order of times to which the measurements relate can differ from the order in which the measurements are processed. For a particle filter solution to the problem of relaxing this assumption, see [32].
1053587X/02$17.00 2002 IEEE
ARULAMPALAM et al.: TUTORIAL ON PARTICLE FILTERS
175
We begin in Section II with a description of the nonlinear tracking problem and its optimal Bayesian solution. When certain constraints hold, this optimal solution is tractable. The Kalman filter and grid-based filter, which is described in Section III, are two such solutions. Often, the optimal solution is intractable. The methods outlined in Section IV take several different approximation strategies to the optimal solution. These approaches include the extended Kalman filter, approximate grid-based filters, and particle filters. Finally, in Section VI, we use a simple scalar example to illustrate some points about the approaches discussed up to this point and then draw conclusions in Section VII. This paper is a tutorial; therefore, to facilitate easy implementation, the pseudo-code for algorithms has been included at relevant points. II. NONLINEAR BAYESIAN TRACKING To define the problem of tracking, consider the evolution of of a target given by the state sequence (1) is a possibly nonlinear function where , is an i.i.d. process noise seof the state are dimensions of the state and process noise quence, vectors, respectively, and is the set of natural numbers. The from meaobjective of tracking is to recursively estimate surements (2) is a possibly nonlinear funcwhere is an i.i.d. measurement noise sequence, tion, are dimensions of the measurement and measureand ment noise vectors, respectively. In particular, we seek filtered based on the set of all available measurements estimates of , up to time . From a Bayesian perspective, the tracking problem is to reat time cursively calculate some degree of belief in the state , taking different values, given the data up to time . Thus, . It is assumed that it is required to construct the pdf of the state vector, which is also the initial pdf known as the prior, is available ( being the set of no measuremay be obtained, ments). Then, in principle, the pdf recursively, in two stages: prediction and update. at time Suppose that the required pdf is available. The prediction stage involves using the system model (1) to obtain the prior pdf of the state at time via the ChapmanKolmogorov equation (3) , Note that in (3), use has been made of the fact that as (1) describes a Markov process of order one. The probabilistic model of the state evolution is defined by the system equation (1) and the . known statistics of
At time step , a measurement becomes available, and this may be used to update the prior (update stage) via Bayes rule (4) where the normalizing constant (5) defined by the depends on the likelihood function measurement model (2) and the known statistics of . In the is used to modify the update stage (4), the measurement prior density to obtain the required posterior density of the current state. The recurrence relations (3) and (4) form the basis for the optimal Bayesian solution.2 This recursive propagation of the posterior density is only a conceptual solution in that in general, it cannot be determined analytically. Solutions do exist in a restrictive set of cases, including the Kalman filter and grid-based filters described in the next section. We also describe how, when the analytic solution is intractable, extended Kalman filters, approximate grid-based filters, and particle filters approximate the optimal Bayesian solution. III. OPTIMAL ALGORITHMS A. Kalman Filter The Kalman filter assumes that the posterior density at every time step is Gaussian and, hence, parameterized by a mean and covariance. is Gaussian, it can be proved that If is also Gaussian, provided that certain assumptions hold [21]: and are drawn from Gaussian distributions of known parameters. is known and is a linear function of and . is a known linear function of and . That is, (1) and (2) can be rewritten as (6) (7) and are known matrices defining the linear functions. and are, respectively, and The covariances of . Here, we consider the case when and have zero mean and are statistically independent. Note that the system and and , as well as noise parameters measurement matrices and , are allowed to be time variant. The Kalman filter algorithm, which was derived using (3) and (4), can then be viewed as the following recursive relationship: (8) (9) (10)
2For clarity, the optimal Bayesian solution solves the problem of recursively calculating the exact posterior density. An optimal algorithm is a method for deducing this solution.
176
IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 50, NO. 2, FEBRUARY 2002
where (11) (12) (13) (14) is a Gaussian density with argument , and where mean , and covariance , and (15) (16) , and are the covariance of the innovation term the Kalman gain, respectively. In the above equations, the transis denoted by . pose of a matrix This is the optimal solution to the tracking problemif the (highly restrictive) assumptions hold. The implication is that no algorithm can ever do better than a Kalman filter in this linear Gaussian environment. It should be noted that it is possible to derive the same results using a least squares (LS) argument [22]. All the distributions are then described by their means and covariances, and the algorithm remains unaltered, but are not constrained to be Gaussian. Assuming the means and covariances to be unbiased and consistent, the filter then optimally derives the mean and covariance of the posterior. However, this posterior is not necessarily Gaussian, and therefore, if optimality is the ability of an algorithm to calculate the posterior, the filter is then not certain to be optimal. Similarly, if smoothed estimates of the states are required, , where ,3 then the that is, estimates of . Kalman smoother is the optimal estimator of This holds if is fixed (fixed-lag smoothing, if a batch of data (fixed-interval smoothing), or if are considered and the state at a particular time is of interest is fixed (fixed-point smoothing). The problem of calculating smoothed densities is of interest because the densities at time are then conditional not only on measurements up to and including time index but also on future measurements. Since there is more information on which to base the estimation, these smoothed densities are typically tighter than the filtered densities. Although this is true, there is an algorithmic issue that should be highlighted here. It is possible to formulate a backward-time Kalman filter that recurses through the data sequence from the final data to the first and then combines the estimates from the forward and backward passes to obtain overall smoothed estimates [20]. A different formulation implicitly calculates the backward-time state estimates and covariances, recursively estimating the smoothed quantities [38]. Both techniques are prone to having to calculate matrix inverses that do not necessarily exist. Instead, it is preferable to propagate different quantities using an information filter when carrying out the backward-time recursion [4].
B. Grid-Based Methods Grid-based methods provide the optimal recursion of the filif the state space is discrete and consists tered density of a finite number of states. Suppose the state space at time consists of discrete states , . For each state , let the conditional probability of that state, given meabe denoted by , that is, surements up to time . Then, the posterior pdf can be written as at (17) is the Dirac delta measure. Substitution of (17) into where (3) and (4) yields the prediction and update equations, respectively (18)
(19) where (20)
(21)
and are known The above assumes that but does not constrain the particular form of these discrete densities. Again, this is the optimal solution if the assumptions made hold. IV. SUBOPTIMAL ALGORITHMS In many situations of interest, the assumptions made above do not hold. The Kalman filter and grid-based methods cannot, therefore, be used as describedapproximations are necessary. In this section, we consider three approximate nonlinear Bayesian filters: a) extended Kalman filter (EKF); b) approximate grid-based methods; c) particle filters. A. Extended Kalman Filter If (1) and (2) cannot be rewritten in the form of (6) and (7) because the functions are nonlinear, then a local linearization of the equations may be a sufficient description of the nonlinearity. is approxThe EKF is based on this approximation. imated by a Gaussian (22) (23) (24)
3If ` = 0, then the problem reduces to the estimation of p(x ered up to this point.
j
z
) consid-
ARULAMPALAM et al.: TUTORIAL ON PARTICLE FILTERS
177
where (25) (26) (27) (28)
where (36)
(37) and are nonlinear functions, and and where now, and are local linearizations of these nonlinear functions (i.e., matrices) (29) denotes the center of the th cell at time index . Here, The integrals in (36) and (37) arise due to the fact that the grid , represent regions of continuous state points , space, and thus, the probabilities must be integrated over these regions. In practice, to simplify computation, a further approx. Specifically, these imation is made in the evaluation of weights are computed at the center of the cell corresponding to (38)
(30) (31) (32) The EKF as described above utilizes the first term in a Taylor expansion of the nonlinear function. A higher order EKF that retains further terms in the Taylor expansion exists, but the additional complexity has prohibited its widespread use. Recently, the unscented transform has been used in an EKF framework [23], [42], [43]. The resulting filter, which is known as the unscented Kalman filter, considers a set of points that are deterministically selected from the Gaussian approximation . These points are all propagated through the true to nonlinearity, and the parameters of the Gaussian approximation are then re-estimated. For some problems, this filter has been shown to give better performance than a standard EKF since it better approximates the nonlinearity; the parameters of the Gaussian approximation are improved. to However, the EKF always approximates be Gaussian. If the true density is non-Gaussian (e.g., if it is bimodal or heavily skewed), then a Gaussian can never describe it well. In such cases, approximate grid-based filters and particle filters will yield an improvement in performance in comparison to that of an EKF [1]. B. Approximate Grid-Based Methods If the state space is continuous but can be decomposed into cells, : , then a grid-based method can be used to approximate the posterior density. Specifically, is given suppose the approximation to the posterior pdf at by (33) Then, the prediction and update equations can be written as (34)
(39)
The grid must be sufficiently dense to get a good approximation to the continuous state space. As the dimensionality of the state space increases, the computational cost of the approach therefore increases dramatically. If the state space is not finite in extent, then using a grid-based approach necessitates some truncation of the state space. Another disadvantage of grid-based methods is that the state space must be predefined and, therefore, cannot be partitioned unevenly to give greater resolution in high probability density regions, unless prior knowledge is used. Hidden Markov model (HMM) filters [30], [35], [36], [39] are an application of such approximate grid-based methods in a fixed-interval smoothing context and have been used extensively in speech processing. In HMM-based tracking, a common approach is to use the Viterbi algorithm [18] to calculate the maximum a posteriori estimate of the path through the trellis, that is, the sequence of discrete states that maximizes the probability of the state sequence given the data. Another approach, due to BaumWelch [35], is to calculate the probability of each discrete state at each time epoch given the entire data sequence.4
V. PARTICLE FILTERING METHODS A. Sequential Importance Sampling (SIS) Algorithm The sequential importance sampling (SIS) algorithm is a Monte Carlo (MC) method that forms the basis for most sequential MC filters developed over the past decades; see [13],
4The Viterbi and BaumWelch algorithms are frequently applied when the state space is approximated to be discrete. The algorithms are optimal if and only if the underlying state space is truly discrete in nature.
(35)
178
IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 50, NO. 2, FEBRUARY 2002
[14]. This sequential MC (SMC) approach is known variously as bootstrap filtering [17], the condensation algorithm [29], particle filtering [6], interacting particle approximations [10], [11], and survival of the fittest [24]. It is a technique for implementing a recursive Bayesian filter by MC simulations. The key idea is to represent the required posterior density function by a set of random samples with associated weights and to compute estimates based on these samples and weights. As the number of samples becomes very large, this MC characterization becomes an equivalent representation to the usual functional description of the posterior pdf, and the SIS filter approaches the optimal Bayesian estimate. In order to develop the details of the algorithm, let denote a random measure that characterizes the , where , is a set posterior pdf , of support points with associated weights and , is the set of all states up to time . The weights are normalized such that . Then, the posterior density at can be approximated as (40) We therefore have a discrete weighted approximation to the . The weights are chosen using the true posterior, principle of importance sampling [3], [12]. This principle relies is a probability density on the following. Suppose can from which it is difficult to draw samples but for which up to proportionality]. In addition, be evaluated [as well as , be samples that are easily generlet called an importance density. Then, a ated from a proposal is given by weighted approximation to the density (41) where (42) is the normalized weight of the th particle. were drawn from an imporTherefore, if the samples , then the weights in (40) are defined tance density by (42) to be (43) Returning to the sequential case, at each iteration, one could have samples constituting an approximation to and want to approximate with a new set of samples. If the importance density is chosen to factorize such that (44) then one can obtain samples each of the existing samples the new state update equation, by augmenting with , . To derive the weight is first expressed in terms of
, , and be derived by integrating (45)
. Note that (4) can
(45)
(46) By substituting (44) and (46) into (43), the weight update equation can then be shown to be
(47) , , , then Furthermore, if and the importance density becomes only dependent on . This is particularly useful in the common case when only is required at each time step. a filtered estimate of From this point on, we will assume such a case, except when need be explicitly stated otherwise. In such scenarios, only and history of stored; therefore, one can discard the path . The modified weight is then observations (48) and the posterior filtered density mated as can be approxi-
(49) where the weights are defined in (48). It can be shown that as , the approximation (49) approaches the true posterior . density The SIS algorithm thus consists of recursive propagation of the weights and support points as each measurement is received sequentially. A pseudo-code description of this algorithm is given by algorithm 1.
Algorithm 1: SIS Particle Filter SIS FOR Draw , Assign the particle a weight, according to (48) END FOR
,
1) Degeneracy Problem: A common problem with the SIS particle filter is the degeneracy phenomenon, where after a few iterations, all but one particle will have negligible weight. It has
ARULAMPALAM et al.: TUTORIAL ON PARTICLE FILTERS
179
been shown [12] that the variance of the importance weights can only increase over time, and thus, it is impossible to avoid the degeneracy phenomenon. This degeneracy implies that a large computational effort is devoted to updating particles whose conis almost zero. A tribution to the approximation to suitable measure of degeneracy of the algorithm is the effective introduced in [3] and [28] and defined as sample size Var (50)
Analytic evaluation is possible for a second class of models for which is Gaussian [12], [9]. This can occur if the dynamics are nonlinear and the measurements linear. Such a system is given by (54) (55) where (56) (57) is a nonlinear function, is an and : and are mutually independent observation matrix, and and . Defining i.i.d. Gaussian sequences with (58) (59) one obtains
, is referred to as the where true weight. This cannot be evaluated exactly, but an estimate of can be obtained by (51)
is the normalized weight obtained using (47). Notice where , and small indicates severe degeneracy. that Clearly, the degeneracy problem is an undesirable effect in particle filters. The brute force approach to reducing its effect is to use a very large . This is often impractical; therefore, we rely on two other methods: a) good choice of importance density and b) use of resampling. These are described next. 2) Good Choice of Importance Density: The first method into minvolves choosing the importance density so that is maximized. The optimal imporimize Var tance density function that minimizes the variance of the true conditioned on and has been shown [12] weights to be
(60) and (61) For many other models, such analytic evaluations are not possible. However, it is possible to construct suboptimal approximations to the optimal importance density by using local linearization techniques [12]. Such linearizations use an importance density that is a Gaussian approximation to . Another approach is to estimate a Gaussian apusing the unscented transform proximation to [40]. The authors opinion is that the additional computational cost of using such an importance density is often more than offset by a reduction in the number of samples required to achieve a certain level of performance. Finally, it is often convenient to choose the importance density to be the prior (62) Substitution of (62) into (48) then yields
(52) Substitution of (52) into (48) yields
(53) This choice of importance density is optimal since for a given , takes the same value, whatever sample is drawn from , . Hence, conditional on , Var . resulting from different This is the variance of the different sampled . This optimal importance density suffers from two major drawbacks. It requires the ability to sample from and to evaluate the integral over the new state. In the general case, it may not be straightforward to do either of these things. There are two cases when use of the optimal importance density is possible. is a member of a finite set. In The first case is when such cases, the integral in (53) becomes a sum, and sampling is possible. An example of an application from is a member of a finite set is a JumpMarkov linear when system for tracking maneuvering targets [15], whereby the discrete modal state (defining the maneuver index) is tracked using a particle filter, and (conditioned on the maneuver index) the continuous base state is tracked using a Kalman filter.
(63) This would seem to be the most common choice of importance density since it is intuitive and simple to implement. However, there are a plethora of other densities that can be used, and as illustrated by Section VI, the choice is the crucial design step in the design of a particle filter. 3) Resampling: The second method by which the effects of degeneracy can be reduced is to use resampling whenever a sigfalls below nificant degeneracy is observed (i.e., when ). The basic idea of resampling is to elimisome threshold nate particles that have small weights and to concentrate on particles with large weights. The resampling step involves generby resampling (with replacement) ating a new set times from an approximate discrete representation of given by (64) . The resulting sample is in fact so that an i.i.d. sample from the discrete density (64); therefore, the
180
IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 50, NO. 2, FEBRUARY 2002
weights are now reset to . It is possible to impleoperations by samment this resampling procedure in ordered uniforms using an algorithm based on order pling statistics [6], [37]. Note that other efficient (in terms of reduced MC variation) resampling schemes, such as stratified sampling and residual sampling [28], may be applied as alternatives to this algorithm. Systematic resampling [25] is the scheme preferred by the authors [since it is simple to implement, takes time, and minimizes the MC variation], and its operation is deis the uniform distribution scribed in Algorithm 2, where (inclusive of the limits). For each resamon the interval , this resampling algorithm also stores the index pled particle of its parent, which is denoted by . This may appear unnecessary here (and is), but it proves useful in Section V-B2. A generic particle filter is then as described by Algorithm 3. Although the resampling step reduces the effects of the degeneracy problem, it introduces other practical problems. First, it limits the opportunity to parallelize since all the particles must be combined. Second, the particles that have high weights are statistically selected many times. This leads to a loss of diversity among the particles as the resultant sample will contain many repeated points. This problem, which is known as sample impoverishment, is severe in the case of small process noise. In fact, for the case of very small process noise, all particles will collapse to a single point within a few iterations.5 Third, since the diversity of the paths of the particles is reduced, any smoothed estimates based on the particles paths degenerate.6 Schemes exist to counteract this effect. One approach considers the states for the particles to be predetermined by the forward filter and then obtains the smoothed estimates by recalculating the particles weights via a recursion from the final to the first time step [16]. Another approach is to use MCMC [5].
Algorithm 3: Generic Particle Filter PF , FOR Draw Assign the particle a weight, , according to (48) END FOR Calculate total weight: SUM FOR Normalize: END FOR Calculate using (51) IF Resample using algorithm 2: RESAMPLE END IF
Algorithm 2: Resampling Algorithm , RESAMPLE Initialize the CDF: FOR Construct CDF: END FOR Start at the bottom of the CDF: Draw a starting point: FOR Move along the CDF: WHILE END WHILE Assign sample: Assign weight: Assign parent: END FOR
5If the process noise is zero, then using a particle filter is not entirely appropriate. Particle filtering is a method well suited to the estimation of dynamic states. If static states, which can be regarded as parameters, need to be estimated then alternative approaches are necessary [7], [27]. 6Since the particles actually represent paths through the state space, by storing the trajectory taken by each particle, fixed-lag and fixed-point smoothed estimates of the state can be obtained [4].
There have been some systematic techniques proposed recently to solve the problem of sample impoverishment. One such technique is the resample-move algorithm [19], which is not be described in detail in this paper. Although this technique draws conceptually on the same technologies of importance sampling-resampling and MCMC sampling, it avoids sample impoverishment. It does this in a rigorous manner that ensures the particles asymtotically approximate samples from the posterior and, therefore, is the method of choice of the authors. An alternative solution to the same problem is regularization [31], which is discussed in Section V-B3. This approach is frequently found to improve performance, despite a less rigorous derivation and is included here in preference to the resample-move algorithm since its use is so widespread. 4) Techniques for Circumventing the Use of a Suboptimal Importance Density: It is often the case that a good importance is density is not available. For example, if the prior used as the importance density and is a much broader distribu, then only a few particles will tion than the likelihood have a high weight. Methods exist for encouraging the particles to be in the right place; the use of bridging densities [8] and progressive correction [33] both introduce intermediate distributions between the prior and likelihood. The particles are then reweighted according to these intermediate distributions and resampled. This herds the particles into the right part of the state space. Another approach known as partitioned sampling [29] is useful if the likelihood is very peaked but can be factorized into a number of broader distributions. Typically, this occurs because each of the partitioned distributions are functions of some (not all) of the states. By treating each of these partitioned distributions in turn and resampling on the basis of each such partitioned distribution, the particles are again herded toward the peaked likelihood. B. Other Related Particle Filters The sequential importance sampling algorithm presented Section in V-A forms the basis for most particle filters that have
ARULAMPALAM et al.: TUTORIAL ON PARTICLE FILTERS
181
been developed so far. The various versions of particle filters proposed in the literature can be regarded as special cases of this general SIS algorithm. These special cases can be derived from the SIS algorithm by an appropriate choice of importance sampling density and/or modification of the resampling step. Below, we present three particle filters proposed in the literature and show how these may be derived from the SIS algorithm. The particle filters considered are i) sampling importance resampling (SIR) filter; ii) auxiliary sampling importance resampling (ASIR) filter; iii) regularized particle filter (RPF). 1) Sampling Importance Resampling Filter: The SIR filter proposed in [17] is an MC method that can be applied to recursive Bayesian filtering problems. The assumptions required to use the SIR filter are very weak. The state dynamics and meaand in (1) and (2), respecsurement functions tively, need to be known, and it is required to be able to sample and realizations from the process noise distribution of needs from the prior. Finally, the likelihood function to be available for pointwise evaluation (at least up to proportionality). The SIR algorithm can be easily derived from the SIS algorithm by an appropriate choice of i) the importance denis chosen to be the prior density sity, where , and ii) the resampling step, which is to be applied at every time index. The above choice of importance density implies that we need . A sample can be samples from generated by first generating a process noise sample and setting , , where is the . For this particular choice of importance density, it pdf of is evident that the weights are given by (65) However, noting that resampling is applied at every time index, ; therefore we have (66) The weights given by the proportionality in (66) are normalized before the resampling stage. An iteration of the algorithm is then described by Algorithm 4. As the importance sampling density for the SIR filter is independent of measurement , the state space is explored without any knowledge of the observations. Therefore, this filter can be inefficient and is sensitive to outliers. Furthermore, as resampling is applied at every iteration, this can result in rapid loss of diversity in particles. However, the SIR method does have the advantage that the importance weights are easily evaluated and that the importance density can be easily sampled. Algorithm 4: SIR Particle Filter SIR FOR Draw Calculate END FOR Calculate total weight: SUM FOR Normalize:
END FOR Resample using algorithm 2: RESAMPLE ,
2) Auxiliary Sampling Importance Resampling Filter: The ASIR filter was introduced by Pitt and Shephard [34] as a variant of the standard SIR filter. This filter can be derived from the SIS , framework by introducing an importance density , where refers to the index which samples the pair . of the particle at By applying Bayes rule, a proportionality can be derived for as
(67) The ASIR filter operates by obtaining a sample from the joint and then omitting the indices in the pair density to produce a sample from the marginalized . The importance density used to draw the density is defined to satisfy the proportionality sample (68) where is some characterization of be the mean, in which case, . By writing and defining (70) it follows from (68) that (71) is then assigned a weight proporThe sample tional to the ratio of the right-hand side of (67) to (68) (72) The algorithm then becomes that described by Algorithm 5. Algorithm 5: Auxiliary Particle Filter APF FOR Calculate Calculate . END FOR Calculate total weight: SUM FOR Normalize: END FOR Resample using algorithm 2: RESAMPLE FOR Draw , as in the SIR filter. using (72) Assign weight , given . This could or a sample (69)
182
IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 50, NO. 2, FEBRUARY 2002
END FOR Calculate total weight: FOR Normalize: END FOR
SUM
density and the corresponding regularized empirical representation in (73), which is defined as MISE (75)
Although unnecessary, the original ASIR filter as proposed in [34] consisted of one more step, namely, a resampling stage, with equal weights. to produce an i.i.d. sample Compared with the SIR filter, the advantage of the ASIR filter , is that it naturally generates points from the sample at which, conditioned on the current measurement, are most likely to be close to the true state. ASIR can be viewed as resampling that at the previous time step, based on some point estimates . If the process noise is small so that characterize is well characterized by , then ASIR is often not are more even. so sensitive to outliers as SIR, and the weights However, if the process noise is large, a single point does not well, and ASIR resamples based on a characterize . In such scenarios, the use poor approximation of of ASIR then degrades performance. 3) Regularized Particle Filter: Recall that resampling was suggested in Section V-B1 as a method to reduce the degeneracy problem, which is prevalent in particle filters. However, it was pointed out that resampling in turn introduced other problems and, in particular, the problem of loss of diversity among the particles. This arises due to the fact that in the resampling stage, samples are drawn from a discrete distribution rather than a continuous one. If this problem is not addressed properly, it may lead to particle collapse, which is a severe case of sample particles occupy the same point impoverishment where all in the state space, giving a poor representation of the posterior density. A modified particle filter known as the regularized particle filter (RPF) was proposed [31] as a potential solution to the above problem. The RPF is identical to the SIR filter, except for the resampling stage. The RPF resamples from a continuous approxima, whereas the SIR resamtion of the posterior density ples from the discrete approximation (64). Specifically, in the RPF, samples are drawn from the approximation (73) where (74) , is the Kernel bandis the rescaled Kernel density is the dimension of the state width (a scalar parameter), are normalized weights. The vector , and , Kernel density is a symmetric probability density function such that
denotes the approximation to given by where the right-hand side of (73).7 In the special case of all the samples having the same weight, the optimal choice of the kernel is the Epanechnikov kernel [31] (76) otherwise is the volume of the unit hypersphere in . Furwhere thermore, when the underlying density is Gaussian with a unit covariance matrix, the optimal choice for the bandwidth is [31] (77) (78) if
Algorithm 6: Regularized Particle Filter RPF FOR Draw Assign the particle a weight, , according to (48) END FOR Calculate total weight: SUM FOR Normalize: END FOR Calculate using (51) IF Calculate the empirical covariance of , matrix Compute such that . Resample using algorithm 2: RESAMPLE , FOR Draw from the Epanechnikov Kernel END FOR END IF
Alhough the results of (76) and (77) and (78) are optimal only in the special case of equally weighted particles and underlying Gaussian density, these results can still be used in the general case to obtain a suboptimal filter. One iteration of the RPF is described by Algorithm 6. The RPF only differs from the generic particle filter described by Algorithm 3 as a result of the addition of the regularization steps when conducting the resampling. Note also that the calculation of the empirical covariance matrix
7As observed by one of the anonymous reviewers, it is worth noting that the use of the Kernel approximation become increasingly less appropriate as the dimensionality of the state increases.
The Kernel and bandwidth are chosen to minimize the mean integrated square error (MISE) between the true posterior
ARULAMPALAM et al.: TUTORIAL ON PARTICLE FILTERS
183
TABLE I TABLE OF THE ALGORITHMS USED, THE SECTIONS OF THE ARTICLE, AND FIGURES THAT RELATE TO THE ALGORITHMS, AND RMSE VALUES (AVERAGED OVER 100 MC RUNS)
is carried out prior to the resampling and is therefore a funcand . This is done since the accuracy of tion of both the any estimate of a function of the distribution can only decrease as a result of the resampling. If quantities such as the mean and covariance of the samples are to be output, then these should be calculated prior to resampling. By following the above procedure, we generate an i.i.d. drawn from (73). random sample In terms of complexity, the RPF is comparable with SIR since additional generations from the kernel it only requires at each time step. The RPF has the theoretic disadvantage that the samples are no longer guaranteed to asymtotically approximate those from the posterior. In practical scenarios, the RPF performance is better than the SIR in cases where sample impoverishment is severe, for example, when the process noise is small. VI. EXAMPLE Here, we consider the following set of equations as an illustrative example: (79) (80) or equivalently (81) (82) where (83) and are zero mean Gaussian random and where and , respectively. We use variables with variances and . This example has been analyzed before in many publications [5], [17], [25]. We consider the performance of the algorithms detailed in Table I. In order to qualitatively gauge performance and discuss resulting issues, we consider one exemplar run. In order to quantify performance, we use the traditional measure of performance: the Root Mean Squared Error (RMSE). It should be noted that this measure of performance is not exceptionally meaningful for this multimodal problem. However, it has been used extensively in the literature and is included here for that reason and because it facilitates quantitative comparison. For reference, the true states for the exemplar run are shown in Fig. 1 and the measurements in Fig. 2.
Fig. 1. Figure of the true values of the state exemplar run.
x
as a function of k for the
Fig. 2. Figure of the measurements z of the states x shown in Fig. 1 for the same exemplar run.
The approximate grid-based method uses 50 states with cen. All the particle filters have 50 ters equally spaced on ). particles and employ resampling at every time step ( . The reguThe auxiliary particle filter uses larized particle filter uses the kernel and bandwidth described in Section V-B3. To visualize the densities inferred by the approximate gridbased and particle filters, the total probability mass at any time is shown as in each of 50 equally spaced regions on images in Figs. 59. At any given time (and in any vertical slice through the image), darker regions represent higher probability than lighter regions. A graduated scale relating intensity to probability mass in a pixel is shown next to each image. A. EKF The EKFs local linearization and Gaussian approximation are not a sufficient description of the nonlinear and non-Gaussian nature of the example. Once the EKF cannot adequately approximate the bimodal nature of the underlying
184
IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 50, NO. 2, FEBRUARY 2002
Fig. 3.
Evolution of the EKFs mean estimate of the state.
Fig. 5. Image representing evolution of probability density for approximate grid-based filter.
B. Approximate Grid-Based Filter This example is low dimensional, and therefore, one would expect that an approximate grid-based approach would perform well. Fig. 5 shows this is indeed the case. The grid-based approximation is able to model the multimodality of the problem. Using the approximate grid-based filter rather than an EKF yields a marked reduction in RMS errors. A particle filter with particles conducts operations per iteration, whereas operations an approximate grid-based filter carries out cells. It is therefore surprising that the RMS errors for with the approximate grid are larger than those of the particle filter. The authors suspect that this is an artifact of the grid being fixed; the resolution of the algorithm is predefined, and the fixed position of the grid points means that the grid points near 25 contribute significantly to the error when the true state is far from these values.
Fig. 4. Evolution of the upper and lower 2 positions of the state as estimated by the EKF (dotted) with the true state also shown (solid).
C. SIR Particle Filter Using the prior distribution as the importance density is in some sense regarded as a standard SIR particle filter and, therefore, is an appropriate particle filter algorithm with which to begin. As can be seen from Fig. 6, the SIR particle filter gives disappointing results with the low number of particles used here. The speckled appearance of the figure is a result of sampling a low number of particles from the (broad) prior. It is an artifact resulting from the inadequate amount of sampling. The RMSE metric shows a marginal improvement over the approximate grid-based filter. To achieve smaller errors, one could simply increase the number of particles, but here, we will now investigate the effect of using the alternative particle filter algorithms described up to this point. D. Auxiliary Particle Filter One way to reduce errors might be that the proposed particle positions are chosen badly. One might therefore think that choosing the proposed particles in a more intelligent manner would yield better results. An auxiliary particle filter would then
posterior, the Gaussian approximation failsthe EKF is prone to either choosing the wrong mode or just sitting on the average between the modes. As a result of this inability to adequately approximate the density, the linearization approximation becomes poor. This can be seen from Fig. 3. The mean of the filter is rarely close to the true state. Were the density to be Gaussian, one would expect the state to be within two standard deviations of the mean approximately 95% of the time. From Fig. 4, it is evident that there are times when the distribution is sufficiently broad to capture the true state in this region but that there are also times when the filter becomes highly overconfident of a biased estimate of the state. The implication of this is that it is very difficult to detect inconsistent EKF errors automatically online. The RMSE measure indicates that the EKF is the least accurate of the algorithms at approximating the posterior. The approximations made by the EKF are inappropriate in this example.
ARULAMPALAM et al.: TUTORIAL ON PARTICLE FILTERS
185
Fig. 6. filter.
Image representing evolution of probability density for SIR particle
Fig. 8. Image representing evolution of probability density for regularized particle filter.
E. Regularized Particle Filter Using the regularized particle filter results in a smoothing of the approximation to the posterior. This is apparent from Fig. 8. The speckle is reduced and the peaks broadened when compared with the previous particle filters images. The regularized particle filter gives very similar RMS errors to the SIR particle filter. The regularization does not result in a significant reduction in errors for this data set. F. Likelihood Particle Filter All the aforementioned particle filters share the prior as a proposal density. For this example, much of the time, the likelihood is far tighter than the prior. As a result, the posterior is closer in similarity to the likelihood than to the prior. The importance density is an approximation to the posterior. Therefore, using a better approximation based on the likelihood, rather than the prior, can be expected to improve performance. Fig. 9 shows that the use of such an importance density (see the Appendix for details) yields a reduction in speckle and that the peaks of the density are closer on average to the true state than for any of the other particle filters. The RMS errors are similar to those for the Auxiliary particle filter. G. Crucial Step in the Application of a Particle Filter The RMS errors indicate that in highly nonlinear environments, a nonlinear filter such as an approximate grid-based filter or particle filter offers an improvement in performance over an EKF. This improvement results from approximating the density rather than the models. When using a particle filter, one can often expect and frequently achieve an improvement in performance by using far more particles or alternatively by employing regularization or using an auxiliary particle filter. For this example, a slight improvement in RMS errors is possible by using an importance . The authors assert that an imdensity other than portance density tuned to a particular problem will yield an appropriate trade off between the number of particles and the com-
Fig. 7. Image representing evolution of probability density for auxiliary particle filter.
seem to be an appropriate candidate replacement algorithm for as a sample from . SIR. Here, we have As shown by Fig. 7, for this example, the auxiliary particle filter performs well. There is arguably less speckle in Fig. 7 than in Fig. 6, and the probability mass appears to be better concentrated around the true state. However, one might think this problem is not very well suited to an auxiliary particle filter since the prior is often much broader than the likelihood. When the prior is broad, those particles with a noise realization that happens to have a high likelihood are resampled many times. There is no guarantee that other samples from the prior will also lie in the same region of the state space since only a single point is being used to characterize the filtered density for each particle. The RMS errors are slightly reduced from those for SIR.
186
IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 50, NO. 2, FEBRUARY 2002
To keep the notation simple, throughout this Appendix, . For a uniform prior on , the density can be written by Bayes rule as otherwise. (84)
[samples are repeatWe can then sample until one is drawn such edly drawn from , i.e., one such that ]. Then, that can be chosen to be a pair of delta functions (85) This can then be used to form a Likelihood based imporconditional on and indepentance density that samples dently from
Fig. 9. Image representing evolution of probability density for likelihood particle filter.
(86) The weight of the sample can be calculated according to (47)
putational expense necessary for each particle, giving the best qualitative performance with affordable computational effort. The crucial point to convey is that all the refinements of the particle filter assume that the choice of importance density has already been made. Choosing the importance density to be well suited to a given application requires careful thought. The choice made is crucial. VII. CONCLUSIONS For a particular problem, if the assumptions of the Kalman filter or grid-based filters hold, th...