Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Development and evaluation of a filter for tracking
highly maneuverable targets
Examensarbete utfört i Reglerteknik vid 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ögskola
Linköpings universitet Linköpings universitet
Development and evaluation of a filter for tracking
highly maneuverable targets
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan i Linköping
av
Viktor Pirard
LiTH-ISY-EX--11/4499--SE
Handledare: Karl Granström
isy, Linköpings universitet
Egils Sviestins
SAAB AB
Examinator: Fredrik Gustafsson
isy, Linköpings universitet
Avdelning, Institution
Division, Department
Division of Automatic Control Department of Electrical Engineering Linköpings universitet
SE-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 LiTH-ISY-EX--11/4499--SE
Serietitel och serienummer
Title of series, numbering
ISSN
—
Titel
Title Utveckling 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
Author
Viktor 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 possible improvements of the tracking performance of this system are explored. The focus has been on improving the tracking of highly maneuverable targets observed with low 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 implemented in 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 fit a particle filter into SAAB’s existing IMM framework is proposed. The differ-ent filters were implemdiffer-ented in Matlab, and evaluation was done by the means of Monte Carlo simulations. The results from Monte Carlo simulations show sig-nificant improvement when tracking in two dimensions. However, the results in three dimensions do not display any substantial overall improvement when using the particle filter compared to using SAAB’s present filter. It is therefore not worthwhile 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 are made.
Nyckelord
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 possible improvements of the tracking performance of this system are explored. The focus has been on improving the tracking of highly maneuverable targets observed with low 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 implemented in 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 fit a particle filter into SAAB’s existing IMM framework is proposed. The differ-ent filters were implemdiffer-ented in Matlab, and evaluation was done by the means of Monte Carlo simulations. The results from Monte Carlo simulations show sig-nificant improvement when tracking in two dimensions. However, the results in three dimensions do not display any substantial overall improvement when using the particle filter compared to using SAAB’s present filter. It is therefore not worthwhile 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 are made.
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öljningsprestanda i ett at SAABs system. Det är förbättringar av följningsprestandan för snabbt manö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 som anvä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ör att verifiera att filtret lätt kan inkluderas i SAABs nuvarande produkt. De olika filtren implementerades i Matlab och utvärdering utfördes med hjälp av Monte Carlo simuleringar. Resultaten från simuleringarna visar på signifikanta förbätt-ringar vid målföljning i horisontalplanet. Simuleförbätt-ringar i tre dimensioner visar dock inte på några stora förbättring för partikel filtet jämfört med SAABs nuvarande filter. Det är därför i nuläget inte motiverat att byta ut SAABs nuvarande filter mot ett partikel filter, i alla fall inte i de scenarios med hög SNR som undersökts
vi
i denna rapport. Framtida studier rekommenderas dock innan några definitiva beslut fattas.
Acknowledgments
The past five years of my life have been spent at Linköping University, pursuing the Master of Science degree in Applied Physics and Electrical Engineering. As this Master thesis will conclude my studies, I would like to seize the opportunity to thank the people who have supported me in some way during my studies. First I 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 finding time in his busy schedule to give me inspiration and guidance. All my colleagues at the data fusion department also deserve my gratitude for making me feel welcome and helping me whenever I had questions. The creation of this thesis was also helped by Lic. Karl Granström and Prof. Fredrik Gustafsson at the division of Automatic control at the department of Electrical Engineering at Linköping University. They helped me with all questions regarding filtering and gave me inspiration for this thesis and Karl gave me great feedback on the layout and content 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 the course of this thesis.
Stockholm, September 27, 2011
Viktor Pirard
Contents
1 Background 1 1.1 Motivation . . . 1 1.2 Problem formulation . . . 2 1.3 Related work . . . 2 1.4 Limitations . . . 3 1.5 Thesis outline . . . 3 2 Radar 5 2.1 Range measurement . . . 5 2.2 Range Rate . . . 6 2.3 Angle measurement . . . 6 2.4 Sensor Modeling . . . 6 2.4.1 Measurement Noise . . . 8 3 System Modelling 11 3.1 Dynamic models . . . 11 3.2 Planar models . . . 12 3.2.1 Constant Velocity . . . 12 3.2.2 Coordinated Turn . . . 133.2.3 Curvilinear motion model . . . 14
3.3 3D models . . . 16
3.3.1 Preliminaries . . . 16
3.3.2 3D Coordinated Turn . . . 17
3.3.3 Variable Turn model . . . 19
3.3.4 Process noise transformation . . . 21
4 Estimation 23 4.1 Estimation . . . 23
4.2 Kalman Filter . . . 24
4.2.1 Algorithm . . . 25
4.3 Extended Kalman Filter . . . 25
4.3.1 Algorithm . . . 26
4.4 Unscented Kalman Filter . . . 27
4.4.1 Sigma Points . . . 27
4.4.2 Algorithm . . . 28 ix
x Contents 4.5 Particle Filter . . . 29 4.5.1 Algorithm . . . 31 4.5.2 Resampling . . . 31 4.5.3 Estimation . . . 32 4.5.4 Prior Sampling . . . 33 4.5.5 Likelihood sampling . . . 33
4.5.6 Auxiliary Particle filter . . . 35
4.6 Interacting Multiple Model . . . 37
4.6.1 State Combination . . . 39
4.7 Performance evaluation . . . 41
4.7.1 Root Mean Square Error . . . 41
4.7.2 Normalized Estimation Error Square . . . 42
5 Results 43 5.1 Filter evaluation . . . 43 5.2 Trajectory 1 . . . 44 5.2.1 Simulation I . . . 45 5.2.2 Simulation II . . . 49 5.3 Trajectory 2 . . . 53 5.3.1 Simulation I . . . 55 5.3.2 Simulation II . . . 58 5.3.3 Conclusions . . . 62
6 Conclusions and Future Work 63 6.1 Conclusions . . . 63
6.2 Future work . . . 64
Bibliography 67 A Appendix 69 A.1 3D Measurement . . . 69
A.2 Simulation Parameters . . . 70
A.2.1 Trajectory 1 . . . 70
Chapter 1
Background
In this chapter a short background to air surveillance systems is given to make the motivation for this thesis clear. After that the purpose of this master thesis is presented, 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 exact situation 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 only give position information. An exact situation picture does not only incorporate knowing the current positions of different air targets, but also the headings and speeds with which the targets are moving. This is where target tracking plays an important 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 different countries 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. when an 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/ascends it is said to be maneuvering. There is also a distinction made between highly maneuvering targets and maneuvering targets, the difference is if the maneuver is a high-g - includes large accelerations - one or not. The cut-off point between the higly maneuvering and maneuvering is in this thesis defined as twice that of the earth gravity 9.82[m
s2], i.e. targets with accelerations larger than 2 · 9.82 are
called highly maneuvering. With this master thesis, SAAB wants to look into the possibility of improving the tracking performance of highly maneuvering targets when the rate at which measurements arrive is low, i.e. Ts> 2, where Ts denotes the sampling time.
2 Background
1.2
Problem formulation
In this master thesis, the problem of tracking highly maneuverable targets with low measurement rate is considered. Since the tracking problem is non-linear, different non-linear filtering methods are considered. Especially the use of particle filters for tracking is explored. The different algorithms are implemented in Matlab and evaluated with Monte Carlo simulations. The most promising filter is then integrated in an Interacting Multiple Model (IMM) structure, together with a different 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 articles about particle filtering and Interacting Multiple Model have been published during the last decades. To give the reader a good overview of all the related work is far beyond scope of this thesis. There is however a couple of articles closely related to the work in this thesis.
In [7] the authors evaluate different EKF, UKF and PF combinations in an IMM constellation. The main difference between the problem that is addressed there and the problem in this thesis is that in [7] the process noise covariance is relatively larger than the measurement noise covariance. This is due to the fact that they have a considerably shorter sampling time (1 second) and have much 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 ten larger, the impact from the process noise over the sampling period makes process noise covariance larger than the measurement noise covariance. The difference in scenarios makes it hard to use the results in [7].
Another interesting article concerning a similar subject is [22]. In this article a different model, suggested in [15], is used to handle the fast maneuvers. The measurements are here given by azimuth, elevation, the change rate of azimuth and elevation as well as the range rate. The performance of an IMM running with two EKF filters is compared to an IMM running with two UKF filters. In the simulation 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 case the UKF greatly outperforms the EKF. One could speculate that this is due to the non-linearities of the measurements equation, in particular the different change rates. The fact that the measurements used in this article differs from the ones in this 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 dealing with tracking in 2D are [13] and [5]. The former compares the performance of an Auxiliary particle filter running three different Coordinated Turn models with that of an IMM running three EKF filters for the same models. The latter compares the performance and robustness of a couple of different algorithms. The measurements are here given by the received power strength and have quite large uncertainty compared to the process noise. This makes it hard to draw parallels to the work done 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 of multiple radars the time between measurements can be decreased/increased. A more realistic scenario would be that the target is not seen by both radars at all times, so the sampling time would be variable.
• Only a single target is considered and there is no false or missed radar measurements. By this assumption the gating and association problem is avoided, 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 presents some related work. The second part presents the theory used in the thesis and is divided into two chapters, the first chapter presents the theory behind the radar and the second chapter presents estimation theory. The third part presents the simulation results, and the fourth part gives some concluding remarks and ideas for future work.
Chapter 2
Radar
This chapter is meant to give the reader a brief overview of the sensor used in this thesis. A full discussion about radar and the different properties is beyond the scope of this text. For a more detailed overview of the radar system, the reader is referred to [4] and [16], which have influenced this text. A radar - RAdio Detection and Ranging - is a sensor used for determining the position of a target relative to the radar station. The radar was developed in the early 1930’s and has since then seen widespread use. A crude explanation of how a radar works is that it sends out electromagnetic energy in a given direction. If there is a target within the range of the radar in this direction, some of the transmitted energy will bounce off the object and return to the radar. The most common measurements available from 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 suitable for different purposes, have been developed. For the purpose of this thesis, only ground based radars are of interest, and therefore only this radar type will be described here. First a short explanation of how the range, range rate, azimuth and elevation are measured by the radar is given. This is followed by a presentation on 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 it takes 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 of visible light. Radio signals travel by the speed of light c. The travelled roundtrip distance, 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 as ˙
R = dRdt of the target. The principle behind the Doppler effect is that waves being reflected by a target moving relative to the radar will cause a shift in the frequency of the reflected waves compared to the frequency of the transmitted waves. If the target is moving towards the radar it will make the wavelength shorter, and if the target is moving away from the radar the wavelength will become longer. The Doppler effect is given by
f′= f −vλr, (2.2.1)
where f′ is the frequency the observer measures, f is frequency transmitted by
the Radar, vr is the target’s radial velocity relative to the Radar and λ is the wavelength of the transmitted waves. From (2.2.1) the following equation can be derived for a Radar,
˙
R = −fd2λ. (2.2.2)
The factor 2 in the denominator is due to the fact that Doppler effect occurs both on the way from the radar to the target and on the way back. By knowing the wavelength λ of the transmitted signal, and calculating the Doppler frequency shift 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 radar oriented spherical coordinates system, which can be transformed into a Cartesian coordinate system, see Figure 2.1. A very simple explanation of how the angle measurements are obtained is that the radar gives its own azimuth and elevation when the reflected signal is detected.
2.4
Sensor Modeling
Ground based radars that are used in air operations systems often work in the frequency band [1 − 2] GHz called the L-band. The antennas used are large and have a long range, and the radar continuously rotates the antenna to get full 360 degree coverage. The long range capability of the L-band radars has the drawback in that it gives the antenna a low turn rate, i.e. the time it takes for the antenna to complete a full 360 degree sweep is high. The shape and size of the radar antenna determines its resolution in azimuth and elevation. In the case of ground based antennas used in air operations systems, the antenna is often parabolic in shape and rotates around a vertical axis, see Figure 2.2. This type of antenna gives a high resolution in azimuth but low resolution in elevation. The range rate measurements from a radar in the L-band is in general not available, and the range rate 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 R Azimuth Eleviation
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 noise sources that affect the radar performance. Examples include hardware dependent noise, e.g. noise from electronic equipment, and thermal noise. Thermal noise is in fact dependent on the elevation angle of the radar, but these effects are not considered here. A standard way of modelling the impact of the hardware and thermal noise is as additive white Gaussian noise, independent in the different measurement quantities (range, azimuth and elevation). This simple model has seen widespread use and has also be used in this thesis. According to [2], this model is also a realistic one.
The radar measurements are modelled in the following way for the 2D case:
R =px2+ 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 =px2+ y2+ z2+ vR, (2.4.2a)
α = arctan 2(y, x) + vα, (2.4.2b)
ǫ = arctan 2(z,px2+ 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 = 0 0 y = 0, x = 0 (2.4.3)
In this thesis, all tracking is done in a radar centred coordinate system, i.e. the radar 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 models to represent differential equations. Next, discrete state space models are presented and also the process of going from a continuous state space model to a discrete state space model. When the discussion of state space models is complete, state space models describing different motions that an aircraft can perform is presented. First planar models that describe aircraft motion in 2D is presented followed by models for aircraft motion in 3D. Finally, a transformation for process noise modelled in the 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 differential equations. A state space model is a powerful way of representing the differential equations that describe the system and measurement dynamics, and is presented next. 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 signal and 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, however only additive noise is considered in this thesis. For the subject of this thesis, there is no known control signal u(t) and it has therefore been omitted in the remainder of 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 for discrete 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)
12 System Modelling
The difference between X(T ) and Xk is that the discrete state vector Xk takes on the 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 continuous state vector X(t) and the discrete time state vector Xk.
To discretize a continuous system, the following formula, called the sampling formula, can be used,
X(t + Ts) = X(t) + Z t+Ts
t
f (X(τ ))dτ . (3.1.3)
Discretizing the state noise is not quite as straightforward and there are several different ways of doing this, corresponding to different assumptions about how the process noise enters the system. In the discretization formula for process noise used here, Qk= Z Ts 0 efx′(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 enter the 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 examined in 2 dimensions. Here the dynamics described by the different models are not as complex as those describing a 3D motion. In the following section, the 2D models are 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, the model 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 - thrust from 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. Thus
a large process noise models a large resulting force acting on the aircraft. This makes it useful for describing fast maneuvers.
The continuous model is given by
˙ X(t) = 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 X(t) + 0 0 0 0 1 0 0 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 σy2
. Using (3.1.3) gives the discrete model,
Xk+1= 1 0 Ts 0 0 1 0 Ts 0 0 1 0 0 0 0 1 Xk+ vk. (3.2.2)
The discrete process noise vk covariance, Qk, is given via (3.1.4) as
Qk = 0 0 0 0 1 0 0 1 Q 0 0 1 0 0 0 0 1 . (3.2.3)
From the Taylor expansion of eAT = I + AT + A2
2!T2+A
3
3!T3+ . . ., and noticing
that A2= (AT)2= 0, the covariance is then given as
Qk= σ2 xT 3 s 3 0 σ2 xT 2 s 2 0 0 σ 2 yT 3 s 3 0 σ2 yT 2 s 2 σ2 xT 2 s 2 0 σx2Ts 0 0 σ 2 yT 2 s 2 0 σ2yTs . (3.2.4)
3.2.2
Coordinated Turn
The coordinated turn (CT) motion describes a circular motion with constant turn rate w and speed s, and it has seen widespread use. In the CT-model, process noise is included as the derivate of turn rate and speed, and represents deviations from an exact circular path. The CT-model can be given either in Cartesian coordinates or 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 smaller linerization error than the Cartesian representation. In polar coordinates, the motion 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 wk sinwkTs 2 cos θk+ wkTs 2 , (3.2.6a) yk+1=yk+ 2sk wk sin wkTs 2 sin θ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 0 0 0 0 0 0 0 0 Ts2σs2 0 0 0 0 0 Ts3σw2/3 Ts2σ2w/2 0 0 0 Ts2σw2/2 Ts2σ2w . (3.2.7)
Here, σsand σwis the standard deviation of the continuous process noise for speed and 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 can describe an object that either accelerates, decelerates or keep their velocity during a turn. The CT-model is a special case of the CL-model for the case when the along-track acceleration is zero. This model described in Cartesian coordinates was given in [15], a polar representation of this model was not found and is next derived.
The state vector is X(t) = (x(t), y(t), s(t), φ(t), w(t), al(t))T, where x(t) and
y(t) denotes the target position in Cartesian coordinates, s(t) the speed, φ(t) the
heading, w(t) the turn rate, al(t) is the longitudinal acceleration and v(w)(t) and
v(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 to go to a discrete state space representation, the assumptions that al(t) and w(t) are
3.2 Planar models 15
a < 0 a > 0 a = 0 Start
Figure 3.1: Simulated trajectories for a Curvilinear motion with the same state
16 System Modelling
constant during a sampling interval are made. These assumptions are the same assumptions made in the Cartesian CL-model presented in [15].
Using(3.1.3) on the x component (3.2.8a) gives
xk+1= xk+ Z t+Ts t s(τ ) cos φ(τ )dτ = xk+ s(τ ) Z cos φ(τ )dτ − Z t+Ts t alZ cos(φ(τ ))dτ = Z cos(φ(τ ))dτ ≈ −1 wsin(φ(τ )) = xk+ −s(τ ) w sin φ(τ ) + Z t+Ts t al wsin(φ(τ ))dτ
=xk+(sk+1w sin φk+1−skwsin φk)+w2al(cos(φk+1)−cos(φk)). (3.2.9) Remaining states can be treated similarly. The deterministic part of the model is thus given by xk+1=xk+sk+1wk sin φk+1−wksksin φk+ al k w2 k
(cos φk+1−cos φk), (3.2.10a) yk+1=yk+wksk cos φk−sk+1wk cos φk+1+ alk w2 k (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) al k+1=alk. (3.2.10f)
Here, the process noise v(t) is assumed to enter the system immediately after a sample, which corresponds to X(t + Ts) = f (X(t) + v(t)). This model is used in a particle filter, see chapter 4, and a discrete process noise has therefore not been derived.
3.3
3D models
In the remainder of this chapter, the 3D counterparts of the planar models de-scribed in the previous section are dede-scribed. Motion in 3D requires more 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 coordinate systems, one that has its center in the target body, denoted B, and an inertial coordinate 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 magnitude of 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 to the inertial frame.
If u(t) is a time varying vector, Saint-Guilhems equation relates the derivative of 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-axis in 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 = (d¯s 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 resulting
vector from a matrix cross product is always orthogonal to the vectors being crossed ( a = b × c ⇒ a⊥b, a⊥c), gives that Ω⊥¯a. This means that the motion is in a plane - ¯s, ¯a is in a plane - with Ω as normal. This plane is called the
maneuverplane, and it is not restricted to the horizontal plane, it can have any rotation in space.
The norm of the angular velocity vector Ω, called the turn rate w, is now defined from (3.3.4) as
w ≡ kΩk=k¯s × ¯ak
s2 . (3.3.5)
3.3.2
3D Coordinated Turn
The three dimensional coordinated turn (3D-CT) model is based on the same assumptions 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 would be a speed change. This means that (3.3.5) simplifies to
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 is
constant ˙Ω = 0, gives
˙¯a = ˙Ω × ¯s + Ω × ¯a = Ω × (Ω × ¯s) = (Ω¯s)Ω − (ΩΩ)¯s = −w2¯s. (3.3.8)
This leads to the following continuous model
˙ X(t)= 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 −w2 0 0 0 0 0 0 0 0 0 −w2 0 0 0 0 0 0 0 0 0 −w2 0 0 0 X(t)+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 1 0 0 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 than in an inertial coordinate system. This means that the noise has to be transformed from the body frame to the inertial frame. This transformation is described in Section 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(wTw s), S2 = w sin(wTs), C1 = 1−cos(wTw2 s) and C2 =
cos(wTs) has been introduced to get a more compact expression. The motion is only coupled via the common turn rate w given by (3.3.6) over the different coordinates.
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 as that 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 white noise jerk model in one dimension with variance σ2
xis Qk = σx2 T5 s 20, T4 s 8 , T3 s 6 Ts3 2 , Ts2 2 , Ts Ts2 2 , Ts, 1 . (3.3.12)
This model does not depend on w, which seems quite strange since this means that the 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 is in 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 the 3DCT-model since it can describe a wider range of motions. The variable turn model does not, like the 3DCT-model, assume that the speed and turn rate vectors are constant. It does assume that the motion is planar so that the angular velocity vector is orthogonal to the speed and acceleration vectors. Using (3.3.1), the jerk in the inertial frame is derived as
˙¯a = (d2¯s
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 · ¯s s2 ¯a. (3.3.14)
Further, the following holds
α ≡ −s · ¯a¯
s2 , (3.3.15a)
v ≡(d
2s¯
dt2)B+ ˙Ω × ¯s, (3.3.15b)
where the variable α can be interpreted as a relative damping coefficient and
v captures the effect of forces and moments acting on the target. With these
20 System Modelling
a < 0 a > 0 a = 0 Start
Figure 3.2: Simulated trajectory for a variable turn motion. The differences be-tween the trajectories are only in the relative dampening α, where the trajectories have α = 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 only coupled via a common w and α. The model in the x-component is given by
˙ X(t) = 0 1 0 0 0 1 0 −(α2+ w2 c) −2α X(t) + 0 0 1 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 frame and transformed to the inertial frame, see Section 3.3.4. In Figure 3.2 a variable turn motion using different values on the relative dampening α is illustrated. Using (3.1.3), the following discrete time model is obtained
Xk+1=
1 2αwc−e−αTs(2αwccos wcTs+(α2−w2c) sin wcTs) wc(α2+w2c)
wc−e−αTs(wccos wcTs+α sin wcTs) wc(α2+w2c) 0 e−αTs(wccos wcTs+α sin wcTs) wc e−αTssin wcTs wc 0 −(α2+w2c)e−αTssin wcTs wc e−αTs(wccos 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 that the 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 coordinate system. A better choice in this sense would be to place a coordinate system at the centre of the earth. This will however not be done in this thesis since the effects from the rotation of the earth, like the Coriolis effect, does not give any substantial 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 is described here. This transformation is done via the assumption that the speed vector ¯s is never parallel to the z-axis in the inertial frame. This assumption does
not always hold, but for the applications in mind for this thesis it is valid. The transformation is performed in the following steps.
The longitudinal process noise vl, given in body coordinates is transformed via the speed vector ¯s to global coordinates,
vl x vl y vl z = sx s sy s sz s sl. (3.3.19)
The transversal process noise vtis then transformed by
s × z = sy −sx 0 , vxt vyt vt z = sy k(sy,−sx,0)Tk −sx k(sy,−sx,0)Tk 0 k(sy,−sx,0)Tk v t. (3.3.20)
22 System Modelling
Finally the process noise in elevation, veis transformed by sx sy sz × sy −sx 0 = sxsz sysz −s2 x− s2y , ve x ve y ve z = sxsz k(sxsz,sysz,−s2x−s2y)Tk sysz k(sxsz,sysz,−s2x−s2y)Tk −s2 x−s 2 y k(sxsz,sysz,−s2x−s2y)Tk ve. (3.3.21) The total process noise in the x-component of the inertial frame is then simply
vX= vxl + vtt+ 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 the Kalman Filter is presented. Different approximations to the Bayesian recursion is then presented: the Extended Kalman Filter, the Unscented Kalman Filter and then the Particle Filter. A way of combining different filters in an interacting way, called Interacting Multiple Model, is then presented. Finally some performance evaluation methods used in this thesis are discussed. The subject of filtering and estimation 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-forward manner.
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 they need to somehow affect the measurement. The measurements are not exact, but influenced by some noise. This means that one can not determine the state exactly with 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 a conditional 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 to be 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 and the latter smoothing.
The Bayesian solution to the filtering problem is given by the PDF p(Xk|Y1:k).
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 state incorporates all the previous measurements and that different measurements are not dependent on each other. Using the Markov property, (4.1.3) can be simplified to 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) =
Z
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) = Z 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 transiequa-tion probability and is connected to the process model (3.1.1a), p(Xk|Y1:k−1) is called the prior and finally p(Xk|Y1:k) is
called the posterior. The estimated state vector at time k given the measurements up to time k is denoted ˆXk|k. Here, k|k means at time k given the measurements up to time k, e.g. the estimated state vector given measurements up to time k − 1 is denoted ˆXk|k−1.
4.2
Kalman Filter
The Kalman Filter (KF) is the solution to (4.1.5) in the case of linear measurement and motion dynamics (3.1.1), with additive Gaussian distributed noise. The white noise assumption means that a stochastic process v(t) with covariance Π obeys the following:
E(v(t)) = 0, (4.2.1a)
4.3 Extended Kalman Filter 25
The process noise v(t) and measurement noise e(t) must also be uncorrelated Cov(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 is
Gaussian, all the distributions in (4.1.5) will be Gaussian and described by their first 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|0and
covari-ance P1|0. The following equations are iterated for k = 1, 2, . . .
Sk = HkPk|k−1HkT + Rk, (4.2.3a) Kk = Pk|k−1HkT(Sk)−1, (4.2.3b) ǫk = yk− HkXˆk|k−1, (4.2.3c) ˆ Xk|k = ˆXk|k−1+ Kkǫk, (4.2.3d) Pk|k = Pk|k−1− Pk|k−1HkTKkHkPk|k−1, (4.2.3e) ˆ Xk+1|k= FkXˆk|k, (4.2.3f) Pk+1|k= FkPk|kFkt+ 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) on the form of (4.2.2) so that the KF theory can be applied. When the system is transformed 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 a Taylor expansion around the current state estimate. There are two types of EKF, the first order EKF1, and the second order, EKF2. The difference between these two 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, Fx′, H ′
x denotes the Jacobians of (4.3.1) with respect to X and similarly,
Fv, H
′
edenotes the Jacobians of (4.3.1) with respect to vk and ek. The Jacobian of a function F (Xk) = [f(1). . . f(m)] with dimension m, with respect to Xk =
[x(1). . . x(nx)] with dimension nxis Fx′ = 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. For the EKF2, even more derivates are needed when also the Hessian needs to be computed. The performance of the EKF is dependent on the accuracy of the Taylor expansion. If the non-linearities are severe, the Hessian of the expansion will become large, and there will be a potential gain in using the second order correction terms given by the EKF2 algorithm.
Next, an example of the Jacobians needed by the EKF for a two dimensional Constant Velocity model and Radar measurements are presented. The tracking is here done in Cartesian coordinates.
Example 4.1
The CV-model is linear (3.2.1), and the Jacobians Fx′, F
′ v are Fx′( ˆXk|k−1) = 1 0 T 0 0 1 0 T 0 0 1 0 0 0 0 1 , (4.3.4a) Fv′( ˆXk|k−1) = 1 0 0 0 0 1 0 0 0 0 1 0 0 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 Hx′, H
′
e, become a bit more complicated,
Hx′(ˆxk|k−1) = xk √ x2 k+y2k yk √ x2 k+y2k 0 0 −yk √ x2 k+y 2 k xk x2 k+y 2 k 0 0 , (4.3.4c) He′(ˆxk|k−1) = 1 0 0 1 . (4.3.4d)
In the appendix, the Jacobians for a 3D CV model are given. Note that the accuracy of a first order Taylor expansion is highly dependent of the choice of coordinate 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 gives a 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 PDF
p(Xk|Y1:k) with a Gaussian distribution. The difference between UKF and EKF is
how this is achieved. The UKF does not approximate the non-linear system with a linear one. Instead, it is based on the idea that it is more accurate to approximate the PDF of the function, than it is to approximate the function and obtain a PDF from the approximated function.
The way the UKF does this is by drawing a small number of points, called sigma points, that describes the prior PDF p(Xk−1|Y1:k−1) as a Gaussian. These
samples are then propagated through the non-linear system and a PDF is then calculated using these samples.
4.4.1
Sigma Points
Before the algorithm for choosing sigma points is revealed, the following notation is needed:
ˆ
X − Mean of the distribution, (4.4.1a)
χi− Sigma Pointi, (4.4.1b)
L − Dimension of the distribution, (4.4.1c)
Wim− Weight for sigma point i, used for estimating the mean, (4.4.1d)
Wc
i − 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)
28 Estimation
Note here that no time index is given to ˆX. The lack of a time index is a design
choice made to emphasize the difference of the unscented transform (UT) which is used in the UKF, and the UKF. The sigma points can be chosen in any way as long 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 Taylor expansion, see [12] for the details. The sigma point selection in this work was done with the sigma point scaling method. The idea behind this selection scheme is to introduce a scaling parameter ψ which controls the spread of the sigma points. Normally ψ is chosen small to minimize the effect of higher order transformation effects such as the third order moment, called skewness, when estimating the mean and 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 +p(L + λ)Px i , (4.4.2c) χi+L= ˆX −p(L + λ)Px i+L , (4.4.2d) Wm 0 = λ L + λ, (4.4.2e) Wc 0 = λ L + λ+ (1 − ψ 2+ β), (4.4.2f) Wim= 1 2(L + λ), (4.4.2g) Wic= Wim. (4.4.2h) Here √Px
i means the i-th row or i-th column of the matrix square root of Px, depending on whether the matrix square root is defined as a solution to Px= ATA or Px = AAT. Note that the weights here sum to one but that not all weights are 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 presented since these are central to the UKF algorithm. The UT can be used to estimate the distribution of a non-linear function, z = F (X). First, the sigma points are chosen, then they are propagated trough F (X). The propagated sigma points can then be used to estimate the first two moments ˆz, Pz of z, i.e. a Gaussian distribution for z. ˆ z ≈ 2L X 0 WimF (χi) (4.4.3a) Pz≈ 2L X i=0 Wic(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 using an 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 with non additive process/measurement noise, and it will be the method of choice in this thesis.
First, create an augmented state space vector Xa
k−1|k−1 and augumented co-variance matrix Pa,
Xa= ( ˆXk−1|k−1, E(vk−1), E(ek−1))T, (4.4.4a)
Pa= Pk|k 0 0 0 Qk 0 0 0 Rk . (4.4.4b)
Determine the augmented sigma points χa
k−1|k−1 = (χXk−1|k−1, χvk−1, χek−1)T and the weights Wm
i , Wic 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 (χXk−1|k−1, χvk−1). (4.4.4c) Compute ˆXk|k−1, Pk|k−1 with (4.4.3). The sigma points are then propogated trough the measurement equation,
χyk|k−1= H(χX
k|k−1, χek). (4.4.4d)
Now, ˆyk|k−1 and Pk|k−1y are estimated with (4.4.3). The covariance P xy k|k−1 is calculated as, Pk|k−1xy = 2L X i=0 Wic(χXk|k−1− ˆXk|k−1)(χ y k|k−1− ˆyk|k−1) T. (4.4.4e)
The estimate ˆXk|k−1 is now updated with the measurement,
Kk= Pk|k−1xy (Pk|k−1y )−1, (4.4.4f) ˆ
Xk|k= ˆXk|k−1+ Kk(y − ˆyk|k−1), (4.4.4g)
Pk|k= Pk|k−1− KkPk|k−1y KkT. (4.4.4h)
4.5
Particle Filter
Unlike the EKF and UKF, the particle filter (PF) does not represent the posterior PDF by its mean and covariance, i.e. as a Gaussian. The particle filter represents the PDF with a set of discrete points, called particles, and their corresponding weights. Unlike the UKF, where the sigma particles are chosen in a deterministic way, 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 probability
of 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 based on importance sampling and therefore the subject of importance sampling will be briefly explained.
Importance Sampling
Importance sampling is a tool used for stochastic integration when it is hard to draw samples directly from a distribution π(X), but it is possible to evaluate the density. A proposal distribution q(X) is introduced with a support greater or equal to that of π(X). The stochastic integration can, when the number of samples N is large, be approximated as I = Z Rn f (X)π(X) q(X)q(X)dX ≈ 1 N N X i=1 f (X(i))π(X(i)), (4.5.1)
here, Xi and ωi, denotes particle and weight i respectivley. The density of π(X) is approximated as
π(X) ≈ N1
N X
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(X1:k(i)|Y1:k)
. (4.5.3)
Writing this in a sequential way, the weight update becomes
ωk(i)∝ ωk−1(i)
p(yk|Xk(i))p(Xk(i)|Xk−1(i) )
q(Xk(i)|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
ωk(i)∝ ωk−1(i) p(yk|Xk(i))p(X (i) k |X (i) k−1) q(Xk(i)|X (i) k−1, yk) . (4.5.4b)
There are several different types of particle filters, all of them trying to handle the following two difficulties:
4.5 Particle Filter 31
• Sample depletion, which means that the number of samples that con-tribute in the representation of the posterior becomes very small. In the worst case only one sample will represent the whole posterior. This may oc-cur because all the other particles become highly unlikely and have a small weight assigned to them. This phenomenon can be measured with the num-ber of effective samples Nef f, which measures how many of the N samples that are actually used in representing p(Xk|Y1:k). It is defined as
Nef f =
N
1 + N2Var(ω(i)
k|k)
. (4.5.5)
In the ideal case Nef f = N and in the worse case Nef f = 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 have an algorithm that is not to computational demanding, i.e. the number of samples cannot be to large. Since the number of particles required to de-scribe a motion increases with the dimension of the state space it becomes increasingly important to keep a high number of effective samples compared to the number of samples.
4.5.1
Algorithm
The easiest proposal distribution to use is the transition density p(Xk|Xk−1) and the algorithm using the transition density as proposal is given below. There are several different ways to perform the resampling and estimation stage, the different options are presented after the algorithm.
Algorithm 1 Particle Filter
Xk(i)= f (Xk−1(i) , v(i)k )
ω(i)k = π(i)k−1p(yk|Xk(i))
π(i)k = ω (i) k PN i=1ω (i) k ˆ Xk=Estimate(Xk(i), π (i) k )
(Xk(i), πk(i)) =Resample(Xk(i), π(i)k )
4.5.2
Resampling
In the resampling step, the particles are sampled from the estimated distribution
p(Xk|X1:k). This means that particles with high weights have a high probability of
being sampled and particles with low weights have low probability. Resampling can either be performed in each time step, without checking if it is needed or not. This is called Sequential Importance Sampling (SIS). If resampling is only performed when the diversity of the particle set is below some threshold, it is called Sequential
32 Estimation −20 −1.5 −1 −0.5 0 0.5 1 1.5 0.5 1 1.5 x p(x) MAP MV
Figure 4.1: The minumin variance and maximum a posteriori estimates for a Multi-modal distribution. The figure shows that the MV estimate may be a poor choice for a multi-modal distribution.
Importance Resampling (SIR). For the SIR algorithm, some diversity measure is
needed. The measure that has been used in this thesis is the effective number of samples Nef f. The definition of Nef f (4.5.5), needs a computable approximation, and is approximated as Nef f ≈ 1 PN i:1(ω (i) k|k) 2. (4.5.6)
The threshold for resampling can for instance be selected as Nef f < N
2, which is
the threshold that has been in use in this thesis. The threshold value was determin in 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 appropriate choice in the uni-modal case, uni-modal means that there is just one local maximum. The MV estimate is
XMV k|k =
Z
Rn
Xkp(Xk|y1:k)dXk, (4.5.7a)
4.5 Particle Filter 33 Xk|kMV ≈ N X i=1 ωk(i)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 is more appropriate to select the most probable value. The MAP estimate is
ˆ
Xk|kMAP = argmaxXk(p(Xk|y1:k)), (4.5.8a) and can be computed from the samples as
i′ = arg imax(ω (i) k ), (4.5.8b) XMAP k|k = Xi ′ k|k. (4.5.8c)
4.5.4
Prior Sampling
The bootstrap particle filter is the most common type of particle filter, and uses the prior as the proposal distribution q(Xk|Xk−1i , yk) = p(Xk|Xk−1i ). 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 ykin the proposal. If the true dynamics of the system are well described by the dynamic equation, this is not a problem. However, when the measurements are much more exact than the dynamics, this is not a suitable proposal. All the other proposals given 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 weight update (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 Algorithm 2 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 states
nxis larger than the dimension of the measurement ny. This is because there has to 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 sampling the system. Below, a 2D CV model is down sampled a factor two.
