discrete-time kalman filter and the particle filter

7/31/2019 Discrete-Time Kalman Filter and the Particle Filter

http://slidepdf.com/reader/full/discrete-time-kalman-filter-and-the-particle-filter 1/12

Discrete-time Kalman Filter and the Particle Filter∗

Com S 477/577

Dec 7, 2010

1 Discrete-Time System

A discrete-time system is a process that transforms input discrete-time signals into output discrete-time signals. Simply stated, it takes an input sequence and produces an output sequence. Such asystem usually takes the form of

xk = F k−1xk−1 + Gk−1uk−1 + wk−1, (1)

where uk is a known input, and wk is white noise with zero mean and covariance Qk. We areinterested in how the mean of the state xk evolves with time.

From now on, we denote the mean of a random variable with an overbar ‘¯’. Take the expectedvalue of both sides of equation (1):

xk = E (xk) = F k−1xk−1 + Gk−1uk−1. (2)

How does the covariance P k = cov(xk) evolve with time? Subtraction of (2) from (1) yields

xk − xk = F k−1(xk−1 − xk−1) + wk−1.

The covariance is then computed as follows:

P k = E

(xk − xk)(xk − xk)T

= E

F k−1(xk−1 − xk−1) + wk−1

F k−1(xk−1 − xk−1) + wk−1

T

= E

F k−1(xk−1 − xk−1)(xk−1 − xk−1)T F T k−1 + wk−1wT k−1 +

F k−1(xk−1 − xk−1)wT k−1 + wk−1(xk−1 − xk−1)T F T k−1

.

Thatxk−

1

− ¯xk−

1

andw

k−1

are uncorrelated implies

E

(xk−1 − xk−1)wT k−1

= E (xk−1 − xk−1)E (wT

k−1) = 0,

E wk−1(xk−1 − xk−1)T

= 0.

∗The material is adapted from Chapters 4.1 and 5.1–5.3 in Dan Simon’s book Optimal State Estimation [1].

1



Two of the summands in the equation for P k vanish. The recursive covariance equation reduces to

P k = F k−1P k−1F T k−1 + Qk−1, (3)

where Qk−1 is the covariance of wk−1. The above is called a discrete-time Lyapunov equation or aStein equation .

Example 1. The following is a linear system that describes the population of a predator x1 and that of itsprey x2, where the second subscript of each item denotes the time step:

x1,k+1 = x1,k − 0.8x1,k + 0.4x2,k + w1,k,

x2,k+1 = x2,k − 0.4x1,k + uk + w2,k.

The first equation says that the predator population decreases due to overcrowding as well as the reductionin the prey population. The second equation says that the prey population decreases due to the predatorpopulation and increases on external food supply uk. Both populations are subject to random disturbances(with variances 1 and 2, respectively) due to environmental factors. We rewrite the system in the state spaceform as

xk+1 = 0.2 0.4

−0.4 1xk +0

1uk + wk,

wk ∼ (0, Q),

where Q = diag(1, 2).The mean and covariance of the populations evolve with time according to equations (2) and (3). Set

uk = 1 and the initial conditions as x0 = (10, 20)T and P 0 = diag(40, 40). The figure1 below depicts the twomeans and the two diagonal elements of the covariance matrix for the first few steps.

From the figure, we see that the mean and covariance eventually reach the steady-state values. Thesteady state value of the mean is the solution of the linear system after setting wk = 0:

x =

2.5

5

.

1Figure 4.1 on p. 110 in [1].

2



The steady-state value of P found by control system software2 is

P ≈

2.88 3.883.08 7.96

.

2 Four Estimates of a State

We would like to estimate the state, i.e., the values of all variables of a dynamic system as itevolves with time. Our method is to derive equations from the system to describe how the meanand covariance of its state x propagate with time. These equations form a new dynamic systemwhich is simulated on a computer. They are the basis for the derivation of the Kalman filter whoseestimate is the mean of the original system’s state x, and whose covariance equals the covarianceof x. Every time a new measurement is taken, the mean and covariance of x are updated, in asimilar way we recursively update the estimate of a constant before.

Consider a linear discrete-time system as follows:

xk = F k−1xk−1 + Gk−1uk−1 + wk−1, (4)

yk = H kxk + vk. (5)

Here, the evolutions of the state x and the output y are subject to some noise processes w andv, respectively. These two noise are white, zero-mean, and uncorrelated. Let Qk and Rk be theircovariance matrices, respectively. Then the noise characteristics are given as follows:

wk ∼ (0, Qk), (6)

vk ∼ (0, Rk), (7)

E (wkwT j ) =

Qk, if k = j;0, otherwise,

(8)

E (vkvT j ) =

Rk, if k = j;0, otherwise,

(9)

E (vkwT j ) = 0. (10)

The goal is to estimate the state xk based on the known system dynamics (4) and (5) and fromthe noisy measurements {yk}.

In estimating the state xk at time k, if we have all the measurements up to and including timek available, then we can form an a posteriori (or a posterior) estimate denoted as x+k . One way isto compute the expected value of xk as

x+k = E (xk | y1,y2, . . . ,yk). (11)

If we have only the measurements before time k available, then we can form a priori (or a prior)estimate denoted as x−

k . One way to form this latter estimate is to compute the expected value of xk as

x−k = E (xk | y1,y2, . . . ,yk−1). (12)

2For example, the MATLAB Control System Toolbox function DLYAP(F, Q).

3



Note that x−k and x+k are both estimates of xk. However, the former is our estimate before the

measurement yk is taken into account, while the latter is our estimate after the measurement yk

is taken into account. With more information, x+k is expected to be a better estimate than x−k .A smoothed estimate of xk is formed if we also have measurements after time k available for

use. One way of doing this is to compute the following expected value:

xk|k+n = E (xk | y1,y2, . . . ,yk+n), (13)

where n > 0 depends on the specific problem that is being solved. A predicted estimate is aprediction of xk one step or more ahead of the available measurements. This is often computed asthe expected value of xk based on the limited measurements that are available:

xk|k−m = E (xk | y1,y2, . . . ,yk−m), (14)

where m > 0 is problem specific.The figure below depicts the relationship between the posterior, a priori , smoothed, and pre-

dicted state estimates. Suppose we are making estimates based on all measurements taken at timek ≤ 5. An estimate of the state at k = 1 is a smoothed estimate. An estimate at k = 5 is the a

posteriori estimate. An estimate at k = 6 is the prior estimate. And an estimate of the state atk = 9 is a prediction.

x5+

a priori x6

−

x1/5

x9/5

5432 76 8 9 101

smoothedestimate

estimate

estimatea posteriori

time

prediction

Starting from now we denote x+0

as our initial estimate of x0 before the first measurement istaken (at time k = 1). Given no measurements available, we form x+

0as the expected value of the

initial estimate:x+0 = x0 = E (x0). (15)

We let P −k be the covariance of the estimation error of x−k , and P +k the covariance of the estimation

error of x+k , all at time k:

P −k = E (xk − x−k )(xk − x−

k )T ,

P +k = E

(xk − x+k )(xk − x+k )T

.

The next figure shows the relationships from time k − 1 to k. After we process the measurementat time k − 1, we have a posterior estimate x+k−1 and the covariance P +k−1. At time k, before

we process the new measurement we compute a prior estimate x−k and the covariance P −k of the

estimate based on the system dynamics. Then we process the measurement yk at time k to refine

4



our estimate of xk. The resulting estimate that takes into account the measurement at time k isx+k with covariance P +k .

Pk

−

Pk

+

+ xk xk + xk

Pk +

− xk

Pk

−

time

−

−1−1

−1 −1

k k −1

3 From a posteriori Estimate to a priori Estimate

The estimation process starts with x+0 , the best estimate of the initial state x0. It then computes

x−1 . Recall from (2) that the mean of x propagates with time, so we obtain

x−1 = F 0x

+0 + G0u0.

This equation generalizes to obtain the prior estimate at time k from the posterior estimate at theprevious time k − 1:

x−k = F k−1x

+

k−1 + Gk−1uk−1.

Namely, the state estimate propagates the same way that the mean of the state does. This updateis based on the system dynamics (1).

Next, we derive the equations for updating the covariance of the state estimation error fromtime k − 1 to time k but before the measurement at time k is considered. We begin with P +

0, the

covariance of our initial estimate x+0

. The covariance represents the uncertainty in x0:

P +0

= E

(x0 − x0)(x0 − x0)T

= E

(x0 − x+0

)(x0 − x+0

)T

. (16)

If the initial state x0 is perfectly known, then P +0 = 0; if it is completely unknown, then P +0 = ∞I .Since the covariance of the state propagates with time according to (3), we immediately obtain

P −1 = F 0P +0 F T 0 + Q0. (17)

The above update is generalized to time k:

P −k = F k−1P +k−1F T k−1 + Qk−1. (18)

4 From a priori Estimate to a posteriori Estimate

Now we need to update the state estimate at time k by taking into account the measurement yk

at time k. In other words, we compute x

+

k from x

−

k , which was obtained without knowledge of yk.Recall from the lecture on recursive least squares that the availability of yk changes the estimateas follows:

K k = P k−1H T k (H kP k−1H T k + Rk)−1,

xk = xk−1 + K k(yk −H kxk−1),

P k = (I −K kH k)P k−1,

5



where xk−1 and P k−1 are the estimate and its covariance before the measurement yk is processed(so they are essentially x−

k and P −k here), and xk and P k are the estimate and its covariance basedon yk (so they are x+k and P +k here). The correspondences are shown below:

xk−1 ↔ x−k

P k−1 ↔ P −kxk ↔ x+kP k ↔ P +k

Applying these replacements, we obtain the measurement update equations

K k = P −k H T k (H kP −k H T k + Rk)−1,

x+k = x−k + K k(yk − H kx

−k ),

P +k = (I −K kH k)P −k ,

5 Kalman Filter

Now we summarize all the equations into an algorithm referred to as the discrete-time Kalman filter . The system dynamics are given by (4) and (5), with noise statistics (8)–(10).

1. Initialize the Kalman filter according to (15) and (16), namely,

x+0 = E (x0) = x0,

P +0 = E

(x− x0)(x0 − x0)T

.

2. Iterate the following sequentially at each time step k = 1, 2, . . .:

P −k = F k−1P +k−1F T k−1 + Qk−1, (19)

K k

= P −

kH T

k(H

kP −

kH T

k+ R

k)−1, (20)

x−k = F k−1x

+

k−1 + Gk−1uk−1, prior state estimate (21)

x+k = x−k + K k(yk − H kx

−k ), p osterior state estimate (22)

P +k = (I −K kH k)P −k (I −K kH k)T + K kRkK T k (23)

= (I −K kH k)P −k . (24)

The first expression for P +k guarantees to be symmetric positive definite, as long as P −k is so, whereasthe second expression is simpler though it does not guarantee symmetry or positive definiteness.

When xk is a constant, then F k = I , Qk = 0, and uk = 0 for all k. So we have

P −k = P +k−1,

x−k = x+k−1,

x+k = x+k−1 + K k(yk − H kx−k ),

P +k = (I − K kH k)P +k−1.

The Kalman filter consists of the last two equations above and (20). It reduces to the recursiveleast squares algorithm for estimating a constant vector.

6



Note that the calculation of P −k , K k, and P +k does not depend on the measurement yk. Itdepends on the system parameters F k, H k, Qk, and Rk. For efficiency, we can precalculate theKalman gain K k up to large enough k and store the data beforehand. When the dynamical systemstarts operating, only x− and x+ need to be computed in real time. This also means that theperformance of the filter can be investigated and evaluated before it is actually run. The reason is

that the accuracy of the filter is indicated by P k, which does not depend on the measurements.Example 2. Consider a noise-free Newtonian system involving position r, velocity v, and constant acceler-ation a. The system equation is

r

v

a

=

0 1 0

0 0 10 0 0

r

v

a

,

Writing x = (r,v,a)T , the system becomesx = Ax.

It has a solutionx = eAtx0,

where

eAt =

∞j=0

(At)j

j!.

Discretize the system with a sample time interval of h:

xk+1 = F xk,

where

F = eAh

= I + Ah +(Ah)2

2!+ · · ·

=

1 h h2/20 1 h0 0 1

.

There is no state noise or output noise. Hence Qk = 0 and Rk = 0 for all k. Also, there is no input soGk = 0. The Kalman filter for the system follows from equations (19) and (21):

P −k = F P +k−1F T .

x−

k = F x+k−1, (25)

The equations (20), (22), (23), and (24) are not needed since we do not make any measurements betweentime (k−1)+ and time k−. For the same reason, we see that the covariance of the estimation error increasesbetween time (k − 1)+ and time k−, as indicated in the first equation above.

Now suppose that we measure the position with a variance of σ2:

yk = H kxk + vk

= (1 0 0)xk + vk,

vk ∼ (0, Rk),

Rk = σ2.

7



We denote by P −k,ij the (i, j)-th entry of the prior covariance. Substituting H k = (1, 0, 0) and Rk = σ2 intothe above equation, we obtain the Kalman gain from (20):

K k =1

P −k,11 + σ2

P −k,11P −k,12P −k,13

.

Meanwhile, the posterior covariance follows from (24) as

P +k = P −k −K kH kP −k . (26)

We obtain its traces:

tr(P +k ) = tr(P −k ) −(P −k,11)2 + (P −k,22)2 + (P −k,33)2

P −k,11 + σ2.

The above equation shows that the covariance decreases when we get a new measurement. Consequently,our state estimate improves.

The figure below3 show the variance of the position estimate (P −k,11 and P +k,11) for the first five time stepsof the Kalman filter. We see that the uncertainty increases from one step to the next, but then decreases asa new measurement is taken into account.

The second figure shows the variance evolution when the Kalman filter runs for the first 60 time steps.It can be seen that, despite the fluctuations before and after each new measurement, the variance convergesto a steady-state value.

3This and the next figures appear as Figures 5.3 and 5.4 on p. 134 of [1].

8



The next figure4 shows the position-measurement error (with a standard deviation of 30) and the error of the posterior estimate x+. The estimate error starts out with a standard deviation close to 30, and decreases

to about 11 by the end of the simulation. The standard deviations are obtained from the covariances.

6 One-step Kalman Filter

This section combines the a priori and a posteriori Kalman filter equations into one. At time k + 1,the prior state estimate is, by (21),

x−k+1 = F kx

+

k + Gkuk.

Now, we substitute (22) into the above and obtain

x−k+1 = F k

x−k + K k(yk −H kx

−k )

+ Gkuk

= F k(I −K kH k)x−k + F kK kyk + Gkuk. (27)

4Figure 5.5 on p. 135 of [1].

9



Similarly, we can obtain a one-step expression for the prior covariance. First, increase the indexin (19) by one:

P −k+1 = F kP +k F T k + Qk.

Next, substitute (24) into the above:

P −k+1 = F k(P

−k −K kH kP

−k )F

T k + Qk

= F kP −k F T k − F kK kH kP −k F T k + Qk. (28)

The Kalman filter at each iteration step sequentially executes (20), (27), and (28).Similarly, we can derive one-step expressions for the a posterior state estimate and covariance

as follows:

x+k = (I −K kH k)(F k−1x+

k−1 + Gk−1uk−1) + K kyk, (29)

P +k = (I −K kH k)(F k−1P +k−1F T k−1 + Qk−1). (30)

The Kalman filter sequentially evaluates (20), (29), and (30) at each iteration step.

7 The Particle Filter

The particle filter is a statistical, brute-force approach to estimation that often works well onproblems that are difficult for the conventional Kalman filter. These problems typically involvesystems that are highly nonlinear. For nonlinear systems, the extended Kalman filter (EKF) isthe most widely applied state estimation technique. When the nonlinearities are high, though,the EKF often gives unreliable estimates since it employs linearization to propagate the mean andcovariance of the state estimate.

The particle filter is a nonlinear state estimator that trades computational effort for the degreeof nonlinearity. A probability-based estimator, it has proven very effective in machine learning,

mapping, and learning robots [2].At the beginning of estimation, we randomly generate a number of state vectors based on the

probability distribution of the initial state (which is assumed to be known). These state vectorsare called particles. At each time step, we propagate the particles to the next time step using theprocess equation. Compute the likelihood of each particle based on the measurement taken at thestep. Then generate a set of new particles based on the computed relative likelihoods. Iterate fora number of steps. Output the mean and covariance of the particles at the last step.

More formally, suppose the nonlinear system and measurement equations are given as followsfor the time step k:

xk+1 = f k(xk,wk),

yk

= hk

(xk

,vk

).

where wk and vk are the process noise and measurement noise, respectively, with known probabilitydensity functions (pdf’s). The functions f k and hk vary with time (i.e., the time step). The pdf p(x0) of the initial state is assumed to be known.

The particle filtering algorithm executes the following steps:

10



1. Randomly generate n initial particles based on p(x0), and denote them as x+0,i (i = 1, . . . , n).

The parameter n is chosen as a trade-off between computational effort and estimation accu-racy.

2. Repeat the following for k = 1, 2, . . . ,.

Propagation For each particle x+

k−1,i perform the time propagation step to obtain a priori particles x−

k,i, based on the system dynamics:

x−k,i = f k−1(x+k−1,i,w

ik−1), for i = 1, . . . , n ,

where each noise vector wik−1 is randomly generated based on the known pdf of wk−1.

Likelihood determination Compute the relative likelihood qi of each particle xk,i, condi-tioned on the measurement yk. This is done by evaluating the pdf p(yk | x

−k,i), since we

know the pdf of the measurement noise. For instance, if an m-dimensional measurementis given as

yk = h(xk) + vk

where vk ∼ N (0, R). Then the relative likelihood of a specific measurement y∗ can becomputed as follows:

qi = Pr(yk = y∗ | x = x−k,i)

= Pr(vk = y∗ − h(x−k,i))

∼1

(2π)m/2|R|1/2exp

−(y∗ − h(x−

k,i))T R−1(y∗ − h(x−k,i))

2

. (31)

The ∼ symbol means that the probability is proportional to the right hand side. Weuse it to compare one relative likelihood with another because all are scaled by the same

factor.Normalization Scale the relative likelihoods obtained in the previous step, for example,

(31), as follows

qi =qin

j=1 q j.

After the scaling, all the likelihoods add up to one.

Resampling Generate n posterior particles x+k,i based on the relative likelihoods qi.

In each iteration step, we have a set of particles x+k,i distributed according to the pdf p(xk | yk).We can compute the mean and covariance.

Example 3. We consider a benchmark problem in nonlinear estimation where the system is given as follows:

xk =1

2xk−1 +

25xk−1

1 + x2k−1+ 8 cos(1.2(k − 1)) + wk,

yk =1

20x2k + vk,

where {wk} and {vk} are Gaussian white noise sequences with zero means, and unit variances.

11



The high degree of nonlinearity in the process and the measurement makes the problem difficult for aKalman filter. We take the initial state x0 = 0.1 and initial estimate x0 = x0, and the initial estimationcovariance for the Kalman filter as P +0 = 2. Simulate the extended Kalman filter (EKF) [1] and the particlefilter over 50 time steps (using 100 particles).

The figure5 below compares the EKF and particle filter estimates. Not only is the EKF estimate poor,but also the EKF thinks (based on the computed covariance) that the estimate is better than it really is. In

comparison, the particle filter does a better job. The estimation errors6

for the Kalman and particle filtersare 16.3 and 2.6, respectively.

References

[1] D. Simon. Optimal State Estimations. John Wiley & Sons, Inc., Hoboken, New Jersey, 2006.

[2] S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. The MIT Press, Boston, MA, 2005.

5Figure 15.3 on p. 470 of [1]6measured as the roots of the mean squares.

12

discrete-time kalman filter and the particle filter

Documents