institutionen för systemteknik - diva...

Institutionen för systemteknikDepartment of Electrical Engineering

Examensarbete

Development and evaluation of a filter for tracking

highly maneuverable targets

Examensarbete utfört i Reglerteknikvid Tekniska högskolan vid Linköpings universitet

av

Viktor Pirard

LiTH-ISY-EX--11/4499--SE

Linköping 2011

Department of Electrical Engineering Linköpings tekniska högskolaLinköpings universitet Linköpings universitetSE-581 83 Linköping, Sweden 581 83 Linköping

Development and evaluation of a filter for tracking

highly maneuverable targets

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköpingav

Viktor Pirard


Handledare: Karl Granströmisy, Linköpings universitet

Egils SviestinsSAAB AB

Examinator: Fredrik Gustafssonisy, Linköpings universitet

Linköping, 22 August, 2011

Avdelning, Institution

Division, Department

Division of Automatic ControlDepartment of Electrical EngineeringLinköpings universitetSE-581 83 Linköping, Sweden

Datum

Date

2011-08-22

Språk

Language

Svenska/Swedish

Engelska/English

⊠

Rapporttyp

Report category

Licentiatavhandling

Examensarbete

C-uppsats

D-uppsats

Övrig rapport

⊠

URL för elektronisk version

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

http://www.ep.liu.se

ISBN

—

ISRN


Serietitel och serienummer

Title of series, numberingISSN

—

Titel

TitleUtveckling och utvärdering av ett filter för målföljning av snabbt manövrerandemål

Development and evaluation of a filter for tracking highly maneuverable targets

Författare

AuthorViktor Pirard

Sammanfattning

Abstract

In modern systems for air surveillance, it is important to have a high quality situa-tion assessment. SAAB has a system for air surveillance, and in this thesis possibleimprovements of the tracking performance of this system are explored. The focushas been on improving the tracking of highly maneuverable targets observed withlow sampling rate. To evaluate improvements of the tracking performance, a com-ponent that is similar to the one used in SAAB’s present tracker was implementedin an Interacting Multiple Model (IMM) structure. The use of an Auxiliary Par-ticle Filter for improving the tracking performance is explored, and a way to fita particle filter into SAAB’s existing IMM framework is proposed. The differ-ent filters were implemented in Matlab, and evaluation was done by the meansof Monte Carlo simulations. The results from Monte Carlo simulations show sig-nificant improvement when tracking in two dimensions. However, the results inthree dimensions do not display any substantial overall improvement when usingthe particle filter compared to using SAAB’s present filter. It is therefore notworthwhile to switch the filter used in SAAB’s present tracker for a particle filter,at least not under the high SNR circumstances presented in this thesis. However,further studies within this area are recommended before any final decisions aremade.

Nyckelord

Keywords Target tracking, Particle Filter, Interacting Multiple Model

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

http://www.ep.liu.se

Abstract

In modern systems for air surveillance, it is important to have a high quality situa-tion assessment. SAAB has a system for air surveillance, and in this thesis possibleimprovements of the tracking performance of this system are explored. The focushas been on improving the tracking of highly maneuverable targets observed withlow sampling rate. To evaluate improvements of the tracking performance, a com-ponent that is similar to the one used in SAAB’s present tracker was implementedin an Interacting Multiple Model (IMM) structure. The use of an Auxiliary Par-ticle Filter for improving the tracking performance is explored, and a way to fita particle filter into SAAB’s existing IMM framework is proposed. The differ-ent filters were implemented in Matlab, and evaluation was done by the meansof Monte Carlo simulations. The results from Monte Carlo simulations show sig-nificant improvement when tracking in two dimensions. However, the results inthree dimensions do not display any substantial overall improvement when usingthe particle filter compared to using SAAB’s present filter. It is therefore notworthwhile to switch the filter used in SAAB’s present tracker for a particle filter,at least not under the high SNR circumstances presented in this thesis. However,further studies within this area are recommended before any final decisions aremade.

Sammanfattning

I moderna ledningssystem för luftövervakning är det viktigt att ha en högkvali-tativ lägesbild. En kritisk komponent i ett ledningssystem utgörs av målföljning.Detta examensarbete har gått ut på att söka förbättrad målföljningsprestandai ett at SAABs system. Det är förbättringar av följningsprestandan för snabbtmanövrerande mål vid glesa inmätningar som har undersökts här. För att utvär-dera följningsprestandan implementerades först en snarlik komponent till den somanvänds i SAABs målföljningssystem i en så kallad Interacting Multiple Model(IMM) struktur. Användningen av ett Auxiliary Partikelfilter för att förbättra no-grannheten undersöks, partikelfiltret har sedan adderats till en IMM struktur föratt verifiera att filtret lätt kan inkluderas i SAABs nuvarande produkt. De olikafiltren implementerades i Matlab och utvärdering utfördes med hjälp av MonteCarlo simuleringar. Resultaten från simuleringarna visar på signifikanta förbätt-ringar vid målföljning i horisontalplanet. Simuleringar i tre dimensioner visar dockinte på några stora förbättring för partikel filtet jämfört med SAABs nuvarandefilter. Det är därför i nuläget inte motiverat att byta ut SAABs nuvarande filtermot ett partikel filter, i alla fall inte i de scenarios med hög SNR som undersökts

v

vi

i denna rapport. Framtida studier rekommenderas dock innan några definitivabeslut fattas.

Acknowledgments

The past five years of my life have been spent at Linköping University, pursuingthe Master of Science degree in Applied Physics and Electrical Engineering. Asthis Master thesis will conclude my studies, I would like to seize the opportunityto thank the people who have supported me in some way during my studies. FirstI would like to thank all my friends at Applied Physics and Electrical Engineering,you know how you are, without you I don’t think I would be writing this thesis.

My supervisor at Saab, Dr. Egils Sviestins, deserves great credit for findingtime in his busy schedule to give me inspiration and guidance. All my colleagues atthe data fusion department also deserve my gratitude for making me feel welcomeand helping me whenever I had questions. The creation of this thesis was alsohelped by Lic. Karl Granström and Prof. Fredrik Gustafsson at the divisionof Automatic control at the department of Electrical Engineering at LinköpingUniversity. They helped me with all questions regarding filtering and gave meinspiration for this thesis and Karl gave me great feedback on the layout andcontent of this thesis.

Finally I would like to extend my deepest gratitude to my mother and father,my sisters Elin and Jenny and my girlfriend Jennifer for all your support over thecourse of this thesis.

Stockholm, September 27, 2011Viktor Pirard

vii

Contents

1 Background 11.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Related work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.4 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.5 Thesis outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Radar 52.1 Range measurement . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 Range Rate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3 Angle measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.4 Sensor Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.4.1 Measurement Noise . . . . . . . . . . . . . . . . . . . . . . . 8

3 System Modelling 113.1 Dynamic models . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2 Planar models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.2.1 Constant Velocity . . . . . . . . . . . . . . . . . . . . . . . 123.2.2 Coordinated Turn . . . . . . . . . . . . . . . . . . . . . . . 133.2.3 Curvilinear motion model . . . . . . . . . . . . . . . . . . . 14

3.3 3D models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.3.1 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . 163.3.2 3D Coordinated Turn . . . . . . . . . . . . . . . . . . . . . 173.3.3 Variable Turn model . . . . . . . . . . . . . . . . . . . . . . 193.3.4 Process noise transformation . . . . . . . . . . . . . . . . . 21

4 Estimation 234.1 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.2.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 254.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . 25

4.3.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264.4 Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . 27

4.4.1 Sigma Points . . . . . . . . . . . . . . . . . . . . . . . . . . 274.4.2 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

ix

x Contents

4.5 Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 294.5.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314.5.2 Resampling . . . . . . . . . . . . . . . . . . . . . . . . . . . 314.5.3 Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . 324.5.4 Prior Sampling . . . . . . . . . . . . . . . . . . . . . . . . . 334.5.5 Likelihood sampling . . . . . . . . . . . . . . . . . . . . . . 334.5.6 Auxiliary Particle filter . . . . . . . . . . . . . . . . . . . . 35

4.6 Interacting Multiple Model . . . . . . . . . . . . . . . . . . . . . . 374.6.1 State Combination . . . . . . . . . . . . . . . . . . . . . . . 39

4.7 Performance evaluation . . . . . . . . . . . . . . . . . . . . . . . . 414.7.1 Root Mean Square Error . . . . . . . . . . . . . . . . . . . 414.7.2 Normalized Estimation Error Square . . . . . . . . . . . . 42

5 Results 435.1 Filter evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435.2 Trajectory 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5.2.1 Simulation I . . . . . . . . . . . . . . . . . . . . . . . . . . . 455.2.2 Simulation II . . . . . . . . . . . . . . . . . . . . . . . . . . 49

5.3 Trajectory 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 535.3.1 Simulation I . . . . . . . . . . . . . . . . . . . . . . . . . . . 555.3.2 Simulation II . . . . . . . . . . . . . . . . . . . . . . . . . . 585.3.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

6 Conclusions and Future Work 636.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 636.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

Bibliography 67

A Appendix 69A.1 3D Measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69A.2 Simulation Parameters . . . . . . . . . . . . . . . . . . . . . . . . . 70

A.2.1 Trajectory 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . 70A.2.2 Trajectory 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

Chapter 1

Background

In this chapter a short background to air surveillance systems is given to makethe motivation for this thesis clear. After that the purpose of this master thesis ispresented, followed by the limitations to the scope of the thesis.

1.1 Motivation

In modern air surveillance systems it is important to have a high quality situa-tion assessment. The operator of an air surveillance system needs to get an exactsituation picture to be able to make the correct decision. Measurements are com-monly given by some type of radar, airborne or ground based, and typically onlygive position information. An exact situation picture does not only incorporateknowing the current positions of different air targets, but also the headings andspeeds with which the targets are moving. This is where target tracking plays animportant role and this is also the context in which this thesis takes place.

SAAB has a system for air operations that is currently in use in differentcountries around the world, and SAAB is continuously trying to improve the per-formance of this system. An aircraft can typically fly in different modes, e.g. whenan aircraft is flying at constant speed and heading, the target is said to be non-maneuvering. When an aircraft is performing turns or rapidly descends/ascendsit is said to be maneuvering. There is also a distinction made between highlymaneuvering targets and maneuvering targets, the difference is if the maneuveris a high-g - includes large accelerations - one or not. The cut-off point betweenthe higly maneuvering and maneuvering is in this thesis defined as twice that ofthe earth gravity 9.82[ m

s2 ], i.e. targets with accelerations larger than 2 · 9.82 arecalled highly maneuvering. With this master thesis, SAAB wants to look into thepossibility of improving the tracking performance of highly maneuvering targetswhen the rate at which measurements arrive is low, i.e. Ts > 2, where Ts denotesthe sampling time.

1

2 Background

1.2 Problem formulation

In this master thesis, the problem of tracking highly maneuverable targets withlow measurement rate is considered. Since the tracking problem is non-linear,different non-linear filtering methods are considered. Especially the use of particlefilters for tracking is explored. The different algorithms are implemented in Matlaband evaluated with Monte Carlo simulations. The most promising filter is thenintegrated in an Interacting Multiple Model (IMM) structure, together with adifferent model responsible for estimation during non-maneuvering phases.

1.3 Related work

The field of target tracking has been around for a half a century, and many articlesabout particle filtering and Interacting Multiple Model have been published duringthe last decades. To give the reader a good overview of all the related work is farbeyond scope of this thesis. There is however a couple of articles closely relatedto the work in this thesis.

In [7] the authors evaluate different EKF, UKF and PF combinations in anIMM constellation. The main difference between the problem that is addressedthere and the problem in this thesis is that in [7] the process noise covarianceis relatively larger than the measurement noise covariance. This is due to thefact that they have a considerably shorter sampling time (1 second) and havemuch lower accuracy in their measurements. In the scenario studied in this thesis,the situation is the opposite. Since the sampling time is up to a factor of tenlarger, the impact from the process noise over the sampling period makes processnoise covariance larger than the measurement noise covariance. The difference inscenarios makes it hard to use the results in [7].

Another interesting article concerning a similar subject is [22]. In this articlea different model, suggested in [15], is used to handle the fast maneuvers. Themeasurements are here given by azimuth, elevation, the change rate of azimuthand elevation as well as the range rate. The performance of an IMM running withtwo EKF filters is compared to an IMM running with two UKF filters. In thesimulation studies conducted in this article the sampling time is quite low (0.5s)and the measurement uncertainty is very low. The authors show that for this casethe UKF greatly outperforms the EKF. One could speculate that this is due tothe non-linearities of the measurements equation, in particular the different changerates. The fact that the measurements used in this article differs from the ones inthis thesis makes it hard to make any direct use of the results in [22].

The two previous articles deal with tracking in 3D. A couple of articles dealingwith tracking in 2D are [13] and [5]. The former compares the performance of anAuxiliary particle filter running three different Coordinated Turn models with thatof an IMM running three EKF filters for the same models. The latter compares theperformance and robustness of a couple of different algorithms. The measurementsare here given by the received power strength and have quite large uncertaintycompared to the process noise. This makes it hard to draw parallels to the workdone in this thesis.

1.4 Limitations 3

1.4 Limitations

To limit the scope of this thesis, the following assumptions are made.

• The measurements are given by a single radar. To simulate the effect ofmultiple radars the time between measurements can be decreased/increased.A more realistic scenario would be that the target is not seen by both radarsat all times, so the sampling time would be variable.

• Only a single target is considered and there is no false or missed radarmeasurements. By this assumption the gating and association problem isavoided, and the problem of variable sampling time is also avoided.

• There is no bias in the radar measurements.

1.5 Thesis outline

The thesis consist of four parts, the first part introduces the problem and presentssome related work. The second part presents the theory used in the thesis and isdivided into two chapters, the first chapter presents the theory behind the radarand the second chapter presents estimation theory. The third part presents thesimulation results, and the fourth part gives some concluding remarks and ideasfor future work.

Chapter 2

Radar

This chapter is meant to give the reader a brief overview of the sensor used in thisthesis. A full discussion about radar and the different properties is beyond thescope of this text. For a more detailed overview of the radar system, the reader isreferred to [4] and [16], which have influenced this text. A radar - RAdio Detectionand Ranging - is a sensor used for determining the position of a target relative tothe radar station. The radar was developed in the early 1930’s and has since thenseen widespread use. A crude explanation of how a radar works is that it sendsout electromagnetic energy in a given direction. If there is a target within therange of the radar in this direction, some of the transmitted energy will bounceoff the object and return to the radar. The most common measurements availablefrom a radar is range, azimuth, elevation and range rate. In an air defence system,radar is the main - if not only - sensor input. Since the first radar in the 1930’s,several different types of radars, working in different frequency intervals suitablefor different purposes, have been developed. For the purpose of this thesis, onlyground based radars are of interest, and therefore only this radar type will bedescribed here. First a short explanation of how the range, range rate, azimuthand elevation are measured by the radar is given. This is followed by a presentationon ground based radars in air defence systems.

2.1 Range measurement

To determine the range to the target, the round trip travel time t - the time ittakes for the transmitted pulse to travel from the radar to the target and back -is measured.

A radio signal is an electromagnetic wave with a frequency lower than that ofvisible light. Radio signals travel by the speed of light c. The travelled roundtripdistance, will be twice the distance R from the radar to the target,

2R = ct. (2.1.1)

A radar measures t and uses (2.1.1) to calculate the distance R.

5

6 Radar

2.2 Range Rate

The Doppler effect can be used to obtain an estimate of the range rate, defined asR = dR

dt of the target. The principle behind the Doppler effect is that waves beingreflected by a target moving relative to the radar will cause a shift in the frequencyof the reflected waves compared to the frequency of the transmitted waves. If thetarget is moving towards the radar it will make the wavelength shorter, and if thetarget is moving away from the radar the wavelength will become longer. TheDoppler effect is given by

f ′ = f − vr

λ, (2.2.1)

where f ′ is the frequency the observer measures, f is frequency transmitted bythe Radar, vr is the target’s radial velocity relative to the Radar and λ is thewavelength of the transmitted waves. From (2.2.1) the following equation can bederived for a Radar,

R = −fdλ

2. (2.2.2)

The factor 2 in the denominator is due to the fact that Doppler effect occurs bothon the way from the radar to the target and on the way back. By knowing thewavelength λ of the transmitted signal, and calculating the Doppler frequencyshift as fd = f − f ′ the range rate can be determined using (2.2.2).

2.3 Angle measurement

For a 3D radar, the angle measurements give the azimuth α and elevation ǫ.The range together with the azimuth and elevation places the target in a radaroriented spherical coordinates system, which can be transformed into a Cartesiancoordinate system, see Figure 2.1. A very simple explanation of how the anglemeasurements are obtained is that the radar gives its own azimuth and elevationwhen the reflected signal is detected.

2.4 Sensor Modeling

Ground based radars that are used in air operations systems often work in thefrequency band [1 − 2] GHz called the L-band. The antennas used are large andhave a long range, and the radar continuously rotates the antenna to get full 360degree coverage. The long range capability of the L-band radars has the drawbackin that it gives the antenna a low turn rate, i.e. the time it takes for the antennato complete a full 360 degree sweep is high. The shape and size of the radarantenna determines its resolution in azimuth and elevation. In the case of groundbased antennas used in air operations systems, the antenna is often parabolic inshape and rotates around a vertical axis, see Figure 2.2. This type of antennagives a high resolution in azimuth but low resolution in elevation. The range ratemeasurements from a radar in the L-band is in general not available, and the rangerate measurements will therefore not be modelled in this thesis.

2.4 Sensor Modeling 7

0

0.1

0.2

0.3

0.4

0.5 0

0.1

0.2

0.3

0.4

0.5

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

y

x

z

RAzimuthEleviation

Figure 2.1: Radar measurements in a Radar oriented Cartesian coordinate system.

8 Radar

Figure 2.2: Radar antenna

2.4.1 Measurement Noise

The measurements given from a radar are not exact, there are many different noisesources that affect the radar performance. Examples include hardware dependentnoise, e.g. noise from electronic equipment, and thermal noise. Thermal noise isin fact dependent on the elevation angle of the radar, but these effects are notconsidered here. A standard way of modelling the impact of the hardware andthermal noise is as additive white Gaussian noise, independent in the differentmeasurement quantities (range, azimuth and elevation). This simple model hasseen widespread use and has also be used in this thesis. According to [2], thismodel is also a realistic one.

The radar measurements are modelled in the following way for the 2D case:

R =√

x2 + y2 + vR, (2.4.1a)

α = arctan 2(y, x) + vα. (2.4.1b)

In the 3D case, the elevation angle is also measured giving the following measure-ment equation:

R =√

x2 + y2 + z2 + vR, (2.4.2a)

α = arctan 2(y, x) + vα, (2.4.2b)

ǫ = arctan 2(z,√

x2 + y2) + vǫ, (2.4.2c)

where vR, vα, vǫ are Gaussian white noise processes and [x, y, z] is the Carte-sian position of the target in a radar centred coordinate system. The function

2.4 Sensor Modeling 9

arctan2(y, x) is defined as

arctan2(y, x) =

arctan( yx ) x > 0

π + arctan( yx ) y ≥ 0, x < 0

−π + arctan( yx) y < 0, x < 0

π2 y > 0, x = 0− π

2 y < 0, x = 00 y = 0, x = 0

(2.4.3)

In this thesis, all tracking is done in a radar centred coordinate system, i.e. theradar is located at the origin. This can be assumed without loss of generality.

Chapter 3

System Modelling

This chapter starts with an introduction of the use of continuous state space modelsto represent differential equations. Next, discrete state space models are presentedand also the process of going from a continuous state space model to a discrete statespace model. When the discussion of state space models is complete, state spacemodels describing different motions that an aircraft can perform is presented. Firstplanar models that describe aircraft motion in 2D is presented followed by modelsfor aircraft motion in 3D. Finally, a transformation for process noise modelled inthe aircraft’s coordinate system to an inertial coordinate system is presented.

3.1 Dynamic models

The dynamic behaviour of a physical system is in general modelled by differentialequations. A state space model is a powerful way of representing the differentialequations that describe the system and measurement dynamics, and is presentednext. Let the system and measurement dynamics be given by

X(t) = f(X(t), u(t)) + v(t), Cov(v(t)) = Q, (3.1.1a)

y(t) = H(X(t), u(t)) + e(t), Cov(e(t)) = R. (3.1.1b)

Here, X(t) denotes the state vector, u(t) is a deterministically known control signaland v(t) and e(t) denote process and measurement noise, respectively. Note that in(3.1.1) the random noise is additive. This is not the most general model, howeveronly additive noise is considered in this thesis. For the subject of this thesis, thereis no known control signal u(t) and it has therefore been omitted in the remainderof the thesis.

The model (3.1.1) is continuous, in practical applications it is more often ap-propriate to describe the system as a discrete one. This leads to the need fordiscrete process and measurement models which is given next,

Xk+1 = F (Xk) + vk, Cov(vk) = Qk, (3.1.2a)

yk = H(Xk) + ek, Cov(ek) = Rk. (3.1.2b)

11

12 System Modelling

The difference between X(T ) and Xk is that the discrete state vector Xk takes onthe value of X(t) at discrete time instances seperated by the sampling time Ts,i.e. Xk = X(k ∗ Ts). Also note the difference in notation between the continuousstate vector X(t) and the discrete time state vector Xk.

To discretize a continuous system, the following formula, called the samplingformula, can be used,

X(t+ Ts) = X(t) +

∫ t+Ts

t

f(X(τ))dτ . (3.1.3)

Discretizing the state noise is not quite as straightforward and there are severaldifferent ways of doing this, corresponding to different assumptions about how theprocess noise enters the system. In the discretization formula for process noiseused here,

Qk =

∫ Ts

0ef ′

x(X(τ))τQe(f ′x(X(τ))τ)T

dτ , (3.1.4)

the process noise is assumed to be continuous white noise. Here, f ′x denotes the

Jacobian of f with respect to x, see Section 4.3. If the noise is assumed to enterthe system directly after a sample, the following discretization scheme is available

Qk = Tsf′(X(kTs))Qf ′(X(kTs))T . (3.1.5)

For a more detailed study on different ways of discretizing process noise, see [10].

3.2 Planar models

To get a good understanding of the problem, the tracking problem is first examinedin 2 dimensions. Here the dynamics described by the different models are not ascomplex as those describing a 3D motion. In the following section, the 2D modelsare extended to three dimensions.

3.2.1 Constant Velocity

The constant velocity (CV) model is a simple model that describes linear motion.There are two different usages for this model. If the process noise is small, themodel is suitable for describing straight motion with small changes in velocity.If the process noise is large it models all the forces acting on the plane - thrustfrom the motor and wind gusts - as white noise. Through Newton’s second law,F = ma, the resulting force can be coupled to the acceleration of the target. Thusa large process noise models a large resulting force acting on the aircraft. Thismakes it useful for describing fast maneuvers.

The continuous model is given by

X(t) =

0 0 1 00 0 0 10 0 0 00 0 0 0

X(t) +

0 00 01 00 1

v(t), (3.2.1)

3.2 Planar models 13

where X(t) = (x, y, x, y)T and Cov(v(t)) = Q =

(

σ2x 0

0 σ2y

)

. Using (3.1.3) gives

the discrete model,

Xk+1 =

1 0 Ts 00 1 0 Ts

0 0 1 00 0 0 1

Xk + vk. (3.2.2)

The discrete process noise vk covariance, Qk, is given via (3.1.4) as

Qk =

0 00 01 00 1

Q

(

0 0 1 00 0 0 1

)

. (3.2.3)

From the Taylor expansion of eAT = I + AT + A2

2! T2 + A3

3! T3 + . . ., and noticing

that A2 = (AT )2 = 0, the covariance is then given as

Qk =

σ2xT 3

s

3 0σ2

xT 2s

2 0

0σ2

yT 3s

3 0σ2

yT 2s

2σ2

xT 2s

2 0 σ2xTs 0

0σ2

yT 2s

2 0 σ2yTs

. (3.2.4)

3.2.2 Coordinated Turn

The coordinated turn (CT) motion describes a circular motion with constant turnrate w and speed s, and it has seen widespread use. In the CT-model, process noiseis included as the derivate of turn rate and speed, and represents deviations froman exact circular path. The CT-model can be given either in Cartesian coordinatesor in polar coordinates, both models describe exactly the same motion. In [10]and [17] it is shown that the representation in polar coordinates gives a smallerlinerization error than the Cartesian representation. In polar coordinates, themotion is given by

x(t) =s(t) cos θ(t), (3.2.5a)

y(t) =s(t) sin θ(t), (3.2.5b)

s(t) =v(s)(t), (3.2.5c)

θ(t) =w(t), (3.2.5d)

w(t) =v(w)(t). (3.2.5e)

14 System Modelling

Using (3.1.3) gives

xk+1 =xk +2sk

wksin

wkTs

2cos θk +

wkTs

2, (3.2.6a)

yk+1 =yk +2sk

wksin

wkTs

2sin θk +

wkTs

2, (3.2.6b)

sk+1 =sk, (3.2.6c)

θk+1 =θk + wkTs, (3.2.6d)

wk+1 =wk. (3.2.6e)

The discrete process noise given here is taken from [15],

Qk =

0 0 0 0 00 0 0 0 00 0 T 2

s σ2s 0 0

0 0 0 T 3s σ

2w/3 T 2

s σ2w/2

0 0 0 T 2s σ

2w/2 T 2

s σ2w

. (3.2.7)

Here, σs and σw is the standard deviation of the continuous process noise for speedand turn rate, respectively.

3.2.3 Curvilinear motion model

A curvilinear (CL) motion is a more general type of motion then a CT-motion,as it represents both along- and cross-track acceleration. This means that it candescribe an object that either accelerates, decelerates or keep their velocity duringa turn. The CT-model is a special case of the CL-model for the case when thealong-track acceleration is zero. This model described in Cartesian coordinateswas given in [15], a polar representation of this model was not found and is nextderived.

The state vector is X(t) = (x(t), y(t), s(t), φ(t), w(t), al(t))T , where x(t) andy(t) denotes the target position in Cartesian coordinates, s(t) the speed, φ(t) theheading, w(t) the turn rate, al(t) is the longitudinal acceleration and v(w)(t) andv(a)(t) are white noise processes. The motion model equations are,

x(t) = s(t) cosφ(t), (3.2.8a)

y(t) = s(t) sin φ(t), (3.2.8b)

s(t) = al(t), (3.2.8c)

φ(t) = w(t), (3.2.8d)

w(t) = v(w)(t), (3.2.8e)

al(t) = v(a)(t). (3.2.8f)

In Figure 3.1, the impact of al(t) on the CL-motion is described. To be able togo to a discrete state space representation, the assumptions that al(t) and w(t) are

3.2 Planar models 15

a < 0a > 0a = 0Start

Figure 3.1: Simulated trajectories for a Curvilinear motion with the same stateX(t) and turn rate w(t), but with different normal accelerations al(t).

16 System Modelling

constant during a sampling interval are made. These assumptions are the sameassumptions made in the Cartesian CL-model presented in [15].

Using(3.1.3) on the x component (3.2.8a) gives

xk+1 = xk +

∫ t+Ts

t

s(τ) cosφ(τ)dτ

= xk +

[

s(τ)

∫

cosφ(τ)dτ

]

−∫ t+Ts

t

al

∫

cos(φ(τ))dτ

=

/∫

cos(φ(τ))dτ ≈ − 1

wsin(φ(τ))

/

= xk +

[

−s(τ)

wsinφ(τ)

]

+

∫ t+Ts

t

al

wsin(φ(τ))dτ

=xk+(sk+1

wsin φk+1−

skw

sin φk)+ al

w2 (cos(φk+1)−cos(φk)). (3.2.9)

Remaining states can be treated similarly. The deterministic part of the model isthus given by

xk+1 =xk+sk+1

wksin φk+1−

skwk

sin φk+al

k

w2k

(cos φk+1−cos φk), (3.2.10a)

yk+1 =yk+skwk

cos φk−sk+1

wkcos φk+1+

alk

w2k

(sin φk+1−sin φk), (3.2.10b)

sk+1 =sk+alkT, (3.2.10c)

φk+1 =φk+wkTs, (3.2.10d)

wk+1 =wk, (3.2.10e)

alk+1 =al

k. (3.2.10f)

Here, the process noise v(t) is assumed to enter the system immediately aftera sample, which corresponds to X(t+Ts) = f(X(t) + v(t)). This model is used ina particle filter, see chapter 4, and a discrete process noise has therefore not beenderived.

3.3 3D models

In the remainder of this chapter, the 3D counterparts of the planar models de-scribed in the previous section are described. Motion in 3D requires more mathe-matics than used in the planar case, therefore a short presentation of the mathe-matics needed to obtain the different motion models are presented first.

3.3.1 Preliminaries

The following derivation draws heavily from [15]. Take two Cartesian coordinatesystems, one that has its center in the target body, denoted B, and an inertialcoordinate system on the ground, denoted I. Let p, s, a be the target’s position,velocity and acceleration vectors in the inertial frame, s and a be the magnitudeof the speed vector and acceleration vector, Ω the angular velocity vector in the

3.3 3D models 17

body frame and ΩBI be the angular velocity vector of the body frame relative tothe inertial frame.

If u(t) is a time varying vector, Saint-Guilhems equation relates the derivativeof u(t) in the body frame to the inertial frame, see e.g. [6],

(du

dt)I = (

du

dt)B + ΩBI × u(t). (3.3.1)

Since the target body frame rotates with the target, Ω = ΩBI . Place the x-axisin the body frame so that it is aligned with the speed vector s, x = s

s. By using

(3.3.1) the acceleration in the inertial frame is obtained as

˙s = (ds

dt)B + Ω × s = (

d

dt· s

s)B + Ω × s = s

s

s+ Ω × s (3.3.2)

Taking the matrix product s× ˙s gives

s× ˙s = s×(

ss

s+ Ω × s

)

= s× (Ω × s) = s2Ω − (Ωs)s (3.3.3)

Assuming that Ω⊥v, the following expression for the angular velocity is ob-tained

Ω =s× a

v2, (3.3.4)

where a = ˙s. Due to the properties of the matrix cross product, i.e. the resultingvector from a matrix cross product is always orthogonal to the vectors beingcrossed ( a = b × c ⇒ a⊥b, a⊥c), gives that Ω⊥a. This means that the motionis in a plane - s, a is in a plane - with Ω as normal. This plane is called themaneuverplane, and it is not restricted to the horizontal plane, it can have anyrotation in space.

The norm of the angular velocity vector Ω, called the turn rate w, is nowdefined from (3.3.4) as

w ≡ ‖Ω‖=‖s× a‖

s2. (3.3.5)

3.3.2 3D Coordinated Turn

The three dimensional coordinated turn (3D-CT) model is based on the sameassumptions as its planar counterpart, repeated here for convenience:

• the speed s is constant, and

• the turn rate w is constant.

The assumption that the speed is constant means that a⊥v, otherwise there wouldbe a speed change. This means that (3.3.5) simplifies to

w ≡ ‖Ω‖=a

s(3.3.6)

18 System Modelling

The constant speed in the body frame gives, together with (3.3.1),

a = Ω × s. (3.3.7)

Taking the time derivative of a, and using the assumption that the turn rate isconstant Ω = 0, gives

˙a = Ω × s+ Ω × a = Ω × (Ω × s) = (Ωs)Ω − (ΩΩ)s = −w2s. (3.3.8)

This leads to the following continuous model

X(t)=

0 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0 0 1 0 00 0 0 0 0 0 0 1 00 0 0 0 0 0 0 0 10 0 0 −w2 0 0 0 0 00 0 0 0 −w2 0 0 0 00 0 0 0 0 −w2 0 0 0

X(t)+

0 0 00 0 00 0 00 0 00 0 00 0 01 0 00 1 00 0 1

v(t)

(3.3.9)

where the state vector is given by X = (x, y, z, x, y, z, x, y, z)T

and

v(t) = (vx(t), vy(t), vz(t))T

.

Note that the noise here is given in the inertial coordinate system. The dynam-ics described here by noise is more naturally described in body coordinates thanin an inertial coordinate system. This means that the noise has to be transformedfrom the body frame to the inertial frame. This transformation is described inSection 3.3.4.

The discrete equivalent to the continuous motion model is then given via(3.1.3),

Xk+1=

1 S1 C1 0 0 0 0 0 0

0 C2 S1 0 0 0 0 0 0

0 −S2 C2 0 0 0 0 0 0

0 0 0 1 S1 C1 0 0 0

0 0 0 0 C2 S1 0 0 0

0 0 0 0 −S2 C2 0 0 0

0 0 0 0 0 0 1 S1 C1

0 0 0 0 0 0 0 C2 S1

0 0 0 0 0 0 0 −S2 C1

Xk, (3.3.10)

where the notation S1 = sin(wTs)w , S2 = w sin(wTs), C1 = 1−cos(wTs)

w2 and C2 =cos(wTs) has been introduced to get a more compact expression. The motionis only coupled via the common turn rate w given by (3.3.6) over the differentcoordinates.

In [15] the discrete covariance is given as a function of w,

3.3 3D models 19

Qk(w) =

6wT −8 sin(wT )+sin(2wT )4w5 , 2 sin4(wT/2)

w4 , −2wT +4 sin(wT )−sin(2wT )4w3

2 sin4(wT/2)w4 , 2wT −sin(2wT )

4w3 , sin2(wT )2w2

−2wT +4 sin(wT )−sin(2wT )4w3 , sin2(wT )

2w2 , 2wT +sin(2wT )4w

,

(3.3.11)which can be obtained via the use of (3.1.4).

In [21] the discrete covariance of the process noise Qk is instead modelled asthat of a constant acceleration model where time derivative of the acceleration,i.e. the jerk, is a white noise process. The discrete covariance matrix for a whitenoise jerk model in one dimension with variance σ2

x is

Qk = σ2x

T 5s

20 ,T 4

s

8 ,T 3

s

6T 3

s

2 ,T 2

s

2 , TsT 2

s

2 , Ts, 1

. (3.3.12)

This model does not depend on w, which seems quite strange since this means thatthe uncertainty given by the process noise will have equal size in all components.This means that the process noise is independent of the motion, e.g. if the turn isin the horizontal plane, the uncertainty will be equally large in the vertical plane,even though no vertical motion is performed.

3.3.3 Variable Turn model

This model is an extension of the planar CL model and is more general than the3DCT-model since it can describe a wider range of motions. The variable turnmodel does not, like the 3DCT-model, assume that the speed and turn rate vectorsare constant. It does assume that the motion is planar so that the angular velocityvector is orthogonal to the speed and acceleration vectors. Using (3.3.1), the jerkin the inertial frame is derived as

˙a = (d2s

dt2)B + Ω × s+ 2Ω × ˙s− Ω × (Ω × s). (3.3.13)

Using the fact that Ω⊥s, (3.3.4) can be used, giving

˙a = (d2s

dt2)B + Ω × s− (

(s× a) · (s× a)

s4)s− 2

a2

s2s+ 2

a · ss2

a. (3.3.14)

Further, the following holds

α ≡ − s · as2

, (3.3.15a)

v ≡(d2s

dt2)B + Ω × s, (3.3.15b)

where the variable α can be interpreted as a relative damping coefficient andv captures the effect of forces and moments acting on the target. With thesedefinitions, and w as defined in (3.3.6), (3.3.14) can be simplified to

20 System Modelling

a < 0a > 0a = 0Start

Figure 3.2: Simulated trajectory for a variable turn motion. The differences be-tween the trajectories are only in the relative dampening α, where the trajectorieshave α = 0.05, α = −0.05, α = 0 respectively.

3.3 3D models 21

˙a = −2αa− (2α2 + w2)s+ v. (3.3.16)

The continuous state space model is the same in all the components and onlycoupled via a common w and α. The model in the x-component is given by

X(t) =

0 1 00 0 10 −(α2 + w2

c ) −2α

X(t) +

001

v. (3.3.17)

The state vector is here given by X(t) = (x, x, x)T and the variable wc = α2 +w2,is introduced to simplify the expressions. Here, v is modelled in the body frameand transformed to the inertial frame, see Section 3.3.4. In Figure 3.2 a variableturn motion using different values on the relative dampening α is illustrated. Using(3.1.3), the following discrete time model is obtained

Xk+1=

12αwc−e−αTs (2αwc cos wcTs+(α2−w2

c ) sin wcTs)wc(α2+w2

c)wc−e−αTs (wc cos wcTs+α sin wcTs)

wc(α2+w2c )

0 e−αTs (wc cos wcTs+α sin wcTs)wc

e−αTs sin wcTs

wc

0 − (α2+w2c )e−αTs sin wcTs

wc

e−αTs (wc cos wcTs−α sin wcTs)wc

Xk.

(3.3.18)The process noise is given in [15] and is too tedious to be repeated here. Worth

noticing is that (3.3.1), which is used to determine all 3D models, assumes thatthe system is given in an inertial coordinate system. Since the earth is rotating,a coordinate system placed on the earth surface is not in an inertial coordinatesystem. A better choice in this sense would be to place a coordinate system atthe centre of the earth. This will however not be done in this thesis since theeffects from the rotation of the earth, like the Coriolis effect, does not give anysubstantial affects on the processes of interest in this thesis.

3.3.4 Process noise transformation

The process noise transformation from the body frame to the inertial frame isdescribed here. This transformation is done via the assumption that the speedvector s is never parallel to the z-axis in the inertial frame. This assumption doesnot always hold, but for the applications in mind for this thesis it is valid. Thetransformation is performed in the following steps.

The longitudinal process noise vl, given in body coordinates is transformed viathe speed vector s to global coordinates,

vlx

vly

vlz

=

sx

ssy

ssz

s

sl. (3.3.19)

The transversal process noise vt is then transformed by

s× z =

sy

−sx

0

,

vtx

vty

vtz

=

sy

‖(sy ,−sx,0)T ‖−sx

‖(sy ,−sx,0)T ‖0

‖(sy ,−sx,0)T ‖

vt. (3.3.20)

22 System Modelling

Finally the process noise in elevation, ve is transformed by

sx

sy

sz

×

sy

−sx

0

=

sxsz

sysz

−s2x − s2

y

,

vex

vey

vez

=

sxsz

‖(sxsz ,sysz,−s2x−s2

y)T ‖sysz


y)T ‖

−s2x−s2

y


y)T ‖

ve.

(3.3.21)The total process noise in the x-component of the inertial frame is then simply

vX = vlx + vt

t + vex. (3.3.22)

The remaining y-component and z-component can be treated similarly.

Chapter 4

Estimation

In this chapter the different filtering algorithms used in this thesis are discussed.First, the estimation problem is presented followed by the Bayesian recursion so-lution, and the analytic solution to the linear Gaussian problem in the form of theKalman Filter is presented. Different approximations to the Bayesian recursion isthen presented: the Extended Kalman Filter, the Unscented Kalman Filter andthen the Particle Filter. A way of combining different filters in an interacting way,called Interacting Multiple Model, is then presented. Finally some performanceevaluation methods used in this thesis are discussed. The subject of filtering andestimation is a very active research area and for a more detailed overview see [9]. In[1] many different particle filter algorithms are presented in a very straight-forwardmanner.

4.1 Estimation

The filtering problem is a part of a lager class of problems called estimation prob-lems. The objective of a solution to an estimation problem is to estimate un-known states (or parameters) Xk at time k from measurements up to time n,Y1:n = y1, . . . , yn. The states do not need to be measured directly, but theyneed to somehow affect the measurement. The measurements are not exact, butinfluenced by some noise. This means that one can not determine the state exactlywith full certainty, instead one can obtain the Probability Density Function (PDF)of the state. The sought solution to the estimation problem can be written as aconditional probability density function,

p(Xk|Y1:n). (4.1.1)

The case when n = k is called filtering, this means that the current state xk is tobe estimated given all measurements Y1:k = y1, . . . , yk from time 1 to time k.Other possible scenarios are n < k and n > k, the former is called prediction andthe latter smoothing.

The Bayesian solution to the filtering problem is given by the PDF p(Xk|Y1:k).

23

24 Estimation

The PDF can be estimated recursively in two steps, the time update and the mea-surement update, for a complete derivation see [9]. By the use of Bayes theorem,

p(A|B,C) =p(B|A,C)p(A|C)

p(B|C), (4.1.2)

the recursive updates can be written as

p(Xk|Y1:k) = p(Xk|yk, Y1:k−1) =p(yk|Xk, Y1:k−1)p(Xk|Y1:k−1)

p(yk|Y1:k−1). (4.1.3)

The Markov property of the state space models means that the current stateincorporates all the previous measurements and that different measurements arenot dependent on each other. Using the Markov property, (4.1.3) can be simplifiedto

p(Xk|Y1:k) =p(yk|Xk, Y1:k−1)p(Xk|Y1:k−1)

p(yk|Y1:k−1)=p(yk|Xk)p(Xk|Y1:k−1)

p(yk|Y1:k−1). (4.1.4)

This gives the following recursion. First, a time update is performed,

p(Xk|Y1:k−1) =

∫

p(Xk|xk−1)p(Xk−1|Y1:k−1)dXk−1. (4.1.5a)

Then a measurement update is made,

p(Xk|Y1:k) =p(yk|Xk)p(Xk|Y1:k−1)

p(yk|Y1:k−1), (4.1.5b)

p(yk|Y1:k−1) =

∫

p(yk|Xk)p(Xk|Y1:k−1)dXk. (4.1.5c)

Here, p(yk|Xk) is called the likelihood and is connected to the measurement equa-tion (3.1.1b), p(Xk|Xk−1) is called the transition probability and is connected tothe process model (3.1.1a), p(Xk|Y1:k−1) is called the prior and finally p(Xk|Y1:k) iscalled the posterior. The estimated state vector at time k given the measurementsup to time k is denoted Xk|k. Here, k|k means at time k given the measurementsup to time k, e.g. the estimated state vector given measurements up to time k− 1is denoted Xk|k−1.

4.2 Kalman Filter

The Kalman Filter (KF) is the solution to (4.1.5) in the case of linear measurementand motion dynamics (3.1.1), with additive Gaussian distributed noise. The whitenoise assumption means that a stochastic process v(t) with covariance Π obeysthe following:

E(v(t)) = 0, (4.2.1a)

E(v(t), v(t + τ)) = Πδ(t− τ). (4.2.1b)

4.3 Extended Kalman Filter 25

The process noise v(t) and measurement noise e(t) must also be uncorrelatedCov(v(t), e(t)) = 0.Linear measurement and motion dynamics means that (3.1.2) simplifies to

Xk+1 = FkXk + vk, Cov(vk) = Qk, (4.2.2a)

yk = HkXk + ek, Cov(ek) = Rk. (4.2.2b)

Given these assumptions and that the initial state distribution X1|0, P1|0 isGaussian, all the distributions in (4.1.5) will be Gaussian and described by theirfirst two moments. The solution in this case will be given by KF algorithm bellow.

4.2.1 Algorithm

The Kalman Filter recursion is initialized with an initial estimate X1|0 and covari-ance P1|0. The following equations are iterated for k = 1, 2, . . .

Sk = HkPk|k−1HTk +Rk, (4.2.3a)

Kk = Pk|k−1HTk (Sk)−1, (4.2.3b)

ǫk = yk −HkXk|k−1, (4.2.3c)

Xk|k = Xk|k−1 +Kkǫk, (4.2.3d)

Pk|k = Pk|k−1 − Pk|k−1HTk KkHkPk|k−1, (4.2.3e)

Xk+1|k = FkXk|k, (4.2.3f)

Pk+1|k = FkPk|kFtk +Qk. (4.2.3g)

In the algorithm (4.2.3a) - (4.2.3e) are the measurement update, and (4.2.3f) -(4.2.3g) are the time update.

4.3 Extended Kalman Filter

Now consider the general non-linear model (3.1.2)

Xk+1 = F (Xk) + vk, Cov(vk) = Qk, (4.3.1a)

yk = H(Xk) + ek, Cov(ek) = Rk. (4.3.1b)

Because of the non-linearity it does not in general exist an exact solution to (4.1.5).The idea behind the Extended Kalman Filter (EKF) is to approximate (4.3.1) onthe form of (4.2.2) so that the KF theory can be applied. When the system istransformed to this form, the prior p(Xk|Y1:k−1) and posterior density p(Xk|Y1:k)are approximated as Gaussian. The system is approximated with a linear one via aTaylor expansion around the current state estimate. There are two types of EKF,the first order EKF1, and the second order, EKF2. The difference between thesetwo variants are how many terms from the Taylor expansion that are included,either one or two. Next, the EKF1 recursion is presented.

26 Estimation

4.3.1 Algorithm

The Extended Kalman Filter recursion is initialized with an initial estimate X1|0

and covariance P1|0. The following equations are iterated

Sk = H′

x(Xk|k−1)Pk|k−1H′

x(Xk|k−1)T +H′

e(Xk|k−1)RkH′

e(Xk|k−1)T ,(4.3.2a)

Kk = Pk|k−1H′

x(Xk|k−1)T (Sk)−1, (4.3.2b)

ǫk = yk −H(Xk|k−1), (4.3.2c)

Xk|k = Xk|k−1 +Kkǫk, (4.3.2d)

Pk|k = Pk|k−1 −KkH′

x(Xk|k−1)Pk|k−1, (4.3.2e)

Xk+1|k = F (Xk|k), (4.3.2f)

Pk+1|k = F′

x(xk|k)Pk|kF′

x(Xk|k)T + F′

v(Xk|k)QkF′

v(Xk|k)T . (4.3.2g)

Here, F′

x, H′

x denotes the Jacobians of (4.3.1) with respect to X and similarly,Fv, H

′

e denotes the Jacobians of (4.3.1) with respect to vk and ek. The Jacobianof a function F (Xk) = [f (1) . . . f (m)] with dimension m, with respect to Xk =[x(1) . . . x(nx)] with dimension nx is

F′

x =

df(1)

dx(1) . . . df(1)

dx(nx)

df(2)

dx(1) . . . df(2)

dx(nx)

......

...df(m)

dx(1) . . . df(m)

dx(nx)

. (4.3.3)

Here, one sees that there are a lot of partial derivates needed for the EKF1. Forthe EKF2, even more derivates are needed when also the Hessian needs to becomputed. The performance of the EKF is dependent on the accuracy of theTaylor expansion. If the non-linearities are severe, the Hessian of the expansionwill become large, and there will be a potential gain in using the second ordercorrection terms given by the EKF2 algorithm.

Next, an example of the Jacobians needed by the EKF for a two dimensionalConstant Velocity model and Radar measurements are presented. The tracking ishere done in Cartesian coordinates.

Example 4.1

The CV-model is linear (3.2.1), and the Jacobians F′

x, F′

v are

F′

x(Xk|k−1) =

1 0 T 00 1 0 T0 0 1 00 0 0 1

, (4.3.4a)

F′

v(Xk|k−1) =

1 0 0 00 1 0 00 0 1 00 0 0 1

. (4.3.4b)

4.4 Unscented Kalman Filter 27

The measurement function (2.4.1), is non-linear, and the Jacobians of the mea-surement function H

′

x, H′

e, become a bit more complicated,

H ′x(xk|k−1) =

xk√x2

k+y2

k

yk√x2

k+y2

k

0 0

−yk√x2

k+y2

k

xk

x2k

+y2k

0 0

, (4.3.4c)

H ′e(xk|k−1) =

(

1 00 1

)

. (4.3.4d)

In the appendix, the Jacobians for a 3D CV model are given. Note that theaccuracy of a first order Taylor expansion is highly dependent of the choice ofcoordinate system which the problem is described in. For instance, in [9] and [17]it is shown that a CT model expressed in Cartesian position polar velocity givesa smaller rest term than the same model expressed in Cartesian coordinates.

4.4 Unscented Kalman Filter

Like the EKF, the Unscented Kalman Filter (UKF) tries to approximate the PDFp(Xk|Y1:k) with a Gaussian distribution. The difference between UKF and EKF ishow this is achieved. The UKF does not approximate the non-linear system with alinear one. Instead, it is based on the idea that it is more accurate to approximatethe PDF of the function, than it is to approximate the function and obtain a PDFfrom the approximated function.

The way the UKF does this is by drawing a small number of points, calledsigma points, that describes the prior PDF p(Xk−1|Y1:k−1) as a Gaussian. Thesesamples are then propagated through the non-linear system and a PDF is thencalculated using these samples.

4.4.1 Sigma Points

Before the algorithm for choosing sigma points is revealed, the following notationis needed:

X − Mean of the distribution, (4.4.1a)

χi − Sigma Pointi, (4.4.1b)

L− Dimension of the distribution, (4.4.1c)

Wmi − Weight for sigma point i, used for estimating the mean, (4.4.1d)

W ci − Weight for sigma point i, used for estimating the covariance, (4.4.1e)

ψ − The spread of the sigma points, (4.4.1f)

κ− Secondary scaling parameter, (4.4.1g)

β − Parameter that incorporates higher order information. (4.4.1h)

28 Estimation

Note here that no time index is given to X . The lack of a time index is a designchoice made to emphasize the difference of the unscented transform (UT) whichis used in the UKF, and the UKF. The sigma points can be chosen in any way aslong as they capture mean and covariance of the distribution to the second order.This means that the algorithm captures at least the first two terms in the Taylorexpansion, see [12] for the details. The sigma point selection in this work was donewith the sigma point scaling method. The idea behind this selection scheme is tointroduce a scaling parameter ψ which controls the spread of the sigma points.Normally ψ is chosen small to minimize the effect of higher order transformationeffects such as the third order moment, called skewness, when estimating the meanand covariance of the transformed variable.

The following sigma point selection scheme is taken from [20]:

λ = ψ2(L+ κ) − L, (4.4.2a)

χ0 = X, (4.4.2b)

χi = X +√

(L+ λ)P xi, (4.4.2c)

χi+L = X −√

(L+ λ)P xi+L

, (4.4.2d)

Wm0 =

λ

L+ λ, (4.4.2e)

W c0 =

λ

L+ λ+ (1 − ψ2 + β), (4.4.2f)

Wmi =

1

2(L+ λ), (4.4.2g)

W ci = Wm

i . (4.4.2h)

Here√P x

i means the i-th row or i-th column of the matrix square root of P x,depending on whether the matrix square root is defined as a solution to P x = ATAor P x = AAT . Note that the weights here sum to one but that not all weightsare positive. The default values for the scaling parameters, ψ = 10−3, κ = 0 andβ = 2, are optimal for a Gaussian prior.

4.4.2 Algorithm

Before giving the UKF algorithm, the estimation steps of the UT are presentedsince these are central to the UKF algorithm. The UT can be used to estimatethe distribution of a non-linear function, z = F (X). First, the sigma points arechosen, then they are propagated trough F (X). The propagated sigma pointscan then be used to estimate the first two moments z, P z of z, i.e. a Gaussiandistribution for z.

z ≈2L∑

0

Wmi F (χi) (4.4.3a)

P z ≈2L∑

i=0

W ci (F (χi) − X)(F (χi) − X)T (4.4.3b)

4.5 Particle Filter 29

There are several ways of implementing a UKF. If for instance the measure-ment equation or the process equation is linear, the standard Kalman filter mea-surement/time update could be used instead. There is also a choice of usingan augmented version of the UKF, or a simplified version valid for additive pro-cess/measurement noise. The augmented UKF is more general since it deals withnon additive process/measurement noise, and it will be the method of choice inthis thesis.

First, create an augmented state space vector Xak−1|k−1 and augumented co-

variance matrix P a,

Xa = (Xk−1|k−1, E(vk−1), E(ek−1))T , (4.4.4a)

P a =

Pk|k 0 00 Qk 00 0 Rk

. (4.4.4b)

Determine the augmented sigma points χak−1|k−1 = (χX

k−1|k−1, χvk−1, χ

ek−1)T and

the weights Wmi ,W c

i via (4.4.2). Here, χXk−1|k−1 denotes the sigma points drawn

from X . The sigma points are now propogated trough the motion model,

χXk|k−1 = F (χX

k−1|k−1, χvk−1). (4.4.4c)

Compute Xk|k−1, Pk|k−1 with (4.4.3). The sigma points are then propogatedtrough the measurement equation,

χyk|k−1 = H(χX

k|k−1, χek). (4.4.4d)

Now, yk|k−1 and P yk|k−1 are estimated with (4.4.3). The covariance P xy

k|k−1 is

calculated as,

P xyk|k−1 =

2L∑

i=0

W ci (χX

k|k−1 − Xk|k−1)(χyk|k−1 − yk|k−1)T . (4.4.4e)

The estimate Xk|k−1 is now updated with the measurement,

Kk = P xyk|k−1(P y

k|k−1)−1, (4.4.4f)

Xk|k = Xk|k−1 +Kk(y − yk|k−1), (4.4.4g)

Pk|k = Pk|k−1 −KkPyk|k−1Kk

T . (4.4.4h)

4.5 Particle Filter

Unlike the EKF and UKF, the particle filter (PF) does not represent the posteriorPDF by its mean and covariance, i.e. as a Gaussian. The particle filter representsthe PDF with a set of discrete points, called particles, and their correspondingweights. Unlike the UKF, where the sigma particles are chosen in a deterministicway, the PF spreads the particles in a stochastic way. This representation makes

30 Estimation

it possible to approximate every possible type of PDF. The PF does not try to es-timate the probability p(Xk|Y1:k), instead it estimates the conditional probabilityof the whole trajectory of the state p(X1:k|Y1:k), where X1:k = X1, . . . , Xk.

The following presentation draws heavily from [1]. Particle filters are basedon importance sampling and therefore the subject of importance sampling will bebriefly explained.

Importance Sampling

Importance sampling is a tool used for stochastic integration when it is hard todraw samples directly from a distribution π(X), but it is possible to evaluate thedensity. A proposal distribution q(X) is introduced with a support greater or equalto that of π(X). The stochastic integration can, when the number of samples Nis large, be approximated as

I =

∫

Rn

f(X)π(X)

q(X)q(X)dX ≈ 1

N

N∑

i=1

f(X(i))π(X(i)), (4.5.1)

here, X i and ωi, denotes particle and weight i respectivley. The density of π(X)is approximated as

π(X) ≈ 1

N

N∑

i=1

ω(i)δ(X −X(i)), (4.5.2a)

ω(i) =π(X(i))

q(X(i)). (4.5.2b)

If the distribution p(X1:k|Y1:k) is approximated using importance sampling,with a proposal distribution on the form q(X1:k|, Y1:k), the weights are given by

ω(i) =p(X

(i)1:k|Y1:k)

q(X(i)1:k|Y1:k)

. (4.5.3)

Writing this in a sequential way, the weight update becomes

ω(i)k ∝ ω

(i)k−1

p(yk|X(i)k )p(X

(i)k |X(i)

k−1)

q(X(i)k |X(i)

1:k−1, Y1:k). (4.5.4a)

When only considering the previous state and measurement in the proposal dis-tribution, this simplifies to q(Xk|Xk−1, yk), and the weight update is given by

ω(i)k ∝ ω

(i)k−1

p(yk|X(i)k )p(X

(i)k |X(i)

k−1)

q(X(i)k |X(i)

k−1, yk). (4.5.4b)

There are several different types of particle filters, all of them trying to handlethe following two difficulties:


• Sample depletion, which means that the number of samples that con-tribute in the representation of the posterior becomes very small. In theworst case only one sample will represent the whole posterior. This may oc-cur because all the other particles become highly unlikely and have a smallweight assigned to them. This phenomenon can be measured with the num-ber of effective samples Neff , which measures how many of the N samplesthat are actually used in representing p(Xk|Y1:k). It is defined as

Neff =N

1 +N2 Var(ω(i)k|k)

. (4.5.5)

In the ideal case Neff = N and in the worse case Neff = 1.

• The computational complexity increases with the number of samples.To be able to have an algorithm running in real-time it is necessary to havean algorithm that is not to computational demanding, i.e. the number ofsamples cannot be to large. Since the number of particles required to de-scribe a motion increases with the dimension of the state space it becomesincreasingly important to keep a high number of effective samples comparedto the number of samples.

4.5.1 Algorithm

The easiest proposal distribution to use is the transition density p(Xk|Xk−1) andthe algorithm using the transition density as proposal is given below. There areseveral different ways to perform the resampling and estimation stage, the differentoptions are presented after the algorithm.

Algorithm 1 Particle Filter

X(i)k = f(X

(i)k−1, v

(i)k )

ω(i)k = π

(i)k−1p(yk|X(i)

k )

π(i)k =

ω(i)

k∑

N

i=1ω

(i)

k

Xk =Estimate(X(i)k , π

(i)k )

(X(i)k , π

(i)k ) =Resample(X

(i)k , π

(i)k )

4.5.2 Resampling

In the resampling step, the particles are sampled from the estimated distributionp(Xk|X1:k). This means that particles with high weights have a high probability ofbeing sampled and particles with low weights have low probability. Resampling caneither be performed in each time step, without checking if it is needed or not. Thisis called Sequential Importance Sampling (SIS). If resampling is only performedwhen the diversity of the particle set is below some threshold, it is called Sequential

32 Estimation

−2 −1.5 −1 −0.5 0 0.5 1 1.50

0.5

1

1.5

x

p(x)

MAPMV

Figure 4.1: The minumin variance and maximum a posteriori estimates for aMulti-modal distribution. The figure shows that the MV estimate may be a poorchoice for a multi-modal distribution.

Importance Resampling (SIR). For the SIR algorithm, some diversity measure isneeded. The measure that has been used in this thesis is the effective number ofsamples Neff . The definition of Neff (4.5.5), needs a computable approximation,and is approximated as

Neff ≈ 1∑N

i:1 (ω(i)k|k)

2 . (4.5.6)

The threshold for resampling can for instance be selected as Neff <N2 , which is

the threshold that has been in use in this thesis. The threshold value was determinin a empirical way.

4.5.3 Estimation

There are basically two different ways to estimate the state. These are

• minimum variance (MV). The minimum variance estimate is the appropriatechoice in the uni-modal case, uni-modal means that there is just one localmaximum. The MV estimate is

XMVk|k =

∫

Rn

Xkp(Xk|y1:k)dXk, (4.5.7a)

and can be computed from the samples as


XMVk|k ≈

N∑

i=1

ω(i)k X

(i)k . (4.5.7b)

• Maximum a posteriori (MAP). For the case of a multi-modal distribution,the MV estimate may be a poor estimate, see Figure 4.1. In this case, it ismore appropriate to select the most probable value. The MAP estimate is

XMAPk|k = argmaxXk

(p(Xk|y1:k)), (4.5.8a)

and can be computed from the samples as

i′ = argimax(ω(i)k ), (4.5.8b)

XMAPk|k = X i′

k|k. (4.5.8c)

4.5.4 Prior Sampling

The bootstrap particle filter is the most common type of particle filter, and uses theprior as the proposal distribution q(Xk|X i

k−1, yk) = p(Xk|X ik−1). This simplifies

the weight update (4.5.4b) to

ωk = ωk−1p(yk|Xk). (4.5.9)

A drawback of this proposal is that it does not use the current measurement yk inthe proposal. If the true dynamics of the system are well described by the dynamicequation, this is not a problem. However, when the measurements are much moreexact than the dynamics, this is not a suitable proposal. All the other proposalsgiven tries to address this problem by including the current measurement.

4.5.5 Likelihood sampling

In the case when the measurements are much more exact than the dynamic model,the likelihood, p(Xk|Xk−1, yk) is well approximated by the likelihood p(yk|Xk).When using the likelihood as proposal, q(Xk|Xk−1, yk) ∝ p(yk|Xk), and the weightupdate (4.5.4b) simplifies to

ωk = ωk−1p(Xk|Xk−1). (4.5.10)

For the case of additive process noise, this can be easily calculated. In Algorithm2 the particle filter algorithm using the likelihood as proposal density is presented.

There is a problem drawing samples from p(yk|Xk) when the number of statesnx is larger than the dimension of the measurement ny. This is because there hasto be a way of expressing the states in terms of the measurements, i.e. the mea-surement equation has to be invertible. In [10], they solve this by down samplingthe system. Below, a 2D CV model is down sampled a factor two.

34 Estimation

Algorithm 2 Likelihood Sampling

X(i)k = H−1(yk, e

(i))

ω(i)k|k = π

(i)k−1p(X

(i)k |X(i)

k−1)

π(i)k =

ω(i)

k∑

N

i=1ω

(i)

k

Xk = Estimate(X(i)k , π

(i)k )

(X(i)k , π

(i)k ) = Resample(X

(i)k , π

(i)k )

Example 4.2

Consider target tracking using a constant velocity motion model and radar mea-surements in the plane. The state vector is given by X = (x, y, x, y) and themotion is given as

Xk+1 =

1 0 T 00 1 0 T0 0 1 00 0 0 1

Xk + vk. (4.5.11)

Assume the same magnitude σ2 of the process noise in both x and y direction.The discretized covariance is then

Q =

σ2T 3

3 0 σ2T 2

2 0

0 σ2T 3

3 0 σ2T 2

2σ2T 2

2 0 σ2T 0

0 σ2T 2

2 0 σ2T

. (4.5.12)

The measurements are given by

r =√

x2 + y2 + er (4.5.13a)

θ = arctan(y/x) + eθ (4.5.13b)

The sampling time is quite high here, Ts = 10s, and the radar that is usedhere is quite accurate compared to the accuracy of the dynamics. A good proposaldensity would in this case be the likelihood. However, in this case nx = 4 andny = 2. To solve this problem, the system can be down sampled a factor 2. Thedown sampling means that in every iteration of the particle filter, two separatemeasurements will be available. This means that nx = ny and the likelihoodbecomes invertible. Samples can now be drawn using the current measurementsrk, θk and the previous measurements rk−1, θk−1 as


X(i)k ∝

(rk + e(i)r )cos(θk + e

(i)θ )

(rk + e(i)r )sin(θk + e

(i)θ )

(rk+e(i)r )cos(θk+e

(i)

θ)−(rk−1+e(i)

r )cos(θk−1+e(i)

θ)

T(rk+e(i)

r )sin(θk+e(i)

θ)−(rk−1+e(i)

r )sin(θk−1+e(i)

θ)

T

. (4.5.14)

The particles are drawn with different realizations of the measurement noise. Theweights are then calculated using the prior (4.5.10).

4.5.6 Auxiliary Particle filter

The Auxiliary Particle Filter (APF) addresses the problem of not having infor-mation about the latest measurement when propagating the particles. The nameAPF comes from the fact that every particle is given an auxiliary index I that isthe particle index at time k − 1.

Including the index p(Xk, I|Y1:k) can, with the help of Bayes Theorem (4.1.2),be defined as

p(Xk, I|Y1:k) ∝ p(yk|Xk)p(Xk|X(I)k−1)ω

(I)k−1|k−1, (4.5.15)

where Ω(I)k−1|k−1 is the previous weight with index I. The importance density

q(Xk, I|Y1:k) is defined to satisfy

q(Xk, I|Y1:k) ∝ p(yk|µIk)p(Xk|X(I)

k−1)ω(I)k−1|k−1, (4.5.16)

where µ(I)k is a likely value for X

(I)k . Using Bayes Theorem (4.1.2), the proposal

density can be written as

q(Xk, I|Y1:k) = q(I|Y1:k)q(Xk|I, Y1:k). (4.5.17a)

By defining

q(Xk|I, Y1:k) ≡ p(Xk|X(I)k ), (4.5.17b)

it follows from (4.5.16) that

q(I|Y1:k) ∝ p(yk|µ(I)k )ω

(I)k−1|k−1. (4.5.17c)

The particle index i, used as an index at time k is now introduce again since after(4.5.16) is sampled, the index I is not valid. The weights at time k are given by

ω(i)k =

p(Xk, I|Y1:k)

q(Xk, I|Y1:k)∝p(yk|X(i)

k )p(X(i)k |X(Ii)

k−1)ω(Ii)k−1|k−1

q(Xk, I(i)|Y1:k)=p(yk|X(i)

k )

p(yk|µ(Ii)k )

. (4.5.18)

This makes it important that µ(I)k is a probable value for Xk. One can therefore

choose µ(I)k to be the expected value, or the value of a sample of X

(I)k−1. The choice

36 Estimation

of using a propagated sample is an attractive alternative since it does not increasethe number of samples that are used at any point. The problem with using a singel

value, such as a sample or the expected value for µ(I)k is that the weights (4.5.18)

are not upper bounded, e.g. the case when µ(Ii)k is improbable andX

(i)k is probable.

In [11], it is shown that the APF algorithm can be interpreted as a SIR-algorithm.This interpretation makes it clear that the proposal q(Xk, I|Y1:k) should have

heavier tails than the true distribution p(Xk, I|Y1:k), and thus p(yk|µ(I)k ) should

be more diffuse than p(yk|Xk).

A better proposal to p(yk|Xk−1) than using some value µk is given in [3] as

p(yk|Xk−1) ∝∫

p(yk|Xk)p(Xk|Xk−1)dXk. (4.5.19)

Here the hat on the distributions p(yk|Xk), p(Xk|Xk−1) represent distributionswith heavier tails than the true distributions. This can be achieved either via theuse of the t-distribution or by making the distributions more diffuse than the truedistributions. The integration should here be done over the whole state space.This is not possible in a real-time application so this integral is approximated viaMonte Carlo integration

∫

p(yk|Xk)p(Xk|Xk−1)dXk ≈ 1

M

M∑

n=1

p(yk|Xnk ), (4.5.20)

where Xn is a sample drawn from p(Xk|Xk−1). The choice of M is a trade-offbetween accuracy and the computional load, M is here chosen as 5. One mightthink that using the APF would give importance weights with a smaller variancethan those given by the standard PF algorithm. The two propagation rounds couldbe believed to lead to a more uniform distribution of the weights and thereforea lower variance. However, as demonstrated in [3] this is not at all certain. InAlgorithm 3, the APF algorithm is presented.

Algorithm 3 Auxiliary Sampling

µ(I)k = F (X

(I)k−1, v

(I)k )

p(yk|Xk−1) ∝∫

p(yk|µ(I)k )p(µk|X(I)

k−1)dXk

ω(I)k|k−1 = π

(I)k−1|k−1p(yk|Xk−1)

i = Resample(I, ω(I)k|k−1)

X(i)k = F (X

(i)k−1, v

(i)k )

ω(i)k|k =

p(y|X(i)

k)

p(yk|x(Ii)

k−1)

π(i)k|k =

ω(i)

k|k∑

i=1Nω

(i)

k|k

ˆXk|k = Estimate(x, πk|k)

4.6 Interacting Multiple Model 37

4.6 Interacting Multiple Model

The motion of an air target can be characterized into a few different types ofmaneuvers, including speed changes, constant velocity motion and fast maneuvers.One idea is to use this fact and have different motion models describing differenttypes of motion. Then each model can be trimmed to make accurate estimationsfor one type of motion. The combined estimates from the models will probablybe a better estimate than the estimate from any one model describing all motion.The presentation here draws heavily from [4], but also from [14].

The Multiple Model (MM) approach runs several filters in parallel, taking themost probable estimate each time. The problem with a MM approach is thatthe different models do not affect each other and transitions between the differentmodels may give a volatile estimator. This means that all the information availableat each time instant is not used.

The Interacting Multiple Model (IMM) structure treats this problem by intro-ducing interaction between the models. In an IMM constellation, several differentfilters run in parallel, and the transition between these filters are given by a Markovmodel. The matrix Pt contains the transition probabilities and is assumed to beknown. The elements of Pt, p(i,j) are the probability of transition from model ito model j. The IMM scheme can be divided into the following steps: Interac-tion/Mixing, Filter update, Combination and Estimation. For an IMM runningm different models, with estimates

X(i)k−1|k−1, P

(i)k−1|k−1, i = 1 . . .m, (4.6.1)

the interaction stage consists of mixing the estimates from the different modelsaccording to ηij

k−1|k−1. ηij is the conditional probability that the target changed

from the motion described by model i to the motion described by model j and iscalculated as,

Cj =

m∑

i=1

p(i,j)γik−1, j = 1 . . .m, (4.6.2a)

ηijk−1|k−1 =

p(i, j)γik−1

Cj, (4.6.2b)

(4.6.2c)

where the mode probability γik−1, is the probability of the target being described

by motion model i at time k − 1 and Cj is the probability after interaction that

the target is described by model j. Note that the notation X(i)k−1|k−1 used here is

similar to the notation that was used for particles in Section 4.5. However, thesuperscript i addresses a motion model in this context and not a particle.

Then, the m different filter estimates can be updated for filter j = 1 . . .m with

38 Estimation

X(j,0)k−1|k−1, P

(j,0)k−1|k−1 as

x(j,0)k−1|k−1 =

m∑

i=1

X ik−1|k−1η

ijk−1|k−1, (4.6.2d)

D(i, j) = X ik−1|k−1 − X

(j,0)k−1|k−1, (4.6.2e)

P(j,0)k−1|k−1 =

m∑

i=1

ηijk−1|k−1(P i

k−1|k−1 +D(i, j)DT (i, j)). (4.6.2f)

After Interaction, the filter update is performed.

The filters all run to produce the estimates X(i)k|k−1, P

(i)k|k−1 for each filter i =

1 . . .m. Using the estimates, the squared distance dj between the predicted mea-surement and actual measurement for model j is calculated as,

dj = (yk −H(Xjk|k−1))(yk −H(Xj

k|k−1))T . (4.6.2g)

The residual covariance Sjk|k−1 for model j is given by

Sjk|k−1 = H

′

x(Xk|k−1)P jk|k−1H

′

x(Xk|k−1)T +R. (4.6.2h)

Here, the notation H′

x denotes the Jacobian of the measurement model H withrespect to X , see Section 4.3. The IMM is centainly not limited to the use ofEKF filters but to get a concise presentation and to point out the connection with(4.3.2a) in the EKF algorithm this presentation was choosen. The likelihood ∆j ofmotion model j is calculated using the current measurement yk with dimensionalityM as

∆j =1

2πM2 ‖Sj

k|k−1‖e−dj/2. (4.6.2i)

The mode probability at time k for model j γjk, is now calculated as

c =m

∑

j=1

∆jCj , (4.6.2j)

γjk =

∆jCj

c, (4.6.2k)

where c is a normalizing constant. Finally, a combined estimate for the differentfilters is given by,

Xk|k =m

∑

j=1

γjXjk|k, (4.6.2l)

P ik|k =

m∑

j=1

γj(P jk|k + (Xj

k|k − Xk|k)(Xjk|k − Xk|k)T ). (4.6.2m)

The combined estimate is the output of the IMM-filter. There might be some dif-ficulties performing the state combination, these are presented in the next section.

4.6 Interacting Multiple Model 39

4.6.1 State Combination

For an IMM-framework to be successful, the models should describe different typesof motion. For different types of motion, different state representations are needed.

For the IMM-framework to be applied, the states need to be transformed to acommon state representation before the interaction and probability update of theestimate is performed.

Next, two examples demonstrating transformations needed to go between dif-ferent state representations are presented. In Example 4.3, the transformationfrom a 6 dimensional Cartesian representation to a 9 dimensional Cartesian repre-sentation is given and in Example 4.4 a transformation from 4 dimensional Carte-sian representation to a 4 dimensional speed and heading representation is pre-sented.

Example 4.3

An IMM is running two different motion models, one CV-model used for track-ing the target when it is not performing any maneuvers and one 3D-CT modelthat should track during maneuvers. The state vector for the CV model is X1 =(x, y, z, x, y, z) with filter estimate X1, P1. The state vector for the 3D-CT modelis X2 = (x, y, z, x, y, z, x, y, z) with filter estimate X2, P2. For the state interac-tion/combination to be performed in the IMM-filter, a transformation to, and backfrom, a common state representation is needed. Here, the state representation X2

is chosen as the representation for the IMM-filter. This choice means that thereis no transformation needed for the 3D-CT model. For the CV model there is aneed for a transformation between the state representation X1 to X2, and back.A suitable assumption for the accelerations for a CV model are that they are zero(constant velocity). This means that the state estimate is transformed as,

X1,T = (X1, 0, 0, 0)T . (4.6.3a)

The covariance matrix P1 is transformed via

P1,T = A12P1AT12, (4.6.3b)

A12 =δX2

δX1=

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0

. (4.6.3c)

The transformations back are then given by,

X1 = (x1,T , y1,T , z1,T , x1,T , y1,T , x1,T )T , (4.6.3d)

40 Estimation

where x1,T is the x state in state representation X2. The covariance matrix P1,T

is transformed via

P1 = A21P1,TAT21, (4.6.3e)

A21 =δX1

δX2=

1 0 0 0 0 0 0 0 00 1 0 0 0 0 0 0 00 0 1 0 0 0 0 0 00 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 0

. (4.6.3f)

In Example 4.3, both filters used Cartesian coordinates but with a different numberof states. In an IMM-filter it can however be the case that different models usedifferent coordinates, which will be explored next.

Example 4.4

An IMM is running two different motion models, one CV-model used for trackingthe target when it is not performing any maneuvers and one CL model used fortracking during maneuvers. The state vector for the CV model is X1 = (x, y, x, y)T

with filter estimate X1, P1. The state vector for the CL model is X2 = (x, y, s, h)and the filter estimates X2, P2. The choice of state vector representation for theIMM-filter isX1, and the CL model representationX2 has to be transformed to thisrepresentation. First the state vector is transformed to the Cartesian coordinatesin a straight forward fashion

X2,T = (x, y, s cos(h), s sin(h)) = (x, y, x, y). (4.6.4a)

The transformation back to the polar representation is given by

X2 = (x, y,√

v2x + v2

y, arctan vy/vx). (4.6.4b)

The covariance matrix P2 is transformed from the polar representation to theCartesian coordinates via

P2,T = A21P2AT21, (4.6.4c)

A21 =δX1

δX2=

1 0 0 00 1 0 00 0 cos(h) −s sin(h)0 0 sin(h) s cos(h)

. (4.6.4d)

The transformation back to the polar representation after the mixing step is given

4.7 Performance evaluation 41

by the following transformation

P2 = A12P2,TAT12, (4.6.4e)

A12 =δX2

δX1=

1 0 0 00 1 0 00 0 vx

svy

s0 0 − vy

s2vx

s2

. (4.6.4f)

The IMM-structure is suitable for Gaussian estimators. However, in this thesisit is of interest to combine different type of estimators, mainly PF-based estimatorswith EKF/UKF estimators. This type of combination is needed to make use of thePF for the highly maneuverable motion, when the distribution of the current stateis not well approximated by a Gaussian, and an EKF for the non-maneuveringcase. For the APF, the filter estimate is updated in the following way: Mi = γiNparticles are drawn from the distribution p(Xk|Y1:k) of model i. In this way, thenumber of particles are constant and the distribution that the PF draws from isnot forced to be a single Gaussian distribution.

4.7 Performance evaluation

To be able to compare different filtering methods, a performance measure isneeded. There are a many different performance measures available. Here, onlythe two methods used during the work in this thesis will presented.

4.7.1 Root Mean Square Error

The Root Mean Square Error (RMSE) is probably the most commonly used per-formance measure, and can e.g. be used to evaluate the tracking performance overMonte Carlo simulations. There are basically two different options when comput-ing the RMSE, either over all time instances and all Monte Carlo simulations

√

√

√

√

1

N(T − T0)

T∑

k=T0

N∑

i=1

(X ik −Xk)2, (4.7.1)

or just over all Monte Carlo simulations

√

√

√

√

1

N

N∑

i=1

(X ik −Xk)2. (4.7.2)

The first option gives a scalar while the latter option gives a vector as a functionof time. When a target performs different types of maneuvers, it is of interest howthe RMSE depends on the type of maneuver performed. Note that the RMSE has

42 Estimation

the unit of the calculated state, i.e. if the state is given in ms then the RMSE is

also in ms . The combination of different RMSE values is meaningful only if they

are given in the same unit, e.g. the position error given in x,y,z coordinates.Note that to calculate the RMSE, one needs to know the true state. For the

case of simulated data, this is not a problem. For real data, the true state isoften not know, but if the states can be measured accurately the RMSE can becalculated.

4.7.2 Normalized Estimation Error Square

The presentation of the Normalized Estimation Error Square (NEES) done heredraws heavily from [8]. Unlike the RMSE measure which measures the accuracyof the estimate, the NEES measures the consistency of the estimate. The NEESis calculated over nmc Monte Carlo runs as,

nmc∑

i=1

(Xk − X ik)T (P i

k|k)−1(Xk − X ik). (4.7.3)

When X is Gaussian distributed, the NEES becomes χ2 distributed with nx ×nmc

degrees of freedom. A confidence interval of level ξ can then be calculated, and ifthe NEES is within this confidence interval at least ξ% of the time, the estimatoris said to be consistent. If the filtered system is non-linear, the assumption thatx is Gaussian does in general not hold, and then the assumption that the NEESis χ2 does not either hold in general. In this case, one cannot in general calculatesome probability gates for whitch the NEES should be located in. Instead, theapproach done in [23] can be used. Here, they use the fact that if the state errorand covariance are consistent with each other, the NEES of one Monte Carlosimulation at time k should be close to the number of states, nx. For nmc MonteCarlo runs and nx states, the NEES can be normed as,

1

nmcnx

nmc∑

i=1

(Xk − X ik)T (P i

t|t)−1(Xk − X i

k) (4.7.4)

and should be close to 1 if the estimator is consistent.One should note that as for the RMSE, to calculate the NEES one needs to

know the true state.

Chapter 5

Results

It is not an easy task to evaluate two filters against each other. If a filter is goodat coping with one type of motion, it may be bad at dealing with other types ofmotion. It is also of importance how a filter interacts with other filters in an IMMfilter, i.e. the filter should not degrade the performance of non-maneuvering filterswhen the target is non-maneuvering. The evaluation in this thesis was conductedaccording to the following steps:

• Optimize a baseline filter for comparison, evaluate by computing the NEES,and position, speed and heading RMSE.

• Optimize a high-maneuver filter to minimize the maximum errors.

• Evaluate the filter performance on other tracks.

• Compare the position, speed and heading-RMSE of the filter against thebaseline filter.

• Compare the performance of the filter in an IMM against an IMM filter withthe baseline filter by evaluating position, speed and heading RMSE.

First, the different tracks on which evaluation is performed will be presented,after that the simulations and finally the results. All the parameters used for thesimulations can be found in the Appendix. Before the tracks are presented, somecritical discussion about the ability to interpret the results of filters is presented.

5.1 Filter evaluation

It is hard to draw any general conclusions of the performance of a certain filterfrom the performance over just one trajectory. In the results that follow, thereis only one track being considered for the planar case and one for the 3D case.A word of caution has to be given that the results and conclusions that followcan only be said to apply to the scenarios that are presented here. It is thereforeimportant to have simulation data that truly resembles real data. In the planar

43

44 Results

Distance [m] Angle change [deg] at

[

ms2

]

al

[

ms2

]

Description20 000 0 0 0 Straight- 135 60 −3 Left turn- 135 25 −1.25 Left turn3 000 0 0 3 Acceleration- 135 20 −1 Right turn3 000 0 0 3 Acceleration- 135 100 −5 Right turn3 000 0 0 3 Acceleration- 135 80 −4 Left turn- 135 35 −1.75 Right turn- 135 40 −2 Left turn- 135 30 −1.5 Left turn3 000 0 0 3 Acceleration- 135 50 −2.5 Left turn15 000 0 0 0 Straight

Table 5.1: The planar trajectory with a decrease in speed during turns.

case, a trajectory is created that resembles real data to some extent. This could beseen as a weakness and one should perhaps give quite a bit of thought on how goodthe simulated data is, since the important thing is that one gets good performancein a real application. One should however be careful to draw conclusions of thegeneral performance of a filter from the performance on one trajectory, especiallywhen the trajectory is created in such a way that it is closely described by themotion model used in the filter. But in the absence of real data, the best one cando is to try to generate trajectories that are as close to the truth as possible.

A second issue that should be mentioned is the number of Monte Carlo sim-ulations conducted, and the certainty of the results. The different filters that arecompared are simulated with the same trajectory and different realisations of mea-surement noise. In the planar case, for the EKF and UKF, the stochastic part isgiven by the initial value and the 2D measurement noise. The number of MonteCarlo simulations that are needed to get a certain confidence in the results is hardto determine. There is a trade-off between the time it takes to conduct the simu-lations and the certainty of the results. The number of Monte Carlo simulationsused in this thesis is 100 for all scenarios.

5.2 Trajectory 1

This track was first presented in [19], and is two dimensional. It consists of manyfast turns that the filter should handle. During turns, the speed decreases with5% of the transversal acceleration. The trajectory is shown in Figure 5.1 and thedifferent manuevers are shown in Table 5.1.

5.2 Trajectory 1 45

8 8.5 9 9.5 10 10.5 11 11.5

x 104

0

0.5

1

1.5

2

2.5

x 104

x[m]

y[m

]

Figure 5.1: The planar trajectory with a decrease in speed during turns. Thetarget starts in (x, y) = (105, 0) with a velocity of (x, y) = (0, 200)

.

5.2.1 Simulation I

First, the performance of the UKF compared to the EKF was evaluated on Tra-jectory 1 for different standard deviations on the measurement noise to try todetermine if the results were connected to the Signal to Noise Ratio (SNR). Sincethe motion model is linear the possible improvement given by the UKF comparedto the EKF would be given by a better estimation of the measurement PDF. Fordecreasing sample time, the accuracy of linearisation done by the EKF will in-crease and so the possible difference between the two filters should decrease. Theparameters used in this simulation are given in the Appendix.

Results

In Figures 5.2 to 5.4 the speed, heading and position RMSE over time are giventogether with the NEES, for three different sample times. In Table 5.2 the RMSEvalues calculated for the entire track are presented.

Conclusion

Since the target is so far away from the radar, one would not expect large lineari-sation errors and therefore a small Hessian. The SNR is also quite high, both ofthese attributes contribute to a well performing EKF, and also to the fact that

46 Results

0 50 100 150 200 250 300 3500

10

20

30

40

50

60

Time [s]

spee

d R

MS

E [m

/s]

UKF−CVEKF−CV

(a) Speed

0 50 100 150 200 250 300 3500

10

20

30

40

50

60

70

80

Time [s]

head

ing

RM

SE

[deg

]

UKF−CVEKF−CV

(b) Heading

0 50 100 150 200 250 300 350200

220

240

260

280

300

320

340

360

380

400

Time [s]

Pos

ition

RM

SE

[m]

UKF−CVEKF−CV

(c) Position

0 50 100 150 200 250 300 3500

1

2

3

4

5

6

Time [s]

NE

ES

UKF−CVEKF−CV

(d) NEES

Figure 5.2: Performance evaluation of a UKF and an EKF running a CV-modelon Trajectory 1 with Ts = 10 and measurement noise R1

k.

Ts Filter Position RMSE (x,y)[m] Speed RMSE [ ms

] Heading RMSE [deg]

10 UKF-CV 66.8787, 259.0788 30.6071 25.880910 EKF-CV 67.4215, 254.0399 27.6188 26.05525 UKF-CV 63.9028, 241.4798 39.7693 22.96415 EKF-CV 65.4078, 252.8558 42.1354 22.04752 UKF-CV 57.4741, 202.7473 36.6339 21.64112 EKF-CV 58.1037, 204.5150 37.0563 22.6717

Table 5.2: RMSE values calculated for Trajectory 1.

5.2 Trajectory 1 47

0 50 100 150 200 250 300 3500

20

40

60

80

100

120

140

160

Time [s]

spee

d R

MS

E [m

/s]

UKF−CVEKF−CV

(a) Speed

0 50 100 150 200 250 300 3500

10

20

30

40

50

60

70

Time [s]

head

ing

RM

SE

[deg

]

UKF−CVEKF−CV

(b) Heading

0 50 100 150 200 250 300 35050

100

150

200

250

300

350

400

450

500

Time [s]

Pos

ition

RM

SE

[m]

UKF−CVEKF−CV

(c) Position

0 50 100 150 200 250 300 3500

1

2

3

4

5

6

7

Time [s]

NE

ES

UKF−CVEKF−CV

(d) NEES


k.

48 Results

0 50 100 150 200 250 300 3500

20

40

60

80

100

120

140

160

180

Time [s]

spee

d R

MS

E [m

/s]

UKF−CVEKF−CV

(a) Speed

0 50 100 150 200 250 300 3500

20

40

60

80

100

120

Time [s]

head

ing

RM

SE

[deg

]

UKF−CVEKF−CV

(b) Heading

0 50 100 150 200 250 300 3500

50

100

150

200

250

300

350

400

450

500

Time [s]

Pos

ition

RM

SE

[m]

UKF−CVEKF−CV

(c) Position

0 50 100 150 200 250 300 3500

1

2

3

4

5

6

7

8

9

Time [s]

NE

ES

UKF−CVEKF−CV

(d) NEES


k.

5.2 Trajectory 1 49

there is not a substantial improvement with the UKF, compared to the EKF. Thelarge peaks in the NEES occur at the same time as there is a large heading error,which occur when the target is performing a turn. The large heading errors inthis case are clearly due to the fact that the model assumes a CV-motion, whichdoes not capture the dynamics of a turn correctly. The large peak in the NEESis due to the fact that the estimated covariance during turns is small comparedto how much the estimator is varying. The opposite goes for the straights in thebeginning and end of the track. Here the estimated covariance is too large due tothe large process noise. The NEES indicates, via the peaks in NEES greater than1, that the estimates given by the filters are not consistent during turns. Since aCV-model cannot describe the turns, the filter will trust the measurement more,and the measurement noise will have a large impact on the estimator.

Since the UKF should give a more accurate approximation of the measurementmodel its estimator should give more consistent estimates in general, and especiallyduring turns. In the case of a ten second sampling time, the process noise of the twoestimators becomes very large, and therefore dominates the covariance estimates.This explains why the UKF does not give a more consistent estimator in this case.Note that since the motion model is linear, the speed and heading estimates forthe two filters should show similar performance. The possible advantage of theUKF is due to a better approximation of the measurement model, which in thiscase only is influenced by the position states.

To summarise for this scenario, the use of an UKF does not seem to consider-ably increase the tracking performance compared to an EKF.

5.2.2 Simulation II

In this simulation, the benchmark filter, a CV-EKF, is compared to the perfor-mance of a curvilinear motion model running in an Auxiliary Particle Filter. Thecurvilinear motion model should be able to better capture the dynamics of thetarget during turns. Since SMC methods do not describe Gaussian distributions,it is hard to say anything about the consistency of the estimator using the NEESperformance measurement and therefore only the RMSE values are calculated inthis simulation. It should be mentioned that to increase the performance of theAPF filter for a long sampling time, Ts ≥ 5, the motion model is simulated twicefor half the sampling time, with different noise realisations in the different periods.This way, the filter recovered much faster after a large estimation error.

Results

The results are presented in Figures 5.5 to 5.7 and in Table 5.3, all filters areevaluated over 100 MC-runs.

Conclusions

There is clearly a significant improvement in the performance using the APF-CLfilter, compared to the EKF-CV, for this trajectory. In Table 5.3 the RMSE valuescomputed for the trajectory give a 40 − 70% improvement for the APF-CL filter

50 Results

0 50 100 150 200 250 300 3500

10

20

30

40

50

60

Time [s]

spee

d R

MS

E [m

/s]

APF−CLEKF−CV

(a) Speed

0 50 100 150 200 250 300 3500

10

20

30

40

50

60

70

80

Time [s]

head

ing

RM

SE

[deg

]

APF−CLEKF−CV

(b) Heading

0 50 100 150 200 250 300 35050

100

150

200

250

300

350

Time [s]

Pos

ition

RM

SE

[m]

APF−CLEKF−CV

(c) Position

Figure 5.5: Performance evaluation of an EKF running a CV-model and an APFrunning a CL-model on Trajectory 1 with Ts = 10 and measurement noise R1

k.

Ts Rk Speed improvement [%] Heading improvement [%]10 R1

k 44.73 12.9210 R2

k 56.66 11.3510 R3

k 62.06 11.665 R1

k 67.71 21.505 R2

k 69.78 39.855 R3

k 65.43 11.132 R1

k 66.08 20.862 R2

k 68.66 33.892 R3

k 67.36 12.83

Table 5.3: RMSE improvement of an APF-CL compared to an EKF-CV on Tra-jectory 1. The results are given for different levels off measurement noise Rk, R1

k

being the smallest and R3k the largest. A positive result indicates an improvement

compared to the EKF-CV and a negative results a decline.

5.2 Trajectory 1 51

0 50 100 150 200 250 300 3500

20

40

60

80

100

120

140

160

Time [s]

spee

d R

MS

E [m

/s]

APF−CLEKF−CV

(a) Speed

0 50 100 150 200 250 300 3500

10

20

30

40

50

60

70

Time [s]

head

ing

RM

SE

[deg

]

APF−CLEKF−CV

(b) Heading

0 50 100 150 200 250 300 35050

100

150

200

250

300

350

400

450

500

Time [s]

Pos

ition

RM

SE

[m]

APF−CLEKF−CV

(c) Position


k.

52 Results

0 50 100 150 200 250 300 3500

20

40

60

80

100

120

140

160

180

Time [s]

spee

d R

MS

E [m

/s]

APF−CLEKF−CV

(a) Speed

0 50 100 150 200 250 300 3500

20

40

60

80

100

120

Time [s]

head

ing

RM

SE

[deg

]

APF−CLEKF−CV

(b) Heading

0 50 100 150 200 250 300 3500

50

100

150

200

250

300

350

400

450

500

Time [s]

Pos

ition

RM

SE

[m]

APF−CLEKF−CV

(c) Position


k.

5.3 Trajectory 2 53

5.56

6.57

7.58

8.5

x 104

0

1

2

3

4

x 104

1000

1500

2000

2500

3000

3500

4000

4500

x[m]y[m]

z[m

]

Figure 5.8: The 3D trajectory, the trajectory starts at an height of 1450m andfinishes of at an height of 4500m.

compared to the EKF-CV filter. For these results to be complete, an evaluation ofan APF-CL filter running in an IMM together with a CV filter should be performedand compared to the results from Figure 5.2. This evaluation in needed to see thatcombining the different filters does not degenerate the performance. However, thisexperiment was not done and is left as future work.

5.3 Trajectory 2

This is a track with 3D-motion that was given to the author from Prof. FredrikGustafsson. It is not known to the author exactly how this track was created,but it is supposed to closely resemble a real flight trajectory. It is not possible topresent all the simulations that have been done performed and some limitationshave to be made. One of these limitations is that the results from the use ofthe variable turn-model is not presented here. This is because when this filter wasevaluated, it did not give any significant improvement in performance compared tothe coordinated turn model. However, the variable turn model is significantly morecomplex. In a particle filter running several thousands of particles, this increasein functional complexity makes the filter much more computationally demanding.The following simulations are therefore not presented for the variable turn model.

In Figure 5.8, a three dimensional plot of the trajectory is presented.

54 Results

0 20 40 60 80 100 120 140 160 180 200260

280

300

320

340

360

380

400

420

440

460

Time [s]

Spe

ed [m

/s]

(a) Speed

0 20 40 60 80 100 120 140 160 180 200−20

0

20

40

60

80

100

120

140

160

180

Time [s]

Hea

ding

[deg

]

(b) Heading

0 20 40 60 80 100 120 140 160 180 200−5

0

5

10

15

20

25

30

35

40

45

Time [s]

Incl

inat

ion

angl

e [d

eg]

(c) Inclination angle

Figure 5.9: Horizontal speed, heading and inclination angle of Trajectory 2 plottedover time.

5.3 Trajectory 2 55

0 20 40 60 80 100 120 140 160 1800

10

20

30

40

50

60

70

80

90

Time [s]

spee

d R

MS

E [m

/s]

APF−CTEKF−CV

(a) Speed

0 20 40 60 80 100 120 140 160 1800

5

10

15

20

25

30

35

40

45

Time [s]

head

ing

RM

SE

[deg

]

APF−CTEKF−CV

(b) Heading

0 20 40 60 80 100 120 140 160 1800

100

200

300

400

500

600

Time [s]

Pos

ition

RM

SE

[m]

APF−CTEKF−CV

(c) Position

Figure 5.10: Performance evaluation of an EKF-CV filter and an APF-CT filteron Trajectory 2 with Ts = 10 and measurement noise R1

k.

5.3.1 Simulation I

In this simulation, the performance of a Auxiliary Particle Filter running a 3DCoordinate-Turn model (APF-CT) is compared to that of an Extended KalmanFilter running a 3D CV-model (EKF-CV), with isotropic process noise (isotropicnoise has the same standard deviations in all directions).

Results

Each filter was evaluated over 100 Monte Carlo runs. The results are presented inFigures 5.10 to 5.12, and in Table 5.4.

Conclusions

From the results given here, one can see that during the turn conducted around60 − 70s in trajectory 2, see Figure 5.9, the APF-CT filter outperforms the EKF-CV filter. The APF-CT model performs well when the motion is performed inthe horizontal plane. The filter has problems dealing with a fast ascent/descent.Another interesting feature that can be seen in Table 5.4, is that the particlefilter seems to drop in performance, compared to the EKF, for decreasing SNR.This should not be the case, and does not intuitively add up with the behaviour

56 Results

0 20 40 60 80 100 120 140 160 1800

20

40

60

80

100

120

Time [s]

spee

d R

MS

E [m

/s]

APF−CTEKF−CV

(a) Speed

0 20 40 60 80 100 120 140 160 1800

5

10

15

20

25

30

35

Time [s]

head

ing

RM

SE

[deg

]

APF−CTEKF−CV

(b) Heading

0 20 40 60 80 100 120 140 160 1800

100

200

300

400

500

600

700

Time [s]

Pos

ition

RM

SE

[m]

APF−CTEKF−CV

(c) Position


k.


k -9.05 29.2410 R2

k -22.83 30.1710 R3

k -12.46 24.955 R1

k -5.10 20.215 R2

k -24.62 18.805 R3

k -17.79 14.012 R1

k -11.04 11.122 R2

k -10.86 10.412 R3

k -30.90 5.2989

Table 5.4: RMSE improvement compared for the APF-CT compared to the EKF-CV on Trajectory 2. The results are given for different levels on the measurementnoise Rk, R1

k being the smallest and R3k the largest. A positive result indicates an

improvement compared to the EKF-CV and a negative results a decline.

5.3 Trajectory 2 57

0 20 40 60 80 100 120 140 160 1800

10

20

30

40

50

60

70

80

90

Time [s]

spee

d R

MS

E [m

/s]

APF−CTEKF−CV

(a) Speed

0 20 40 60 80 100 120 140 160 1800

5

10

15

20

25

30

35

40

Time [s]

head

ing

RM

SE

[deg

]

APF−CTEKF−CV

(b) Heading

0 20 40 60 80 100 120 140 160 1800

100

200

300

400

500

600

700

Time [s]

Pos

ition

RM

SE

[m]

APF−CTEKF−CV

(c) Position


k.

58 Results

Ts Filter Position RMSE (x,y,z)[m] Speed RMSE [ ms

] Heading RMSE [deg]

10 APF-3DCT 80.8876,163.2746,333.0769 38.2366 8.563810 EKF-3DCV 82.6490,193.8306,327.0052 34.1509 13.09945 APF-3DCT 78.7477,147.2470,349.7306 23.5180 7.17975 EKF-3DCV 79.4655,176.5592,253.2163 28.5696 11.30532 APF-3DCT 66.6036,115.5998,240.2547 23.9779 7.78902 EKF-3DCV 71.8577,134.5735,210.4131 24.3677 9.5782

Table 5.5: RMSE values calculated for Trajectory 2 for the case with measurementnoise covariance R1

k.

seen in Table 5.3. The explanation to this is that when the measurement noiseincreases, this also indirectly means that there is a need for more particles to coverthe distribution. Since the number of particles is held constant for all three levelsof measurement noise, this is a probable the cause of the degeneration. Anotherreason for the APF drop in performance, compared to the EKF, could perhapsalso be coupled to the fact that an APF was used. As showed in [3], the APFmay not outperform the PF in the low SNR cases. It would be interesting tocompare the performance of the PF against that of the APF on trajectory 2. Thisinvestigation is left as future work.

5.3.2 Simulation II

Here, two IMM filters are compared. The first IMM runs with two EKFs, both witha CV-model, one with small process noise and one with large process noise andis denoted IMM-CV. The second IMM combines an EKF running a CV-modelwith small process noise and an APF running a 3DCT-model and is denotedIMM-CT. Important in an IMM is that the models do not interact in a way thatdegrades the performance. In this case, this means that the filters responsible fordealing with the fast maneuvers (FM), i.e. the CV with large process noise and the3DCT model, do not degrade the performance of the small noise CV-model duringstraight line motion. The interaction in the IMM is measured with the probabilityof the filter dealing with FM, i.e. the probability of the FM filter should be closeto 1 during maneuvers and 0 during straight line motion. The IMM interactionwill, together with the speed, heading and position RMSE be used to compare theperformance of the filters. The parameters used in this simulation can be foundin the Appendix. The heading and speed RMSE are calculated in the horizontal(x, y)-plane.

Results

Each filter was evaluated over 100 Monte Carlo runs. The results are presented inFigures 5.13 to 5.15. Here, the actual mode is also given, corresponding to when afast maneuver occurs, not including linear accelerations. In Table 5.5 the RMSEvalues for the entire track is given and in Table 5.6 the RMSE improvement of theAPF-CT filter, compared to the EKF-CV filter, for different SNR is given.

5.3 Trajectory 2 59

0 20 40 60 80 100 120 140 160 1800

20

40

60

80

100

120

140

Time [s]

spee

d R

MS

E [m

/s]

IMM−CTIMM−CV

(a) Speed

0 20 40 60 80 100 120 140 160 1800

5

10

15

20

25

30

35

40

Time [s]

head

ing

RM

SE

[deg

]

IMM−CTIMM−CV

(b) Heading

0 20 40 60 80 100 120 140 160 1800

100

200

300

400

500

600

700

800

Time [s]

Pos

ition

RM

SE

[m]

IMM−CTIMM−CV

(c) Position

0 20 40 60 80 100 120 140 160 1800

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Time [s]

Pro

babi

lity

IMM−CTIMM−CVFM probability

(d) Mode Probability

Figure 5.13: Performance evaluation for an IMM using an APF-CT as FM-filter,compared to using an EKF-CV as FM-filter on Trajectory 2. The measurementnoise covariance is R1

k and Ts = 10. The actual mode given in red corresponds towhen the fast maneuvers occur, not including linear acceleration.


k -11.96 34.6210 R2

k -16.20 39.5410 R3

k -8.67 49.335 R1

k 17.68 36.495 R2

k -3.15 14.805 R3

k -1.81 29.212 R1

k 1.60 18.682 R2

k 6.86 24.642 R3

k -8.58 19.80

Table 5.6: RMSE improvement for an IMM using an APF-CT as FM filter com-pared, to using an EKF-CV on Trajectory 2. The results are given for differentlevels off measurement noise Rk, R1

k being the smallest and R3k the largest. A pos-

itive result indicates an improvement compared to the EKF-CV and a negativeresults a decline.

60 Results

0 20 40 60 80 100 120 140 160 1800

10

20

30

40

50

60

70

80

90

Time [s]

spee

d R

MS

E [m

/s]

IMM−CTIMM−CV

(a) Speed

0 20 40 60 80 100 120 140 160 1800

5

10

15

20

25

30

35

Time [s]

head

ing

RM

SE

[deg

]

IMM−CTIMM−CV

(b) Heading

0 20 40 60 80 100 120 140 160 1800

100

200

300

400

500

600

700

800

Time [s]

Pos

ition

RM

SE

[m]

IMM−CTIMM−CV

(c) Position

0 20 40 60 80 100 120 140 160 1800

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Time [s]

Pro

babi

lity





5.3 Trajectory 2 61

0 20 40 60 80 100 120 140 160 1800

10

20

30

40

50

60

70

80

90

100

Time [s]

spee

d R

MS

E [m

/s]

IMM−CTIMM−CV

(a) Speed

0 20 40 60 80 100 120 140 160 1800

5

10

15

20

25

30

Time [s]

head

ing

RM

SE

[deg

]

IMM−CTIMM−CV

(b) Heading

0 20 40 60 80 100 120 140 160 1800

100

200

300

400

500

600

700

800

Time [s]

Pos

ition

RM

SE

[m]

IMM−CTIMM−CV

(c) Position

0 20 40 60 80 100 120 140 160 1800

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Time [s]

Pro

babi

lity





62 Results

5.3.3 Conclusions

A first observation is that the speed and heading estimates seem to be a bit moreaccurate with the IMM-CT, except during the rapid ascent which takes placebetween 130 and155s into the simulation, see Figure 5.9. If one considers themode probabilities, it is clear that for the IMM-CT, during the ascent the CTfilter obtains a lower probability during than the CV filter with large process noisein the IMM-CV model. This is due to the fact that the 3DCT model has a hardtime describing this motion. This fact also leads to the large position error inthe end. Here the slow maneuver filter has a hard time keeping up without theinteraction of the FM filter.

One would think that increasing the process noise in the up and downwarddirection for the 3DCT-filter would improve the quality of the estimate. However,when increasing the noise, the curse of dimensionality affects the problem, sincethe state vector is 9-dimensional. An intuitive view of the problem is to considerthat the particles should cover the probable parts of the state space. When thesampling time increases, the state space to cover also increases and when thedimensionality of the state space is high, combined with a high sampling time, thenumber of particles needed increases rapidly.

For a real-time application, increasing the number of particles towards infinityis not a feasible solution. The standard solution when using the particle filter inhigh-dimensional cases, is using the Marginalized Particle Filter (MPF), see [18].The key idea behind the MPF is to marginalize out states that are linear, giventhe non-linear states, and run a KF on the conditionally linear states. The MPFwould be the way to go if it was not for the coupling in w. Since the positionstates are measured non-linearly, and w is calculated from both the velocity statesand the acceleration states, it is not possible to marginalize out any of the states.

The 3D-CT model gives a decreased tracking performance on rapid ascent,however it does improve the performance for the fast turn. The process noise ofthe 3D-CT model is modelled in body coordinates, and the distribution of thisnoise is calculated with the help of the current state of the particles. The fact thatprocess noise will be distributed with the state of the particles will make the filtermore dependent on the previous estimates. This increases the uncertainty of theestimates, and the filter will have a longer recovery time after a large estimationerror. The CV-model is instead uncoupled in the x, y, z-components, while thecoordinated turn model is coupled via the turn rate w.

The fact that the speed RMSE is calculated in the horizontal plane is perhapsnot fair. In an uncoupled motion model like the CV, the speed in each componentis uncoupled, but in the 3DCT-model the speed is coupled over the different states.This means that, e.g. overestimating the speed in the vertical component couldlead to an underestimation of the speed in the horizontal plane. This shows as alarge speed RMSE in the horizontal plane that is really due to a bad estimation ofthe inclination angle. The very high peak in RMSE in Figures 5.13 to 5.15 around130 − 155s into the simulation is explained by this.

Chapter 6

Conclusions and Future

Work

The thesis is concluded with a summary of the observations and results that havebeen obtained. Finally, the authors thoughts on how to proceed with the workdone in this thesis are presented.

6.1 Conclusions

In this thesis, different filters for tracking highly maneuvering targets observed bya radar has been designed and evaluated, special interest has been paid the use ofan Auxiliary Particle Filter (APF). The case of low sampling rates, Ts ≤ 10s, wasalso of special interest.

The implementation was done in Matlab and the evaluation was done by themeans of Monte Carlo simulations, with RMSE and NEES values calculated forthe different filters. Some of the filters were implemented in a Interacting MultipleModels (IMM) filter and the different IMM filters were also evaluated. A way toincorporate a particle filter in an IMM was also developed.

The results show that the use of a APF for tracking highly maneuvering targetsgives substantial improvement when tracking in the horizontal plane. However,for the 3D-scenarios used in this thesis, the APF did not show any substantialimprovement of the tracking performance compared to the use of less computa-tionally demanding methods such as the Extended Kalman Filter.

When tracking highly maneuverable targets, one wants a coupling between theCartesian coordinates to be able to give a good description of maneuvers. Accu-rate 3D-models, that give a physically correct description of all the possible fastmaneuvers that can take place from one time to another, have a high state vectordimensionality (i.e. it is not enough with position and velocity states). Whenthe number of states used by the filter increases, using a good proposal density ina particle filter becomes increasingly important for a real time application. Thestandard way to apply SMC-methods in high dimensional state space is to use the

63

64 Conclusions and Future Work

Marginalized Particle Filter (MPF). However, when there is a non-linear couplingbetween the different coordinates it is not possible to use a technique like the MPF.

The attempt performed in this thesis to obtain a good proposal distributionwas to use an APF. When using the standard variant of the APF, where somelikely sample is used to get a good proposal density, the variance of the weights wasfound to not be upper bounded. To solve this problem, the ideas presented in [11]were used. Simulation results do not show a vast improvement, compared to usinga less computationally demanding EKF running a CV-model with large processnoise. A possible way to improve performance would be to limit the proposaldensity to the planar case, and to marginalize out the z and z from the filter. Thisgives a physically inaccurate model, but would probably improve the performanceof the APF. In [7] they report good performance from the 3DCT model runningjust 600 particles, with a sample time of 1s. The authors use an additive isotropicprocess noise, instead of the body oriented process noise used in this thesis. Futher,the measurements are not given by a radar, but by the positions states directlywith isotropic measurement noise. Since they compare an IMM running differentfilters, but for the same motion models, and evaluation is performed using differenttrajectories, it is hard to directly compare to the result in this thesis.

Tracking highly maneuverable targets over long sampling times has shown to bea really hard problem because of the lack of measurements. Having an accuratephysical model can improve tracking performance. However, since only range,azimuth and elevation is measured in this work, and the number of possible routesto the same point increases with the sampling time, it becomes increasingly hardto say anything about the states not being measured. In these scenarios, theuncertainty given from the motion models becomes so large, that the best onecan do is to rely on the measurements as much as possible. The situation wouldhowever be different if the prediction problem had been investigated instead of thefilter problem. In a high SNR case, most filters will work great with a large processnoise. For the prediction problem the model is more important and thereby alsothe potential gain of using the particle filter.

6.2 Future work

The use of SMC-methods in SAAB’s tracker for tracking highly maneuverable tar-gets have the potential to give an improvement to the tracking performance. Theauthor does however have doubts if these methods would be practically useful inthe case of high sampling time. There is also a trade-off between computationalcomplexity, and tracking accuracy. Here, the question is if the possible gain ofusing SMC-methods overweighs the potential cost of increased computational de-mands. Modern radars give quite accurate measurements (and they will probablyonly become more accurate in the future), and the EKF performs quite well underthese conditions. Hence, there is no need to change the filter for fast maneuversquite yet, at least not with a particle filter running in 3D. However, there is morework to be done, and more possibilities to explore before any definitive conclu-sion can be made. Other non-linear filtering methods such as the UKF perhaps

6.2 Future work 65

deserves more attention. Maybe not for the FM filter running a linear model, asexplored in this thesis, where the only non-linearity comes from the measurement,but for more accurate non-linear models. Here are a few points to show whatshould be explored in the future:

• The idea to down-sample the measurement equation of the system as pre-sented in [10], and to do optimal sampling with the linearized likelihood,seems like a promising idea. It should be a more consistent estimator overthe large variety of possible SNRs that are due to different sampling periods.

• Try to use decoupled filters for the planar and vertical motion.

• Some more thought has to be given to modelling the process noise in bodycoordinates. When doing this instead of using isotropic noise, one couldthink of it as increasing the impact of the previous state estimate to thepresent. This should give improved results in the case when the previousstate estimate was accurate, but it could also mean that the filter will havea harder time of recovering after a large estimation error.

• The SMC-methods dependency on initiation conditions must be evaluated. Ifa method is robust against a inaccurate initiation, a track could be initiatedwith a few measurements, i.e. if the method is less robust it would requiera longer initiation.

• Evaluate the combination of an SMC-method with an EKF in an IMM-framework. The properties of performing the combination this way has tobe thoroughly explored before it could see any active use. Since all theweights have to be made equal after sampling is done from the differentfilters, and thereby the prior distribution knowledge is destroyed, this maynot be the best solution to this problem. The drawbacks of the combinationdone with the EKF and SMC-method become even more apparent whenusing an APF as the SMC-method of choice. In the APF, the first stageweights are calculated from the previous second stage weights, and these aredestroyed with the proposed IMM interaction. An idea to the solution ofthe problem would be to try to incorporate the probabilities of the differentdistributions given by the IMM, as well as the probabilities of the previousweights, so that one does not loss the prior information with each IMMinteraction.

• Extend the IMM structure to incorporate the other models used in SAAB’stracker, to see that they do not interact in a way that degenerates the per-formance.

• Develop some method to use so that the SMC-method only runs when it isneeded. Since much of the time, aircrafts are non-maneuvering, and SAAB’stracker can track several hundreds of targets at the same time, it would be awaste of resources to run an SMC-filter for all targets. There should perhapsbe some logic rules that tell the tracker when to use the SMC-methods.

66 Conclusions and Future Work

• Try to optimize the implementation of the different algorithms to evaluatethe increment in computational cost of running an IMM with a particle filter,compared to an IMM running with just EKF filters.

• Evaluate the possible use of a Filter bank. A possibility that could give quitegood results is to use the linear structure of the motion model in the casewhen the turn rate w is known. One could therefore implement a filter bankrunning a range of different w values.

Bibliography

[1] M. Sanjeev Arulampalam, S. Maskell, N. Gordon, and T. Clapp. A tutorialon particle filters for online nonlinear/non-gaussian bayesian tracking. IEEETransactions on signal processing, 2002.

[2] Y. Bar-Shalom and X. R. Li. Multitarget - Multisensor Tracking. ArtechHouse, 1995.

[3] D. Barber, A. T. Cemgil, and S. Chiappa. Bayesian Time Series Models.Cambridge University Press, 2011.

[4] S. Blackman and R. Popoli. Design and Analysis of Modern Tracking Systems.Artech House, 1999.

[5] M.F. Bugallo, Xu Shanshan, and P.M. Djuric. Comparison of EKF- and PF-based methods in tracking maneuvering targets. In Aerospace Conference,2006 IEEE, page 6 pp., 0-0 2006.

[6] P. Christensen. Stelkroppsmekanik. Bokakademin, LiU, 2010.

[7] P. H. Foo and G.W. Ng. Combining IMM method with particle filters for 3Dmaneuvering target tracking. In Information Fusion, 2007 10th InternationalConference on, pages 1 –8, july 2007.

[8] K. Granström. Loop detection and extended target tracking using laser data.PhD thesis, Linköping UniversityLinköping University, Automatic Control,The Institute of Technology, 2011.

[9] F. Gustafsson. Statistical Sensor Fusion. Studentlitteratur, 2010.

[10] F. Gustafsson, S. Saha, and U. Orguner. Non-linear filtering based on obser-vations from gaussian processes. In Aerospace Conference, 2011 IEEE, pages1 –6, march 2011.

[11] A.M. Johansen and A. Doucet. A note on Auxiliary Particle Filters. Statisticsand Probability Letters, 2008.

[12] S.J. Julier. The scaled unscented transformation. In American Control Con-ference, 2002. Proceedings of the 2002, volume 6, pages 4555 – 4559 vol.6,2002.

67

68 Bibliography

[13] R. Karlsson and N. Bergman. Auxiliary Particle Filters for tracking a ma-neuvering target. In Proceedings of the 39th IEEE Conference on Decisionand Control, pages 3891–3895, 2000.

[14] R. G. Karlsson. Particle Filtering for Positioning and Tracking Applications.PhD thesis, Linköping UniversityLinköping University, The Institute of Tech-nology, Automatic Control, 2005.

[15] X. Rong Li and V. Jilkov. Survey of maneuvering target tracking - part 1:Dynamic models. IEEE Transactions on aerospace and electronic systems,2003.

[16] B.R. Mahafza and A.Z. Elsherbeni. MATLAB simulations for radar systemsdesign. Electrical engineering textbook series. CRC Press/Chapman & Hall,2004.

[17] M. R. Morelande and N. J. Gordon. Target tracking through a coordi-nated turn. In Acoustics, Speech, and Signal Processing, 2005. Proceedings.(ICASSP ’05). IEEE International Conference on, volume 4, pages iv/21 –iv/24 Vol. 4, march 2005.

[18] T. Schön, F. Gustafsson, and P-J. Nordlund. Marginalized particle filters formixed linear nonlinear state-space models. IEEE Trans. on Signal Processing,53:2279–2289, 2005.

[19] E. Sviestins. Nonlinear filtering applied to tracking of rapidly turning targets.Unpublished report, 1994.

[20] E.A. Wan and R. Van Der Merwe. The unscented kalman filter for nonlinearestimation. In Adaptive Systems for Signal Processing, Communications, andControl Symposium 2000. AS-SPCC. The IEEE 2000, pages 153 –158, 2000.

[21] G. A. Watson and W. D. Blair. IMM algorithm for tracking targets thatmaneuver through coordinated turns. Signal and Data Processing of SmallTargets, 1698(1):236–247, 1992.

[22] R. Zhan and J. Wan. Passive maneuvering target tracking using 3D constant-turn model. In Radar, 2006 IEEE Conference on, page 8 pp., april 2006.

[23] Z. Zhanlue, C. Huimin, C. Genshe, K. Chiman, and X. Rong Li. Comparisonof several ballistic target tracking filters. In American Control Conference,2006, page 6 pp., june 2006.

Appendix A

Appendix

A.1 3D Measurement

The Jacobians for the 3D radar measurements are given here. The measurementequations are repeated for convenience,

R =√

x2k + y2

k + z2k + er, (A.1.1a)

α = arctan 2(yk, xk) + eα, (A.1.1b)

ǫ = arctan 2(zk,√

x2k + y2

k) + eǫ, (A.1.1c)

or in a general form

y = H(Xk, e), (A.1.1d)

where X = (x, y, z, x, y, z)T , y is the measurement and e is measurement noise.The Jacobians of the measurement equation, needed by an EKF filter is

dHdX

(Xk) =

xk√x2

k+y2

k+z2

k

yk√x2

k+y2

k+z2

k

zk√x2

k+y2

k+z2

k

0 0 0

−yk

x2k

+y2k

xk

x2k

+y2k

0 0 0 0

−xkzk√x2

k+y2

k(x2

k+y2

k+z2

k)

−ykzk√x2

k+y2

k(x2

k+y2

k+z2

k)

√x2

k+y2

k

x2k

+y2k

+z2k

0 0 0

.

(A.1.2a)

dH

de(Xk|k−1) =

1 0 00 1 00 0 1

(A.1.2b)

69

70 Appendix

Parameter ValueTs 10, 5, 2Qk diag(800, 800, 800)Rk diag(502, 0.152)P0 diag(100, 100, 10, 10)

(a) First setup


(b) Second setup


(c) Third setup

Parameter Valueα 10−3

β 2κ 0

(d) UKF parameters

Table A.1: Parameter values for EKF-CV and UKF-CV filters

A.2 Simulation Parameters

The parameters used for the simulations are given here. All filters are initializedwith a state vector that is drawn from a Gaussian distribution with the true valueas the mean value and initial covariance P0 as covariance.

A.2.1 Trajectory 1

EKF/UKF parameters

The simulation parameters used for Trajectory 1 is given in this section. Here, boththe UKF and EKF filters are running the same model and use the same parameters.The main difference is that the UKF algorithm has a couple of more parametersthat need to be chosen. The parameters used in the different setups are given inTable A.1. The UKF specific parameters are the same for the different setups.One should note that the values on the UKF parameters are those suggested in[20]. Tuning was performed but these values gave satisfactory results.

Auxiliary Particle Filter parameters

The Auxiliary Particle filter running a Curvilinear motion model (APF-CL) usesthe parameters given in Table A.2. Note that the process noise is added to thesystem before it is propagated through the motion-model.

A.2.2 Trajectory 2

For Trajectory 2, two simulations were presented. The first simulation evaluatesan EKF, running a Constant Velocity model (EKF-CV), against an AuxiliaryParticle Filter, running a 3D Coordinated Turn model (APF-CT). In the secondsimulation, each filter is placed in an IMM structure together with a CV modelwith small process noise. The parameters used for the simulations are given in

A.2 Simulation Parameters 71

Parameter ValueTs 10, 5, 2w 0.04at 5Rk diag(502, 0.152)P0 diag(100, 100, 10, 0.1, 0.01, 0.01)N 10000

Table A.2: Parameter values for the APF-CL filter

Table A.3. The IMM parameters used for the second simulation, as well as theparameters used for the small process noise CV-model, are given in Table A.4.

Parameter ValueTs 10, 5, 2Qk diag(800, 800, 800)R1

k diag(502, 0.152, 0, 32)R2

k diag(752, 0.2252, 0.452)R3

k diag(1002, 0.32, 0.62)P0 diag(100, 100, 100, 10, 10, 10)

(a) EKF-CV values

Parameter ValueTs 10, 5, 2Qk diag(52, 302, 52)R1

k diag(502, 0.152, 0, 32)R2

k diag(752, 0.2252, 0.452)R3

k diag(1002, 0.32, 0.62)P0 diag(100,100,100,10,10,10,1,1,1)

N 50000, 25000, 10000

(b) APF-CT values

Table A.3: Parameter values used for simulating the APF-CT model

Parameter ValueτCV 600τF M 10µ0

CV 0.9µ0

F M 0.1

(a) IMM Values

Parameter ValueQk diag(50, 50, 50)P0 diag(100, 100, 100, 10, 10, 10)

(b) Small process noise CV-model

Table A.4: Simulation parameters used for the IMM constellation.

institutionen för systemteknik - diva...

Documents