Recursive state estimation of nonlinear systemswith applications to integrated navigation

Per-Johan Nordlund

Department of Electrical Engineering

Linkoping University, SE-581 83 Linkoping, Sweden

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

Email: perno@isy.liu.se

November 29, 2000

REGLERTEKNIK

AUTOMATIC CONTROL

LINKÖPING

Report no.: LiTH-ISY-R-2321

Technical reports from the Automatic Control group in Linkoping are availableby anonymous ftp at the address ftp.control.isy.liu.se. This report iscontained in the file 2321.pdf.

1

2

Recursive state estimation of nonlinear systems

with applications to integrated navigation

Per-Johan NordlundDepartment of Electrical Engineering

Linkoping UniversitySE-58183 Linkoping, Sweden

perno@isy.liu.se

November 29, 2000

Abstract

Today, accurate navigation systems often consists of several separatenavigation systems which are integrated together to provide a more accu-rate and reliable navigation solution. Integrated navigation is a nonlinearand non-Gaussian system integration problem. Sequential Monte Carlomethods (particle filters) handle recursive state estimation of arbitrarysystems. A direct application of the particle filter shows that a vast num-ber of particles is needed for the filter to work well.

Another method is developed in this report using the typical structureof an integrated navigation system. This method takes advantage of theparts of the state equations that are linear and Gaussian and estimatesthese parts using the Kalman filter. The nonlinear and non-Gaussianparts are estimated using the particle filter. Based on Bayes’ rule thesolutions from the two filters are fused together. This two-filter approachis applied to the same system and the result shows that the number ofparticles needed to achieve the same performance as the particle filter ismuch less.

1

2

Contents

1 Introduction 5

2 Problem formulation 5

3 Particle filtering 7

4 Combined particle and Kalman filtering 10

5 Simulations 13

6 Conclusions 16

List of Figures

1 An example of an integrated navigation system. . . . . . . . . . . 62 The number of particles found in the marked area divided by the

total number of particles represents the discrete approximationP (dx) of P (x ∈ dx) ≈ p(x)dx. . . . . . . . . . . . . . . . . . . . . 8

3 The terrain profile used by TAN during the simulations. . . . . . 154 RMSE of xMMS

t|t and PMMSt|t provided by the particle filter based

on 100 Monte Carlo simulations (2 runs removed because of di-vergence). Number of particles = 30000. . . . . . . . . . . . . . . 19

5 RMSE of xMMSt|t and PMMS

t|t provided by the combined particle andKalman filtering method based on 100 Monte Carlo simulations.Number of particles = 1000. . . . . . . . . . . . . . . . . . . . . . 20

List of Tables

1 The particle filter algorithm. . . . . . . . . . . . . . . . . . . . . 102 The combined Kalman and particle filter algorithm. . . . . . . . 143 Percentage of diverged runs and mean RMSE of the estimated

states for the particle filter. . . . . . . . . . . . . . . . . . . . . . 154 Percentage of diverged runs and mean RMSE of the estimated

states for the combined Kalman and particle filter. . . . . . . . . 16

3

4

1 Introduction

So far, system integration has been based on the extended Kalman filter (EKF)[1]. Unfortunately, due to nonlinearities and non-Gaussian noise, the EKF of-ten provides a solution that is far from optimal. Another possible techniquefor integration is to use sequential Monte Carlo methods, or particle filters [9],[7]. Particle filters provide a numerical solution to the general nonlinear and/ornon-Gaussian filtering problem, where linearisations and/or Gaussian approxi-mations are intractable or would yield too low performance.

An example of nonlinear and non-Gaussian system integration is integratednavigation. The primary source of navigational data is often the Inertial Nav-igation System (INS). The quantities calculated by the INS are the vehicle’sattitude, heading, velocity, position and altitude. In integrated navigation, oneusually tries to estimate the errors in the quantities calculated by the INS. Themain supporting component for estimating the INS errors is terrain navigation,which is highly nonlinear and non-Gaussian. In case of a failure in the sup-porting component one still wants a solution which is as accurate as possible(graceful degradation). This means that one needs to estimate the errors ofthe whole chain of quantities calculated by the INS, for the INS to be able toprovide accurate stand-alone data. Until now, this problem has been tackled ina suboptimal manner using the EKF.

Unfortunately, the dimension of the state vector representing the INS errorsis quite high (≥ 9). Applying the particle filter directly on this high dimensionalproblem would require too many particles. One way of dealing with this is tosolve as much as possible analytically, taking advantage of the parts of thesystem that are linear and Gaussian. The remaining part of reduced dimensioncan then be solved using particle filtering.

The main purpose of this report is to propose a modified particle filter thatrequires much less particles but at the same time retains the accuracy. Thisfilter is then applied to a simplified integrated navigation system.

2 Problem formulation

The primary source of navigational data is often an INS (Inertial NavigationSystem), due to its good dynamical properties and high reliability. Based onmeasurements of acceleration and angular velocity in three dimensions, the INScalculates the vehicle’s attitude, heading, velocity, position and altitude. Thisis a dead-reckoning system which utilizes an accurate nonlinear motion model.However, due to sensor errors the errors in the calculated quantities grow withtime.

In integrated navigation one tries to obtain better accuracy usually by esti-mating the INS errors and compensate for them. The estimation is performedusing different supporting systems like the Global Positioning System (GPS)and Terrain Aided Navigation (TAN).

GPS is not further mentioned in this report, but for an example of inte-gration of INS and GPS see [12]. Terrain Aided Navigation is used during thesimulations described in Section 5, and a short description of it is thereforejustified. Based on measurements of height over the terrain and altitude overmean sea-level TAN obtains a measured terrain elevation. The height over the

5

terrain is provided by the Radar Altimeter (RA) and the altitude over meansea-level comes from the Air Data Computer (ADC). A Geographical Informa-tion System (GIS) stores the terrain elevation over a certain area as a functionof the position, i.e. TAN obtains a corresponding terrain elevation from GIS,where the position comes from the INS. If no errors were present the two ter-rain elevations would be exactly the same. There are of course errors present,particularly an INS position error, but given the terrain elevation informationit is possible to estimate a probability density function (pdf) for the position.For more information on terrain navigation see e.g. [4].

A graphical representation of how an integrated navigation system couldlook like is shown in Figure 1.

INS+

-

TAN

FUSION FILTER

GISRA

ADC

GPS

INS error model

Other sensor error models

Figure 1: An example of an integrated navigation system.

The aim in this report is not to provide a solution to the entire integratednavigation problem, but to propose a method on how to integrate a more orless linear and Gaussian motion model with a nonlinear and/or non-Gaussiancomponent such as TAN. The proposed method is compared with a standardparticle filter.

Consider the triple integrator

x1 = x2

x2 = x3 (1)x3 = v,

where x1, x2 and x3 represent the position, velocity and acceleration error inone dimension and v is a time continuous white noise. The discrete time versionof this is (see e.g. [10] for details on discretization)

xt+1 =

1 T T 2

20 1 T0 0 1

xt +

T 3

6T 2

2T

wt, (2)

6

where T is the sampling period. The system (2) can be looked upon as a verysimple INS error model in one dimension. For more information on inertialnavigation theory see e.g. [13]. The question here is how to estimate xt in anoptimal manner using nonlinear and/or non-Gaussian measurements accordingto

yt = h(x1t ) + et, (3)

where h is a known nonlinear function and et a white noise sequence with aknown but arbitrary pdf.

The system according to (2) and (3) is used during the simulations describedin Section 5. The purpose is to compare the particle filter, described in Section3, with the filter developed in Section 4.

3 Particle filtering

Consider the state space model

xt+1 = f(xt) + wt,

yt = h(xt) + et,(4)

where the process and measurement noises, wt ∈ Rnw and et ∈ Rne respectively,are assumed independent, and their probability density functions, p(wt) =p(xt|xt−1) and p(et) = p(yt|xt), are assumed known for every time t. Fur-thermore, the state of the system, xt ∈ Rnx , is considered to be a Markovprocess,

p(x0:t) = p(x0)t∏

k=1

p(xk|xk−1), (5)

where the prior distribution for the state at t = 0 is given by p(x0). Theobservations, yt ∈ Rny , are conditionally independent given the states,

p(y0:t|x0:t) =t∏

k=0

p(yk|xk). (6)

In the linear and Gaussian case the posterior filtering density p(xt|y0:t) isGaussian and therefore totally described by its expected value and covariance.In the general case this is not true, which means that one has to calculate thewhole pdf. The aim here is to calculate the filtering pdf p(xt|y0:t), one of themarginals to the total posterior density p(x0:t|y0:t), recursively as more and moreobservations become available. Using Bayes’ rule, p(x0:t|y0:t) can be computedrecursively according to

p(x0:t|y0:t) =p(yt|xt)p(xt|xt−1)

p(yt|y0:t−1)p(x0:t−1|y0:t−1). (7)

Unfortunately, it is very seldom a closed-form expression exists for p(xt|y0:t),not to mention p(x0:t|y0:t). One of these rare cases is the linear and Gaussiancase. This implies that some kind of approximation is necessary. The principle

7

for Monte Carlo methods is to approximate the posterior pdf by a discrete ver-sion utilizing a large number of samples. The expression for this approximationis

P (dx0:t|y0:t) =1N

N∑i=1

δxi0:t

(dx0:t), (8)

where δ(x) is the Dirac delta function. The set of the random samples, {xi0:t}N

i=1,are drawn randomly from the posterior pdf p(x0:t|y0:t). A large concentrationof samples in a particular area corresponds to a pdf having a large support inthat area. The one-dimensional case of (8) is depicted in Figure 2. From this

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

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

��������

��������

��������

��������

��������

��������

��������

��������

��������

��������

xdx

p(x)

Figure 2: The number of particles found in the marked area divided by thetotal number of particles represents the discrete approximation P (dx) of P (x ∈dx) ≈ p(x)dx.

set of samples one can easily estimate any moment according to

E[g(x0:t)] =∫

g(x0:t)P (dx0:t|y0:t) =1N

N∑i=1

g(xi0:t). (9)

Based on the law of large numbers one can show

E[g(x0:t)]a.s.−→ E[g(x0:t)] =

∫g(x0:t)p(x0:t|y0:t)dx0:t (10)

as N → ∞ (a.s. stands for almost surely convergence). For information onalmost surely convergence and probability in general see e.g. [11].

Here, what we are trying to estimate is the posterior pdf p(x0:t|y0:t). In otherwords, we do not have access to an analytical expression of this pdf which wecan sample from. To overcome this problem one uses a known pdf, q(x0:t|y0:t),written recursively using Bayes’ rule as

q(x0:t|y0:t) = q(xt|x0:t−1, y0:t)q(x0:t−1|y0:t−1). (11)

Note that on the right hand side of (11) we write q(x0:t−1|y0:t−1) instead ofq(x0:t−1|y0:t). This is a restriction on q(x0:t|y0:t) saying that the state at timet − 1 and backwards is independent of measurements at time t and onwards.This restriction allows us to draw {xi

t}Ni=1 from q(xt|x0:t−1, y0:t) and then just

add this new set of samples to the previous set, {xi0:t}N

i=1 = {xi0:t−1, x

it}N

i=1,without adjusting {xi

0:t−1}Ni=1.

8

Together with the so called importance weights,

wt =p(x0:t|y0:t)q(x0:t|y0:t)

=p(yt|xt)p(xt|xt−1)q(xt|x0:t−1, y0:t)

wt−1, (12)

we obtain the posterior pdf approximation

P (dx0:t|y0:t) =

∑Ni=1 wi

tδxi0:t

(dx0:t)∑Ni=1 wi

t

. (13)

This gives, using (9), that any moment can be approximated by

E[g(x0:t)] =∑N

i=1 witg(xi

0:t)∑Ni=1 wi

t

. (14)

Again, the law of large numbers states that

E[g(x0:t)]a.s.−→ E[g(x0:t)] =

Eq(x0:t|y0:t)[wtg(x0:t)]Eq(x0:t|y0:t)[wt]

(15)

as N →∞.A simple choice of a pdf to draw from is to use the prior, i.e.

q(xt|x0:t−1, y0:t) = p(xt|xt−1). (16)

The corresponding weights will then be updated, using (12), according to

wt = p(yt|xt)wt−1. (17)

To avoid degeneracy of the filter, i.e. most of the weights tends to zeroand therefore making the approximation of the posterior pdf worse, one hasto resample among the particles at regular intervals. See also [14] for moreaspects on resampling. On the other hand, to make the algorithm less sensitiveto outliers resampling should not be performed too often.

The minimum mean square (MMS) estimate of xt and its covariance areestimated, using (14), according to

xMMSt|t = Ep(xt|y0:t)[xt]

≈N∑

i=1

witx

it,

(18)

and

PMMSt|t = Ep(xt|y0:t)[(xt − Ep(xt|y0:t)[xt])2]

≈N∑

i=1

wit(x

it − xMMS

t|t )(xit − xMMS

t|t )T .(19)

In (18) and (19)

wit =

wit∑N

i=1 wit

, (20)

where wit is calculated according to (12).

The algorithm is summarized in Table 1. For more information on sequentialMonte Carlo methods see e.g. [3] and [7].

9

1. Generate {xi0}N

i=1 from p(x0), and set wit = 1

N , i = 1, .., N

2. Update the weights and normalize:

wit = p(yt|xi

t)wit−1∑ N

j=1 p(yt|xjt)w

jt−1

, i = 1, .., N

3. At every k:th iteration resample with replacement:{x1,j

t }Nj=1 ∈ {x1,i

t }Ni=1, where p(x1,j

t = x1,it ) = wi

t.wi

t = 1N , i = 1, .., N

4. Predict the particles:{xi

t+1}Ni=1 ∈ p(xi

t+1|xit)

Table 1: The particle filter algorithm.

4 Combined particle and Kalman filtering

A special case of (4) is when the system can be partitioned into a linear and anonlinear part according to

x1t+1 = f(x1

t ) + A1t x

2t + B1

t w1t

x2t+1 = A2

t x2t + B2

t w2t (21)

yt = h(x1t ) + et.

(Note that the system defined by (1) and (2) is a special case of (21) where f isa linear function.)

Assume that the properties which applies to (4) also applies to (21). Fur-thermore, the pdf for the process noise is Gaussian with zero mean and knownvariance, i.e

wt =(

w1t

w2t

)∈ N (0,

(Q1

t St

STt Q2

t

)). (22)

Also, the pdf for x20 is Gaussian, N(x2

0|−1, P20|−1). The pdfs for et and x1

0 areknown but arbitrary.

The aim here is, as for the general case in (4), to compute the filteringposterior pdf

p(xt|y0:t) = p(x1t , x

2t |y0:t). (23)

To do this consider instead the pdf p(x10:t, x

2t |y0:t). Using Bayes’ rule this pdf

can be factorized into two parts according to

p(x10:t, x

2t |y0:t) = p(x2

t |x10:t, y0:t)p(x1

0:t|y0:t). (24)

The first pdf on the right hand side of (24) can be rewritten as

p(x2t |x1

0:t, y0:t) = p(x2t |x1

0:t), (25)

10

due to the fact that the measurement, yt, is conditionally independent given x1t .

Consider now the system

zt = A1t x

2t + B1

t w1t

x2t+1 = A2

t x2t + B2

t w2t ,

(26)

where zt = x1t+1 − f(x1

t ). The system (26) is linear and Gaussian, i.e. theoptimal solution is provided by the Kalman filter. While z0:t provides the sameinformation as x1

0:t+1 as far as x2t concerns, the pdf in (25) is Gaussian

x2t |x1

0:t ∈ N (x2t|t−1, P

2t|t−1), (27)

where the Kalman filter estimates x2t|t−1 and P 2

t|t−1 are given by the Kalmanfilter equations (assuming uncorrelated w1

t and w2t )

Kt = P 2t|t−1(A

1t )

T (A1t P

2t|t−1(A

1t )

T + B1t Q1

t (B1t )T )−1

x2t+1|t = A2

t (x2t|t−1 + Kt(zt −A1

t x2t|t−1)) (28)

P 2t+1|t = A2

t (P2t|t−1 −KtA

1t P

2t|t−1)(A

2t )

T + B2t Q2

t (B2t )T .

The second pdf on the right hand side of (24) can be rewritten recursively(using Bayes’ rule iteratively) according to

p(x10:t|y0:t) =

p(yt, x10:t|y0:t−1)

p(yt|y0:t−1)

=p(yt|

x1t︷ ︸︸ ︷

x10:t, y0:t−1)p(x1

0:t|y0:t−1)p(yt|y0:t−1)

=p(yt|x1

t )p(x1t |

x10:t−1︷ ︸︸ ︷

x10:t−1, y0:t−1)

p(yt|y0:t−1)p(x1

0:t−1|y0:t−1)

=p(yt|x1

t )p(x1t |x1

0:t−1)p(yt|y0:t−1)

p(x10:t−1|y0:t−1).

(29)

One can apply particle filtering to this part. Let the importance weights berepresented by the likelihood, p(yt|x1

t ). The particles will then be sampledaccording to p(x1

t |x10:t−1). Using the state equation for x1

t according to (21) weget that

x1t+1|x1

0:t = A1t x

2t |x1

0:t + f(x1t ) + B1

t w1t , (30)

due to the fact that x1t is given and w1

t is independent of x10:t. This gives that

p(x1t |x1

0:t−1) is also Gaussian,

x1t |x1

0:t−1 ∈ N (f(x1t−1) + A1

t x2t−1|t−2, A

1t P

2t−1|t−2(A

1t )

T + B1t Q1

t (B1t )T ). (31)

To deal with correlated process noise, St 6= 0 in (22), the state equationsaccording to (21) has to be adjusted slightly. Replace w2

t with

w2t = w2

t − STt (Q1

t )−1w1

t . (32)

11

This new process noise is uncorrelated with w1t ,

E[(

w1t

w2t

) (w1

t

w2t

)T

] =(

Q1t 0

0 Q2t − ST

t (Q1t )−1St

). (33)

In (21), the new state equation for x2t will be

x2t+1 = (A2

t − CtA1t )x

2t + B2

t w2t + Ct(x1

t+1 − f(x1t )), (34)

where

Ct = B2t ST

t (Q1t )−1((B1

t )T B1t )−1(B1

t )T . (35)

Note that in our case, i.e. with a system defined by (2), the two process noisesare the same (w1

t = w2t ). This gives the expressions for w2

t and Ct using (32)and (35)

w2t = 0 (36)

Ct = B2t ((B1

t )T B1t )−1(B1

t )T . (37)

For a more detailed description of how to deal with correlated noise see e.g. [10].The MMS estimate of x1

t and its covariance are calculated according to (18)and (19) respectively. The MMS estimate of x2

t is given by

x2,MMSt|t = Ep(x2

t |y0:t)[x2t ]

=∫

x2t p(x2

t |y0:t)dx2t

=∫

x2t (

∫p(x1

0:t, x2t |y0:t)dx1

0:t)dx2t

=∫

p(x10:t|y0:t)(

∫x2

t p(x2t |x1

0:t)dx2t )dx1

0:t

=∫

Ep(x2t |x1

0:t)[x2

t ]p(x10:t|y0:t)dx1

0:t

≈N∑

i=1

witEp(x2

t |x1,i0:t)

[x2t ].

(38)

The corresponding estimate of the covariance is given by

P 2,MMSt|t = Ep(x2

t |y0:t)[(x2t − Ep(x2

t |y0:t)[x2t ])

2]

≈N∑

i=1

witEp(x2

t |x1,i0:t)

[(x2t −

N∑i=1

witEp(x2

t |x1,i0:t)

[x2t ])

2].(39)

Using the quantities given by the Kalman filters

x2,it|t−1 = Ep(x2

t |x1,i0:t)

[x2t ], (40)

P 2,it|t−1 = Ep(x2

t |x1,i0:t)

[(x2t − Ep(x2

t |x1,i0:t)

[x2t ])

2], (41)

12

(38) and (39) can be rewritten according to

x2,MMSt|t ≈

N∑i=1

witx

2,it|t−1

P 2,MMSt|t ≈

N∑i=1

wit(P

2,it|t−1 + (x2,i

t|t−1 − x2,MMSt|t−1 )2)

= P 2t|t−1 +

N∑i=1

wit(x

2,it|t−1 − x2,MMS

t|t−1 )2

(42)

where wit is calculated according to (12).

The algorithm is summarized in Table 2. Note that the covariances P 2,it|t−1

provided by the Kalman filters are all the same (which is used in the last stepin (42) as well). Therefore it is sufficient to update P 2,i

t|t−1 = P 2t|t−1 once for each

iteration.The feature to partition the state vector similar to (24) is sometimes referred

to as Rao-Blackwellisation, and for other applications see e.g. [8], [5], [2] and[6].

5 Simulations

The system according to (2) has been simulated with wt ∈ N (0, 0.00012). Themodel of the system used during the simulations has been slightly differentcompared to the true system. The reason for this was to be able to deal withdivergence problems otherwise encountered with the particle filter. More specif-ically the model was set to

xt+1 =

1 T T 2

20 1 T0 0 1

xt +

T T 3

6

0 T 2

20 T

wt (43)

with T = 1 and

wt ∈ N (0,

(22 00 0.00012

)), (44)

i.e. an additional noise component enters the position error state. This isnecessary for the particle filter not to diverge without using to many particles.A more detailed explanation of the problems and how to handle them is foundbelow.

The initial value and initial covariance of the estimated state vector were setto

x0|−1 =

0

00

, P0|−1 =

5002 0 0

0 22 00 0 0.022

(45)

The true initial value of the state vector was drawn randomly from N (0, P0|−1).Terrain Aided Navigation (TAN) was used as the supporting system, with

a terrain profile depicted in figure 3. Adding the simulated errors to a constant

13

1. Generate {x1,i0 }N

i=1 from p(x10).

Initialize the Kalman filters by {x2,i0|−1}N

i=1 and P 20|−1.

2. Compute the weights and normalize:wi

t = p(yt|xit)w

it∑ N

j=1 p(yt|xjt)w

jt

, i = 1, .., N

3. At every k:th iteration resample with replacement:{x1,j

t }Nj=1 ∈ {x1,i

t }Ni=1, where p(x1,j

t = x1,it ) = wi

t.

4. Predict the particles:{x1,i

t+1}Ni=1 ∈ N (f(x1,i

t ) + A1t x

2,it|t−1, A

1t P

2t|t−1(A

1t )

T + B1t Q1

t (B1t )T )

5. Update the Kalman filters:Kt = P 2

t|t−1(A1t )

T (A1t P

2t|t−1(A

1t )

T + B1t Q1

t (B1t )T )−1

x2,it|t = x2,i

t|t−1 + Kt(x1,it+1 − f(x1,i

t )−A1t x

2,it|t−1), i = 1, .., N

P 2t|t = P 2

t|t−1 −KtA1t P

2t|t−1

6. Predict the Kalman filters:x2,i

t+1|t = (A2t − CtA

1t )x

2,it|t + Ct(x

1,it+1 − f(x1,i

t )), i = 1, .., N

P 2t+1|t = (A2

t − CtA1t )P

2t|t(A

2t − CtA

1t )

T + B2t Q

2

t (B2t )T

whereQ

2

t = Q2t − ST

t (Q1t )−1St

Ct = B2t ST

t (Q1t )−1((B1

t )T B1t )−1(B1

t )T

Table 2: The combined Kalman and particle filter algorithm.

reference speed of 50 m/s provides the absolute position needed for TAN toin its turn provide a likelihood. Furthermore, to compute the weights of thedifferent particles the estimated errors are fed back to the TAN. This gives themeasurement equation

yt = h(50t + x1t − x1

t|t) + et, (46)

where the measurement noise was set to

et ∈ N (0, 62). (47)

First the particle filter is applied to the entire state vector, using the algo-rithm according to Table 1 with k = 5. Using 30000 particles, the RMSE, basedon 100 Monte Carlo runs, of the estimated position, velocity and accelerationerrors together with their estimated covariances are shown in figure 4. Note that2 of the runs diverged, and therefore removed before the RMSE was calculated.

To demonstrate the filtering method developed in Section 4 we partitioned

14

0 5 10 1540

60

80

100

120

140

160

180

position (km)

terra

in e

leva

tion

(m)

Figure 3: The terrain profile used by TAN during the simulations.

the state vector into two parts,

x1t = x1,t, x2

t =(

x2,t

x3,t

), (48)

and applied the particle filter to x1t and the Kalman filter to x2

t . See Table 2for details on the algorithm. Resampling was performed every 5:th iteration.The number of particles used was 1000. The RMSE, again based on 100 MonteCarlo runs, of estimated position, velocity and acceleration errors together withtheir estimated covariances are shown in figure 5.

Tables 3 and 4 give the performance of the two filters as a function of thenumber of particles. More specifically the table shows how many runs out of atotal of 100 simulations that diverged. Furthermore, the table gives the meanRMSE of the three estimated states after a burn-in time set to 20, 60 and 120sec for the position, velocity and acceleration respectively.

Number of particles 5000 10000 20000 30000Diverged (%) 8 6 2 21/n

∑nt=1 RMSE(x1

t|t), tburn = 20 13 12 11 111/n

∑nt=1 RMSE(x2

t|t), tburn = 60 0.48 0.46 0.45 0.421/n

∑nt=1 RMSE(x3

t|t), tburn = 120 0.0042 0.0040 0.0038 0.0035

Table 3: Percentage of diverged runs and mean RMSE of the estimated statesfor the particle filter.

One clearly sees that it takes a lot of particles for the particle filter to workwell. There are two main reasons for this. First of all if the process noise has a

15

Number of particles 125 250 500 1000Diverged (%) 6 1 1 01/n

∑nt=1 RMSE(x1

t|t), tburn = 20 17 12 11 111/n

∑nt=1 RMSE(x2

t|t), tburn = 60 0.63 0.45 0.40 0.391/n

∑nt=1 RMSE(x3

t|t), tburn = 120 0.0064 0.0045 0.0035 0.0035

Table 4: Percentage of diverged runs and mean RMSE of the estimated statesfor the combined Kalman and particle filter.

small variance. Secondly, if the dimension on the state vector is high, especiallywhen the dimension of the observation vector is low relative to the dimensionof the state vector.

If for some reason the cloud of particles ends up slightly besides the truestate. The only way for the cloud to move back is by the process noise spreadingthe particles. Together with resampling the cloud hopefully finds its way backto again cover the true state. This is of course under the condition that thecloud has not ended up so far off that the likelihood p(yt|xt) is almost flat(or even worse equal to zero as could be the case when p(yt|xt) has a uniformdistribution). If this is the case the risk for divergence is high. Of course onecan always increase the process noise to make the filter more robust, but theprice for this is a worse accuracy.

The reason for the cloud to move away from the true state in the firstplace could for example be outliers. Note that the effect of outliers can bereduced by resampling only every k:th iteration. Another reason is when thestates not directly visible in the measurements are off track. This causes theparticles to be spread unnecessarily much, leaving only a few samples near thetrue state. A peaked likelihood together with a resampling step leaves a cloudvery concentrated to small areas all more or less off track. This phenomenais clearly more pronounced during the first iterations of the filter, and a largeprocess noise initially is therefore important. Note that the combined Kalmanand particle filter deals with this nicely. The particles are spread according to(see Table 2)

{x1,it+1}N

i=1 ∈ N (m, C),

where C depends on the uncertainty in the estimate of x2t which is large initially

but then decreases gradually.

6 Conclusions

In this report, we have proposed a modified particle filter based on a combinationof a standard particle filter and the Kalman filter. This filter has been comparedto a standard stand-alone particle filter.

Simulations show that one has to use a large number of particles, when thedimension of the state vector is high, for the particle filter to work well. One canargue about what is meant about a large number of particles, which of course isdependent on what computational load is acceptable for the problem at hand.Note that the different numbers of particles used during the simulations aresurely possible to decrease by optimising the filter parameters, and this applies

16

to both filters. What is important is that it is possible to reduce the number ofparticles significantly by using the combined Kalman and particle filter.

The great advantage with the particle filter is its enormous flexibility. Onecan apply it to practically anything within a statistical framework. On theother hand, the performance as a function of the number of particles can bequite bad. If possible, it is probably better to linearise as much as possible andsolve the linearised parts analytically and save the particle filter to the reallysevere nonlinear and/or non-Gaussian parts.

References

[1] B.D.O. Anderson and J.B. Moore. Optimal Filtering. Prentice Hall, En-glewood Cliffs, N.J. USA, 1979.

[2] C. Andrieu and A.Doucet. Optimal estimation of amplitude and phasemodulated signals. Technical Report CUED/F-INFENG/TR395, Depart-ment of Engineering, University of Cambridge CB2 1PZ Cambridge, 2000.

[3] C. Andrieu, A. Doucet, and W J. Fitzgerald. An introduction to MonteCarlo methods for Bayesian data analysis.

[4] N. Bergman. Recursive Bayesian Estimation, Navigation and Tracking Ap-plications. Phd thesis 579, Department of Electrical Engineering, LinkopingUniversity, Linkoping, Sweden, 1999.

[5] A. Doucet and C. Andrieu. Particle filtering for partially observed Gaus-sian state space models. Technical Report CUED/F-INFENG/TR393, De-partment of Engineering, University of Cambridge CB2 1PZ Cambridge,September 2000.

[6] A. Doucet, N. de Freitas, K. Murphy, and S. Russell. Rao-Blackwellisedparticle filtering for dynamic Bayesian networks. In Proceedings of the 35thIEEE Conference on Decision and Control, volume 3, pages 3145–3150,1996.

[7] A. Doucet, S.J. Godsill, and C. Andrieu. On sequential simulation-basedmethods for Bayesian filtering. Statistics and Computing, 10(3):197–208,2000.

[8] A. Doucet, N.J. Gordon, and V. Krishnamurthy. Particle filters for stateestimation of jump Markov linear systems. Technical Report CUED/F-INFENG/TR359, Department of Engineering, University of CambridgeCB2 1PZ Cambridge, 1999.

[9] N.J. Gordon, D.J. Salmond, and A.F.M. Smith. Novel approach tononlinear/non-Gaussian Bayesian state estimation. In IEE Proceedings onRadar and Signal Processing, volume 140, pages 107–113, 1993.

[10] F. Gustafsson. Adaptive Filtering and Change Detection. John Wiley &Sons Inc., 1st edition, 2000.

[11] A. Gut. An Intermediate Course in Probability. Springer-Verlag, 1995.

17

[12] B.W. Leach. A Kalman filter integrated navigation design for the IARtwin otter atmospheric research aircraft. Technical Report NRC No. 32148,Institute for Aerospace Research, National Research Council Canada, April1991.

[13] P.G. Savage. Strapdown Inertial Navigation - Lecture Notes. StrapdownAssociates Inc., Plymouth, MN, USA, February 1990.

[14] A.F.M. Smith and A.E. Gelfand. Bayesian statistics without tears: Asampling-resampling perspective. The American Statistician, 46(2), 1992.

18

0 50 100 150 200 250 3000

10

20

30

40

50

RMSE of x1,t

(solid) and P1,t

(dashed)

m

0 50 100 150 200 250 3000

0.5

1

1.5

2

RMSE of x2,t

(solid) and P2,t

(dashed)

m/s

0 50 100 150 200 250 3000

0.005

0.01

0.015

0.02

RMSE of x3,t

(solid) and P3,t

(dashed)

m/s

2

sec

Figure 4: RMSE of xMMSt|t and PMMS

t|t provided by the particle filter based on100 Monte Carlo simulations (2 runs removed because of divergence). Numberof particles = 30000.

19

0 50 100 150 200 250 3000

10

20

30

40

50

RMSE of x1,t

(solid) and P1,t

(dashed)

m

0 50 100 150 200 250 3000

0.5

1

1.5

2

RMSE of x2,t

(solid) and P2,t

(dashed)

m/s

0 50 100 150 200 250 3000

0.005

0.01

0.015

0.02

RMSE of x3,t

(solid) and P3,t

(dashed)

m/s

2

sec

Figure 5: RMSE of xMMSt|t and PMMS

t|t provided by the combined particle andKalman filtering method based on 100 Monte Carlo simulations. Number ofparticles = 1000.

20