Citation |

- Permanent Link:
- https://ufdc.ufl.edu/UFE1001174/00001
## Material Information- Title:
- Kalman Filtering Methods for Moving Vehicle Tracking
- Copyright Date:
- 2008
## Subjects- Subjects / Keywords:
- Acceleration ( jstor )
Covariance ( jstor ) Engine noise ( jstor ) Estimated cost to complete ( jstor ) Evolutionary noise ( jstor ) Kalman filters ( jstor ) Noise measurement ( jstor ) Standard deviation ( jstor ) Statistical estimation ( jstor ) Trajectories ( jstor )
## Record Information- Source Institution:
- University of Florida
- Holding Location:
- University of Florida
- Rights Management:
- Copyright the author. Permission granted to the University of Florida to digitize, archive and distribute this item for non-profit research and educational purposes. Any reuse of this item in excess of fair use or other copyright exemptions requires permission of the copyright holder.
- Embargo Date:
- 8/8/2004
- Resource Identifier:
- 80611057 ( OCLC )
## UFDC Membership |

Downloads |

## This item is only available as the following downloads: |

Full Text |

PAGE 1 KALMAN FILTERING METHODS FO R MOVING VEHICLE TRACKING By NICOLAS OBOLENSKY A THESIS PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF SCIENCE UNIVERSITY OF FLORIDA 2002 PAGE 2 Copyright 2002 by Nicolas Obolensky PAGE 3 To my parents PAGE 4 ACKNOWLEDGMENTS I would like to express my sincere gratitude towards my advisor, Dr. Jos Prncipe, for giving me an opportunity to work on this challenging topic and for providing continuous feedback during the course of this work. Thanks are due to Dr. Erik Larsson and Dr. Yuguang Fang for agreeing to be on my committee. I wish to thank my parents and my sister for their constant emotional support and encouragement throughout my academic career. This work was partially supported by a Honeywell Corporation grant. iv PAGE 5 TABLE OF CONTENTS page ACKNOWLEDGMENTS.................................................................................................iv ABSTRACT.......................................................................................................................xi CHAPTER 1 INTRODUCTION...........................................................................................................1 Related Work.................................................................................................................1 Approach........................................................................................................................2 2 THE KALMAN FILTER APPLIED TO MOVING VEHICLE TRACKING...............4 A Description of the Kalman Filter...............................................................................4 A Two-State Filter: Friedlandâ€™s Model..........................................................................8 Dynamic Model.......................................................................................................9 Measurement Model.............................................................................................10 A Three-State filter: Ramachandraâ€™s Model................................................................10 Dynamic Model.....................................................................................................11 Measurement Equation.........................................................................................12 Drawbacks of the Classical Methods....................................................................12 3 A TIME-VARYING KALMAN FILTER....................................................................13 Time Varying Kalman Filter........................................................................................14 A Time-Varying State Model......................................................................................14 Model Update...............................................................................................................16 Results..........................................................................................................................18 4 THE UNSCENTED TRANSFORMATION ON NONLINEAR MEASUREMENTS34 The Extended Kalman Filter Applied to Moving Vehicle Tracking...........................35 A Description of the Extended Kalman Filter......................................................36 The Extended Kalman Filter and its Flaws...........................................................37 The Unscented Kalman Filter......................................................................................39 The Unscented Transformation.............................................................................40 The Application to the Kalman Filter...................................................................41 v PAGE 6 Results..........................................................................................................................44 Robustness to State Noise.....................................................................................46 Robustness to Measurement Noise.......................................................................53 Conclusion...................................................................................................................57 5 CONCLUSION AND FURTHER RESEARCH..........................................................58 Conclusion...................................................................................................................58 Further Research..........................................................................................................58 REFERENCES..................................................................................................................60 BIOGRAPHICAL SKETCH.............................................................................................62 vi PAGE 7 LIST OF TABLES Table page 2.1 Discrete Kalman Filter time update equations.........................................................7 2.2 Discrete Kalman Filter measurement update equations...........................................7 4.1 Extended Kalman Filter time update equations.....................................................37 4.2 Extended Kalman Filter measurement update equations.......................................37 4.3 Computation of the sigma points...........................................................................43 4.4 Unscented Kalman Filter time update equations...................................................43 4.5 Unscented Kalman Filter measurement update equations.....................................43 vii PAGE 8 LIST OF FIGURES Figure page 2.1 The ongoing discrete Kalman filter cycle................................................................6 2.2 A complete picture of the operation of the Kalman filter........................................8 3.1 Example of video images. First video, the vehicle is appearing............................19 3.2 Example of video images. First video, the vehicle is slowing down.....................19 3.3 Example of video images. First video, the vehicle is stopping..............................19 3.4 Example of video images. First video, the vehicle is speeding up........................20 3.5 Example of video images. First video, the vehicle is disappearing.......................20 3.6 Example of videos. Second video, the vehicle is entering.....................................20 3.7 Example of videos. Second video, the vehicle is stopping....................................21 3.8 Example of videos. Second video, the vehicle is stopped and the camera is shaking...................................................................................................................21 3.9 Example of videos. Second video, the vehicle is moving slowly..........................21 3.10 Example of videos. Second video, the vehicle goes back......................................22 3.11 Example of videos. Third video, the moving vehicle is on the road.....................22 3.12 Example of videos. Third video, the vehicle is approaching a crossroad..............22 3.13 Example of videos. Third video, the vehicle is crossing the crossroad.................23 3.14 Example of videos. Third video, the vehicle is speeding up.................................23 3.15 Example of videos. Third video, the vehicle is disappearing................................23 3.16 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =5, for the first video.............................................25 viii PAGE 9 3.17 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =5, for the second video........................................26 3.18 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =5, for the third video............................................27 3.19 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =10, for the first video...........................................28 3.20 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =10, for the second video......................................28 3.21 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =10, for the third video..........................................29 3.22 Actual trajectory, noisy measurements and estimated trajectory on the first video with a noise standard deviation of 5......................................................................30 3.23 Actual trajectory, noisy measurements and estimated trajectory on the first video with a noise standard deviation of 10....................................................................30 3.24 Actual trajectory, noisy measurements and estimated trajectory on the second video with a noise standard deviation of 5.............................................................31 3.25 LMS algorithm step size evolution with respect to time for the second video......31 3.26 LMS algorithm step size evolution with respect to time for the third video.........32 4.1 A complete picture of the operation of the extended Kalman filter......................38 4.2 The Unscented Transformation estimates for the mean and the covariance compared to the actual mean and covariance, and to the â€œfirst-orderâ€ estimates..42 4.3 Experimental set for the Unscented Kalman Filter performance test....................45 4.3 Unscented Kalman Filter subsystem......................................................................45 4.4 Actual and Estimated Trajectories for the Extended Kalman Filter for a state noise of 10 -3 .....................................................................................................................46 4.5 Actual and Estimated Trajectories for the Unscented Kalman Filter for a state noise of 10 -3 ............................................................................................................47 4.6 Range Estimation Residual of the Extended Kalman Filter for a state noise of 10 -3 .48 4.7 Range Estimation Residual for the Unscented Kalman Filter for a state noise of 10 -3 .........................................................................................................................48 ix PAGE 10 4.8 Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the Extended Kalman Filter for a state noise of 10 -3 ........................49 4.9 Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the Unscented Kalman Filter for a state noise of 10 -3 ......................49 4.10 Actual and Estimated Trajectories for the Extended Kalman Filter for a state noise of 10 -1 .....................................................................................................................50 4.11 Actual and Estimated Trajectories for the Unscented Kalman Filter for a state noise of 10 -1 ............................................................................................................51 4.12 Range Estimation Residual for the Extended Kalman Filter for a state noise of 10 -1 ..............................................................................................................................51 4.12 Range Estimation Residual for the Unscented Kalman Filter for a state noise of 10 -1 .........................................................................................................................52 4.13 Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the Extended Kalman Filter for a state noise of 10 -1 ........................52 4.14 Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the Unscented Kalman Filter for a state noise of 10 -1 ......................53 4.15 Actual and Estimated Trajectories for the Extended Kalman Filter for a measurement noise of 100.....................................................................................54 4.16 Actual and Estimated Trajectories for the Unscented Kalman Filter for a measurement noise of 100.....................................................................................54 4.17 Range Estimation Residual for the Extended Kalman Filter for a measurement noise of 100............................................................................................................55 4.18 Range Estimation Residual for the Unscented Kalman Filter for a measurement noise of 100............................................................................................................55 4.19 Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the Extended Kalman Filter for a measurement noise of 100..........56 4.20 Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the Unscented Kalman Filter for a measurement noise of 100........56 x PAGE 11 Abstract of Thesis Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Master of Science KALMAN FILTERING METHODS FOR MOVING VEHICLE TRACKING By Nicolas Obolensky August 2002 Chair: Dr. Jose Principe Major Department: Electrical and Computer Engineering Since the time of its introduction, the Kalman filter has been the subject of extensive research and application, particularly in the area of target tracking. This is likely due in large part to advances in digital computing that made the use of the filter practical, but also to the relative simplicity and robust nature of the filter itself. This thesis presents improvements in Kalman filtering applied to target tracking. First, we describe a time varying extension to the Kalman Filter for tracking moving objects in images. Classical methods for tracking assume either a constant acceleration model or a random acceleration. Those assumptions often turn out to be inaccurate. The proposed Kalman filter adapts its model (i.e., the state transition matrix) at each step to better estimate the movement of the maneuvering target. The performance of this time-varying Kalman filter is tested on an airborne surveillance video. Then, we introduce an improvement to the classical extended Kalman filter, called the unscented Kalman filter. The state distribution is represented, as in the xi PAGE 12 extended Kalman filter, by a Gaussian random variable, but is now described using a minimal set of carefully chosen points that, when propagated through the true nonlinear system, capture the posterior mean and covariance accurately, and help improve the tracking performance. The combination of the two methods leads to an algorithm that efficiently tracks moving targets with noisy radar measurements. xii PAGE 13 CHAPTER 1 INTRODUCTION Each time an outfielder tracks and catches a fly ball, he intuitively solves a problem of target tracking. That problem has stymied engineers, mathematicians, and computer scientists for years. Two great mathematicians, the American Norbert Wiener and the Russian Andrey Kolmogorov, first approached the problem during World War II. Rather than catching fly balls, Kolmogorov and Wiener were trying, in the days before computers, to develop mathematical algorithms that would help to track enemy aircraft by radar and automatically guide an anti-aircraft gun to shoot it down. The research started by Kolmogorov and Wiener has developed into a thriving area of applied mathematics known as Optimum Filtering Theory. For the past two decades, because of the development of computer technologies and computer vision, target tracking has evolved in many ways and has spread out to a large number of areas from traffic control to radar missile targeting. We will focus here on moving vehicle tracking with radar or ladar observations. It consists in predicting the future position of a vehicle moving on the ground, given range and angle (azimuth and elevation) data. Related Work As a fruitful application for state estimation, the problem of tracking a moving object from radar or ladar measurements has received a great deal of attention in the 1 PAGE 14 2 literature. Various issues such as decoupled tracking, tracking in cluttered environment, tracking a maneuvering target, and tracking multiple targets have been studied in detail [1,2]. However a number of problems are still open, and reducing computational requirements, while achieving some desired tracking performance is another important issue in many practical applications. Usually a Cartesian coordinate system is well suited to describe the target dynamics since the dynamic equations are often linear and uncoupled. Within the significant toolbox of mathematical tools that can be used for linear state estimation from noisy sensor observations, one of the most often-used tools in the target tracking field is what is known as the Kalman Filter. But, because measurements of the target location are expressed as nonlinear equations in coordinates, the tracking problem is connected with nonlinear estimation. The natural way to generalize the methods applied to linear estimation was to extend the Kalman Filter to nonlinear recursive equations: this classical extended algorithm is called the Extended Kalman Filter. These two algorithms are the foundation of most moving vehicle tracking algorithms, but they have a lot of important flaws that need to be addressed. Approach The Kalman Filter is the foundation of our moving vehicle tracking method, because of its adaptivity for implementation on a digital computer for on-line estimation and usefulness of the state-space approach. The state-space equations are decomposed into two parts: the state equation and the measurement equation. The state equation describes the target dynamics and are chosen in order to be linear. The measurement equation, that connects the state vector to the radar observations, is consequently PAGE 15 3 nonlinear. To solve this system of linear and nonlinear recursive equations, we introduce a combination of two filtering methods, each one improving the classical methods in their domain. This work presents in a first place a description of the Kalman Filter. It then describes a new version of the Kalman Filter that improves target tracking in a linear case. This improved version of the Kalman Filter, the time-varying Kalman Filter, is tested on a set of vehicle tracking surveillance videos. Finally, this work introduces a new extension to the Kalman Filter to nonlinear equations, which, combined with the time-varying Kalman Filter, constitutes an efficient moving target tracking method, using radar measurements. PAGE 16 CHAPTER 2 THE KALMAN FILTER APPLIED TO MOVING VEHICLE TRACKING Within the significant toolbox of mathematical tools that can be used for stochastic estimation from noisy sensor measurements, one of the most well-known and often-used tools is what is known as the Kalman Filter [3]. The Kalman filter is essentially a set of mathematical equations that implement a predictor-corrector type estimator that is optimal in the sense that it minimizes the estimated error covariance-when some presumed conditions are met. Rarely do the conditions necessary for optimality actually exist, and yet the filter apparently works well for many applications in spite of this situation. A Description of the Kalman Filter The Kalman Filter has made a dramatic impact on linear estimation because of its adaptivity for implementation on a digital computer for on-line estimation and usefulness of the state-space approach. Today, the Kalman Filter is an established technique widely applied in the fields of navigation [4], guidance, aircraft [5] and missile tracking [6], reentry of space vehicles, etc. This section describes the filter in its original formulation [3], where the measurements occur and the state is estimated at discrete points in time. The Kalman filter addresses the general problem of trying to estimate the state of a discrete-time controlled process that is governed by the linear stochastic difference equation kkkkkwBuxAx1 4 PAGE 17 5 with a measurement z k that is kkkvHxy The random variables w k and v k represent the process and measurement noise (respectively). They are assumed to be independent (of each other), white, and with normal probability distribution p(w)~N(0,Q) p(v)~N(0,R) In practice, the process noise covariance Q and measurement noise covariance R matrices might change with each time step or measurement, however here we assume they are constant. The n x n matrix A in the difference equation relates the state at the previous time step k-1 to the state at the current step k, in the absence of either a driving function or process noise. Note that in practice A might change with each time step, but here we assume it is constant. The nx1 matrix B relates the optional control input u k l to the state x k+1 . The m x n matrix H in the measurement equation relates the state to the measurement z k . In practice, H might change with each time step or measurement, but here we assume it is constant. The Kalman filter estimates a process by using a form of feedback control: the filter estimates the process state at some time and then obtains feedback in the form of (noisy) measurements. As such, the equations for the Kalman filter fall into two groups: time update equations and measurement update equations. The time update equations are responsible for projecting forward (in time) the current state and error covariance estimates to obtain the a priori estimates for the next time step. The measurement update PAGE 18 6 equations are responsible for the feedback, i.e. for incorporating a new measurement into the a priori estimate to obtain an improved a posteriori estimate. The time update equations can also be thought of as predictor equations, while the measurement update equations can be thought of as corrector equations. Indeed the final estimation algorithm resembles that of a predictor-corrector algorithm for solving numerical problems. The time update projects the current state estimate ahead in time. The measurement update adjusts the projected estimate by an actual measurement at that time. The process can be described by the following figure: Figure 2.1:The ongoing discrete Kalman filter cycle The first task during the measurement update is to compute the Kalman gain K k . The next step is to actually measure the process to obtain z k , and then to generate an a posteriori state estimate by incorporating the measurement. The final step is to obtain an a posteriori error covariance estimate P k . After each time and measurement update pair, the process is repeated with the previous a posteriori estimates used to project or predict the new a priori estimates. PAGE 19 7 Table 2.1 Discrete Kalman Filter time update equations kkkBuxAx1 QAAPPTkk1 Table 2.2 Discrete Kalman Filter measurement update equations 1)(RHHPHPKTkTkk )(kkkkkxHyKxx kkkPHKIP)( The symbol â€œ^â€ highlights that the calculated value is an estimate. The superscript â€œ-â€ in or means that an a priori estimate is computed. H kx kP T is the transpose of the matrix H. In the actual implementation of the filter, the measurement noise covariance R is usually measured prior to operation of the filter. Measuring the measurement error covariance R is generally practical (possible) because we need to be able to measure the process anyway (while operating the filter) so we should generally be able to take some off-line sample measurements in order to determine the variance of the measurement noise. The determination of the process noise covariance Q is generally more difficult as we typically do not have the ability to directly observe the process we are estimating. Sometimes a relatively simple (poor) process model can produce acceptable results if one "injects" enough uncertainty into the process via the selection of Q. Certainly in this case one would hope that the process measurements are reliable. In either case, whether or not we have a rational basis for choosing the parameters, often times superior filter performance (statistically speaking) can be obtained by tuning the filter parameters Q and PAGE 20 8 R. The tuning is usually performed off-line, frequently with the help of another (distinct) Kalman filter in a process generally referred to as system identification. Figure 2.2: A complete picture of the operation of the Kalman filter A Two-State Filter: Friedlandâ€™s Model Consider an aircraft or similar space vehicle with constant velocity perturbed by a zero mean random acceleration ([7,8]). The position of the vehicle is assumed to be measured by a track-while-scan radar sensor at uniform sampling intervals of time T seconds and all measurements are noisy. The problem is to obtain the optimum estimates of position and velocity of the vehicle. This model assumes that each component of the vehicle position is independently measured by a radar sensor in the Cartesian coordinate system with constant accuracy, and that the observation errors have zero mean and are uncorrelated. PAGE 21 9 Dynamic Model As the model assumes that each position coordinate is measured independently, each coordinate is uncoupled from the other two and hence can be treated separately. For each coordinate, the vehicle dynamics is assumed to be described by TaxxTaTxxxkkkkkkk12121 where x k is the position at scan k, the velocity at scan k, a kx k the acceleration acting on the vehicle at scan k, T the sample rate. In that given model, the acceleration is assumed to be a random constant between successive observations with zero mean and uncorrelated with its values at other intervals, i.e., constaEaEakk22][0][ 0][knaaE if kn In the vector-matrix form, the vehicle dynamics may be written as X k+1 =AX k +Ba k with , kkkxxX 101TA and TTB22 . PAGE 22 10 X k is the vehicle state vector consisting of position and velocity components, A is the transition matrix, and B is the input distribution matrix. Measurement Model The position of the vehicle is assumed to be measured by a radar at uniform intervals of time T seconds and each observation noisy. The measurement equation is given by x m (k)=x k + k where x k is the true position at scan k, and k is random noise corrupting the measurement at scan k. The statistical properties of the noise are assumed to be constEEkk22][0][ 0][knE if kn In terms of the state vector X k , the measurement equation can be written as x m (k)=HX k + k with . 01H A Three-State filter: Ramachandraâ€™s Model Ramachandraâ€™s model [9] is a one-dimensional model for estimating the optimum steady-state position, velocity and acceleration of an aircraft moving with a constant acceleration perturbed by a zero mean plant noise with accounts for maneuvers and/or other random factors. The position coordinate of the moving vehicle is assumed to be measured by radar at uniform intervals of time T seconds through random noise. PAGE 23 11 Dynamic Model Each position coordinate of the moving vehicle is assumed to be described by the following equations of motion: nkkkkkkkkkaxxTxxxTxTxxx11212 where x k is the vehicle position at scan k , the vehicle velocity at scan k , the vehicle acceleration at scan k , T the sample time and a kx kx k the plant noise that perturbs the acceleration and accounts for both maneuvers and other modeling errors. a k is assumed to be zero mean and of constant variance a 2 and also uncorrelated with its values at other time intervals. In vector-matrix, the dynamic equation can be written as: kkkBaAXX1 with kkkkxxxX 10010212TTTA and 100B PAGE 24 12 Measurement Equation The measurement equation can be written as: x m (k)=Hx k + k where x m (k) is the measured position at scan k , k is the random noise corrupting at scan k and H . The statistical properties of the measurement noise are assumed to be the same as in the previous models. 001 Drawbacks of the Classical Methods The problem of these last two models is that they make assumptions on the form of the acceleration: random acceleration with zero mean in Friedland's model and constant acceleration in Ramachandra's model. In some cases, these models could be inappropriate. For Friedland's model, the Kalman Filter will minimize the random acceleration, considered as noise, and will therefore try to make the velocity constant. For Ramachandra's model, as the acceleration is considered to be constant perturbed by random noise, the Kalman filter will make the velocity of the vehicle increase toward infinity. Those important drawbacks lead us to think of a better model in order to better track moving vehicles. PAGE 25 CHAPTER 3 A TIME-VARYING KALMAN FILTER For target tracking in video images, two main assumptions are made in the Kalman filter formulation: a constant state model with the acceleration as a state, often using position and rate measurements, as in Ramachandra et al. [10]; or a constant state model with the acceleration considered as zero mean random input, as Friedland [7] described. In the first model, the acceleration is considered to be constant. In that case, the vehicle velocity remains constant for a zero acceleration or grows towards infinity in the direction of the acceleration. The alternative method to this unrealistic model is a random acceleration. But the Kalman filter, when the acceleration is an input noise, will eventually cancel out a zero mean acceleration as an input to the state equations. In that method also, the vehicle velocity will tend to be constant. In any practical model to track moving objects in images, the velocity, as well as the acceleration, should be able to vary. Still, the acceleration variations for maneuvering targets may be considered smooth. It is therefore conceivable to predict the current acceleration given the previous accelerations. In that case, the accelerations are no longer an input to the system, but become states as suggested in Singer [11]. At the same time, however, it is possible to estimate the acceleration at each step given the three previous (noisy) position measurements. This acceleration estimate will allow us to adapt the state transition matrix during operation and therefore obtain a time varying model. Consequently, the Kalman filter model becomes time varying and will be adapted to the maneuvering target movement. 13 PAGE 26 14 Time Varying Kalman Filter The maneuvering target movement can be described by the dynamic equations kkkkkkkDuHxyBuxAx1 where x k is the state vector, u k is the input vector and y k is the output. x k will be defined as kkkkavpx , where p k is the position of the moving vehicle, v k is its velocity and Nkkkkaaaa1 N2, with a i being the acceleration at step i. In our formulation there is no input to the system (u k =0) because the accelerations are considered as states. A Time-Varying State Model We have decided to work on the accelerations because they are the only possible innovation on the moving vehicle model. We also assumed that the acceleration varies with time (sample time T), because, as we already said, a constant acceleration would make the velocity either constant or eventually infinite. The second major assumption is that the current acceleration depends smoothly on the previous accelerations. If the number of previous accelerations is not too large (recent past), we furthermore assume that the acceleration can be modeled as a weighted sum of previous accelerations. At each step, we do a linear fitting of the acceleration data. Since there is noise in our PAGE 27 15 estimation of the acceleration, we consider that the acceleration vector is corrupted by additive Gaussian noise n k , with zero mean and covariance matrix Q k to be estimated in the design. ]1[]1,1[]2[]1,2[]2,2[]1,2[]1[],1[]2,1[]1,1[11121111111111NknaNwanaNwawawaknaNwawawaTavvTvxxNkkNkkNkkkkkkkNkkkkkkkkkkkkk where is the j ],[1jiwk th element of the weight vector corresponding to the acceleration a k+1-i . In a vector-matrix form, these dynamic equations can be written as: kkkknxAx1 where kkkkavpx , A k = ]1,1[000000]1,2[0],1[]2,1[0000100011111NwwNwwTTkkkk n k = . ]1[]2[]1[00Nnnnkkk PAGE 28 16 The measurement equation can be written as: y k = Hx k + k where H = and n 1000000010000000100000000001 k is a zero mean noise with a constant covariance matrix R. The measurements are p k , the vehicle position at time step k and a k-2 the acceleration at time step k, given by )2(22212kkkkppp T a Notice that if there is a measurement error, the error on a i is correlated to the error on a i-1 , and therefore the noise is not exactly white. But this only changes the shape of the noise covariance matrices from diagonal matrices to symmetric Toeplitz matrices with all zero diagonals except the first principal diagonal. Only the correlation between the a k-N and a k-N-1 errors cannot be represented in the model. Model Update In practice the acceleration variations are smooth, so the assumed weighted linear dependence will not change drastically every step. Therefore, we use an on-line method to update the weights. Each weight vector ]1[,],2[],1[ Nwwwkkk is updated using the LMS algorithm. PAGE 29 17 ][])[(][][111iiaiikkkikkkaawww (6) wherea is the input vector to the Kalman filter at step k-1 and the desired response a is the acceleration at the output of Kalman filter at the same step. So the error is the difference between the optimal acceleration computed by the Kalman filter and the acceleration estimated by the LMS algorithm. We can also write the Model Update with the following formula: 1][Nkikkaai1ik][11aieikkw ][][1iikka A k =A k-1 + 1111211111]1[00000]2[a]2[0]1[a]1[000000NkkNkkkkNkkkkaNeaeeaee The step size has to be chosen very carefully because it can make the whole algorithm fail. Too big a step size would make the model diverge, whereas too small a step size would prevent the model from adapting. Therefore, the approach for choosing properly is to make it dependent on the system dynamics. The faster the Kalman filter predicts and corrects the states (and consequently the accelerations), the faster the LMS algorithm has to adapt. The dynamics of the Kalman filter are given by the matrix (A k-1 â€“K k-1 H), where K k-1 is the Kalman gain at time step k-1. The norm of this matrix determines the speed of the Kalman filter. Therefore, it seems natural to make the LMS step size proportional to it: HKAkkkk11 PAGE 30 18 But the proportionality coefficient has now to be chosen wisely. To ensure the stability of the LMS algorithm, the proportionality coefficient k is chosen so that the step size k is always less than 22ka , where 2ka is the instantaneous power of the acceleration vector ka at step k. For example, the condition is satisfied if k is chosen equal to ak , where 21ka is the instantaneous power of the acceleration vector 1ka at step k-1, and p is the minimum reachable power. Indeed, we have 211212HKAaakkkk and HKAkkkk11 . pHKAkk311212 Results The performance of this adaptive Kalman filter has been tested on a set of surveillance videos. These videos are very useful because the detection part of tracking can be done manually. The advantage of a manual detection is that it has theoretically no error, therefore the output of this manual detection will be considered as the ground truth. The detection noise statistics can also be controlled, allowing a better analysis of the results. The algorithm tracked only one vehicle on the ground, i.e. there is only two tracked coordinates x and y, the altitude not being considered. Three representative videos have been chosen to test the performance of the algorithm. The three of them are 320 pixels by 240 pixels. The first video shows a vehicle crossing the image relatively slowly, with little vibrations of the camera. The second one shows a moving vehicle followed by a shaky camera. The third one shows a small vehicle crossing the image followed by a PAGE 31 19 very shaky camera. The figures 3.1 to 3.5 represent 10 different frames from the first video, at 5 different stages. Figure 3.1 Example of video images. First video, the vehicle is appearing. Figure 3.2 Example of video images. First video, the vehicle is slowing down. Figure 3.3 Example of video images. First video, the vehicle is stopping. PAGE 32 20 Figure 3.4 Example of video images. First video, the vehicle is speeding up. Figure 3.5 Example of video images. First video, the vehicle is disappearing. The figures 3.6 to 3.10 show 10 different frames at five different stages from the second video. Figure 3.6 Example of videos. Second video, the vehicle is entering. PAGE 33 21 Figure 3.7 Example of videos. Second video, the vehicle is stopping. Figure 3.8 Example of videos. Second video, the vehicle is stopped and the camera is shaking. Figure 3.9 Example of videos. Second video, the vehicle is moving slowly. PAGE 34 22 Figure 3.10 Example of videos. Second video, the vehicle goes back. The figures 3.11 to 3.15 show the movement of the vehicle on the third video. Figure 3.11 Example of videos. Third video, the moving vehicle is on the road. Figure 3.12 Example of videos. Third video, the vehicle is approaching a crossroad. PAGE 35 23 Figure 3.13 Example of videos. Third video, the vehicle is crossing the crossroad. Figure 3.14 Example of videos. Third video, the vehicle is speeding up. Figure 3.15 Example of videos. Third video, the vehicle is disappearing To evaluate the performance of the adaptive Kalman filter, a noise with a constant variance has been added to the ground truth. The performance criteria are the errors on the position p k and acceleration a k-2 measurements (we have considered N=2 in the a k vector). The proposed algorithm is also compared to a classical method for tracking: the constant acceleration model (Ramachandraâ€™s model [5]). They have been computed for PAGE 36 24 different variances on the whole set of videos. Because in both cases the error mean quickly decays to zero, the criterion to compare the two methods is the standard deviation of the position and acceleration errors. In all the following figures, 4 plots are shown: 2 plots for the x coordinate on the left-hand side and 2 plots for the y coordinate on the right-hand side. For each coordinate, the first row shows the position error standard deviation in pixels versus the adaptation steps, the solid line being the constant acceleration model error standard deviation, the dash dotted line being the noise standard deviation and the dashed line being the proposed algorithm position error standard deviation. The second row represents the standard deviation on the pixel error between the real acceleration a k-2 and the acceleration computed by the proposed algorithm (dashed line) and the constant acceleration model (solid line). The next three figures show the performance of the algorithms on the three videos when an additive Gaussian noise with a standard deviation of 5 is added. We first observe that the position errors for the proposed algorithm decrease for both the x and y coordinates. After 250 steps, the standard deviation of the filter output error goes beneath that of the measured position error. As expected, the measured position error standard deviation is around 5 pixels, whereas after 1500 steps, the filter output error standard deviation goes below 3 pixels. However, the classical method position error does not even decrease and remains high the whole time. This high error also appears in the classical algorithm, but is hardly compensated later, whereas, for the time-varying Kalman filter, the standard deviation quickly decays. PAGE 37 25 Figure 3.16 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =5, for the first video. x and y decreases quickly to around 3 pixels per second 2 , despite large errors at the beginning, whereas the classical model acceleration error standard deviation decreases to a higher value of 5 pixels per second 2 . For the second video, the position error standard deviation goes below the noise standard deviation only after 150 steps. But the error standard deviation for the classical algorithm remains around 15 pixels, three times as high as the noise standard deviation. For the acceleration error, the standard deviation also decreases quickly to around 2 pixels per second 2 for the time-varying Kalman filter, and to a higher value of 6 pixels per second 2 for the classical method. This third plot (Figure 3.18) also highlights the performance of the proposed algorithm compared to the classical method. PAGE 38 26 Figure 3.17 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =5, for the second video. The time-varying Kalman filter errors standard deviations are always lower than that of the constant acceleration model and are even much lower than the measurement error standard deviations. The three following plots (Figures 3.19, 3.20 and 3.21) show the performance of the algorithm with a higher noise standard deviation of =10 for the three representative videos. The behavior of the proposed algorithm is still the same even with a higher standard deviation. However, we can notice a higher error in the first hundred adaptation steps of the algorithm. The classical method acceleration error standard deviation is at least three times as high as that of the proposed algorithm. The change of the noise power does not alter the performance of the time-varying Kalman filter. PAGE 39 27 Figure 3.18 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =5, for the third video. The standard deviations still decay quickly, despite large errors in the beginning, and goes under the noise standard deviation. The acceleration error standard deviation is still negligible for the time-varying model (ten times as low as the theoretical noisy measurement error standard deviation), whereas for the classical model, the acceleration error standard deviation is still high (between 8 and 14 pixels per second 2 ). We can observe in all the previous plots that the constant acceleration model gives large errors. In deed, the large errors in the first adaptation steps are not compensated and the bad estimates for the acceleration propagate through the filter. On the contrary, the time-varying model compensates for those large errors and adapts to the vehicle movement. Besides, the acceleration error is quickly negligible, meaning that the algorithm adapts quickly to the real acceleration and that therefore the choice of the step size, that controls the adaptation speed, is suited. PAGE 40 28 Figure 3.19 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =10, for the first video. Figure 3.20 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =10, for the second video. PAGE 41 29 Figure 3.21 Position and Acceleration Error Standard Deviation for an added Gaussian noise with a standard deviation of =10, for the third video. The performance of the algorithm can be seen on the actual videos. Let us compare the actual trajectory, the noisy measurements and the trajectory estimated with the proposed algorithm. The trajectory estimated by the constant acceleration model algorithm is not even represented because the error variance is too large and therefore adds a salt-and-pepper noise to the figures. The actual trajectory is drawn in pink, the noisy measurements in green and the estimated trajectory in black. The actual trajectory does not exactly fit the road: despite the movement of the camera, the video frames are not re-centered. The next images show the different trajectories for noise variances of 5 and 10 on the first video. We can see on both images that the algorithm improves the tracking performance. There is apparently no bias in the estimated trajectory and the error variance is rather small. PAGE 42 30 Figure 3.22 Actual trajectory, noisy measurements and estimated trajectory on the first video with a noise standard deviation of 5. The same observations can be made on the second video. The next figure (Figure 3.24) represents the actual, the noisy and the estimated trajectories in pink, green and black respectively. Finally, let us evaluate the influence of the chosen step size . As in the previous tests, the state vector contains three accelerations a k , a k-1 and a k-2 for each coordinate. Figure 3.23 Actual trajectory, noisy measurements and estimated trajectory on the first video with a noise standard deviation of 10. PAGE 43 31 Figure 3.24 Actual trajectory, noisy measurements and estimated trajectory on the second video with a noise standard deviation of 5. Therefore, 6 LMS algorithms run in parallel. Let us see on the next plot the evolution of the 6 step sizes with respect to the adaptation time, for the second video where a noise with a standard deviation of 5 has been added. Figure 3.25 LMS algorithm step size evolution with respect to time for the second video. PAGE 44 32 All the step sizes have two states: a transition state and a steady state. The transition state in all the cases is rather short and consists of a quick exponential increase. In the steady state, the step sizes are almost constant, which means that appropriate step sizes have been found. The step sizes have the same types of behavior as for the second video: they reach a constant value after a short transition state. This proves the usefulness of an adapted value of the step size. For the third video, with the same noise level, the step sizes look as in Figure 3.26. Actually, if constant step sizes had been chosen in the beginning, the algorithm would have proven suboptimal. Figure 3.26 LMS algorithm step size evolution with respect to time for the third video. PAGE 45 33 Indeed, in order for the algorithm not to diverge, the best choice for a constant step size is to take the minimum of all step sizes. But it is obvious that, for the second video, in the steady state, the minimum value is not the optimum value, and for the third video, in the transition state, the minimum value is still not the optimal value. It is therefore important to use an adaptive step size that fits the dynamics of the Kalman filter. Consequently, the time-varying Kalman filter seems to give convincing results and to cope with the problems of the classically used methods (such as the constant acceleration method model). Indeed, the constant acceleration model is too sensitive to the initial acceleration, while the time-varying Kalman filter, even if the LMS tends to make the acceleration constant, is far more robust to acceleration changes, because it fits quickly to the new constant acceleration. Besides, the algorithm is computationally efficient, is designed to be implemented online and is the combination of two classical algorithms, the LMS algorithm and the Kalman filter, which makes it very easy to implement in hardware. However, the choice of the measurement model is not really adapted to real data; in deed, moving vehicle are often tracked by radar or ladar that collect range and angle data and not positions on the ground. Therefore, the measurement model needs to take into account the passage from position data to range and angle data, which is highly nonlinear. PAGE 46 CHAPTER 4 THE UNSCENTED TRANSFORMATION ON NONLINEAR MEASUREMENTS In the previous chapter, we described an efficient method that could help track a vehicle on a surveillance video. The measurements, i.e. the output of the state equation model, are a position and, in certain cases, the acceleration. But, practically, the useful data are often given by radar or Ladar. In deed, the idea is to scan large areas and look for special shapes, which are characteristic for certain types of targets. Besides, the real 3D radar of Ladar measurements can see much more than a 2D camera and none have got successful in hiding its own volume on a portable system. Basic or Laser radar can see through a lot of vegetation so this is no guarantee to be hidden for a target. The 3D data consists generally of the range r and two angles, the azimuth angle and the elevation angle . Therefore, if we want to keep the state equation model, with positions, velocities and accelerations, we need to introduce a nonlinear equation to relate them to the range and the angles: )arctan()arctan(22222yxzxyzyxr where r , , are respectively the range, the azimuth and the elevation, and x, y, z are the target positions in the Cartesian referential. 34 PAGE 47 35 The problem with the introduction of the nonlinearity is that the classical Kalman filter no longer works. Therefore, a new method trying to cope with that issue appeared at the end of the 1980â€™s: the so-called extended Kalman filter (EKF). The Extended Kalman Filter Applied to Moving Vehicle Tracking Kalman filter that linearizes about the current mean and covariance is referred to as an extended Kalman filter. In something akin to a Taylor series, we can linearize the estimation around the current estimate using the partial derivatives of the process and measurement functions to compute estimates even in the face of nonlinear relationships. Let us assume that our process again has a state vector x k , but that the process is now governed by a linear or nonlinear stochastic difference equation: ),,(111kkkkwuxfx with a measurement equation z k that is: ) ,(kkkvxhz where the random variables w k-1 and v k again represent the process and measurement noise. In this case the nonlinear function f in the difference equation relates the state at the previous time step k-1 to the state at the current time step k. It includes as parameters any driving function u k and the zero-mean process noise w k . The non-linear function h in the measurement equation relates the state x k to the measurement z k . In practice of course one does not know the individual values of the noise w k and v k at each time step. However, one can approximate the state and measurement vector without them as: ),,(11kkkuxfx PAGE 48 36 and ),(kkxhz where is some a posteriori estimate of the state (from a previous time step k). It is important to note that a fundamental flaw of the EKF is that the distributions (or densities in the continuous case) of the various random variables are no longer normal after undergoing their respective nonlinear transformations. The EKF is simply an ad hoc state estimator that only approximates the optimality of Bayes' rule by linearization. 1kx A Description of the Extended Kalman Filter To estimate a process with nonlinear difference and measurement relationships, we begin by writing new governing equations that linearize an estimate about the two previous equations, 111)(kkkkkWwxxAxx kkkkkVvxxHzz)( where x k and z k are the actual state and measurement vectors, kx and kz are the approximate state and measurement vectors, kx is an a posteriori estimate of the state at step k, the random variables w k and v k represent the process and measurement noise, A is the Jacobian matrix of partial derivatives of f with respect to x, that is: ),,(1,kkjijiuxxfA W is the Jacobian matrix of partial derivatives of f with respect to w, ),,(1,kkjijiuxwfW PAGE 49 37 H is the Jacobian matrix of partial derivatives of h with respect to x, ),(,kjijixxhH V is the Jacobian matrix of partial derivatives of h with respect to v, ),(,kjijixvhV Note that for simplicity in the notation we do not use the time step subscript k with the Jacobians A, W, H and V, even though they are in fact different at each time step. The complete set of EKF equations is shown below in Table 4.1 and Table 4.2. Table 4.1:Extended Kalman filter time update equations ),,(1kkkuxfx TkkkTkkkkWQWAPAP11 Table 4.2:Extended Kalman filter measurement update equations 1)(TkkkTkkkTkkkVRVHPHHPK )(kkkkkkxHyKxx kkkPCKIP)( The basic operation of the EKF is the same as the linear discrete Kalman filter as shown in Figure 2.1. Figure 4.1 below offers a complete picture of the operation of the EKF, combining the high-level diagram of Figure 2.1 with the equations from Table 4.1 and Table 4.2. The Extended Kalman Filter and its Flaws Given the noisy observation z k , a recursive estimation for can be expressed in the form: kx kx = (optimal prediction of x k ) +K k [z k -(optimal prediction of z k )] PAGE 50 38 Figure 4.1: A complete picture of the operation of the extended Kalman filter This recursion provides the optimal MMSE estimate for x k assuming the prior estimate and current observation z kx k are Gaussian. We need not assume linearity of the model. The optimal terms in this recursion are given by )],,([11kkkkwuxfEx 1~~kkkkzzyxkPPK )],([kkkvxhEz where the optimal prediction of x k is written as and corresponds to the expectation of a nonlinear function of the random variables x kxk k-1 and w k-1 . The same interpretation is done for the optimal prediction . The optimal gain term K kz k is expressed as a function of posterior covariance matrices (with kkzzz ~ ). Note these terms also require taking expectations of a nonlinear function of the prior state estimates. PAGE 51 39 The Kalman filter calculates these quantities exactly in the linear case, and can be viewed as an efficient method for analytically propagating a Gaussian random variable through linear system dynamics. For nonlinear models, however, the extended Kalman filter approximates the optimal term as: ),,(1kkkuxfx 1~~kkkkzzyxkPPK ),(kkxhz where predictions are approximated as simply the function of the prior mean value for estimates(no expectations are taken). The covariances are determined by linearizing the dynamic equations, and then determining the posterior covariance matrices analytically for the linear system. In other words, in the extended Kalman filter, the state distribution is approximated by a Gaussian random variable, which is then propagated analytically through the â€œfirst-orderâ€ linearization of the nonlinear system. As such, the extended Kalman filter can be viewed as providing â€œfirst orderâ€ approximations to the optimal terms. These approximations, however, can introduce large errors in the true posterior mean and covariance of the transformed (Gaussian) random variable, which may lead to suboptimal performance and sometimes divergence of the filter. Therefore, to improve the performance of the tracking filter, we need to address the problem of estimates of distributions propagating through nonlinear equations. The Unscented Kalman Filter The unscented Kalman filter addresses the approximation issues of the extended Kalman filter [12]. In its case, the state distribution is again represented by a Gaussian PAGE 52 40 random variable but is now described using a minimal set of carefully chosen points. These points capture the true mean and covariance of the Gaussian random variable, and when propagated through the true nonlinear system, capture the posterior mean and covariance accurately to the second order for any nonlinearity. The unscented Kalman filter is based on a mathematical transformation: the unscented transformation. The Unscented Transformation This transformation is used to calculate the statistics of a random variable, which undergoes a nonlinear transformation. Let us consider a random variable x of dimension L propagating through a nonlinear function y=f(x). Let us assume that x has mean x and covariance P x . To compute the statistics of y, a matrix of 2L+1 sigma points i is formed, according to the following: x0 ixiPLx))(( i=1,,L ixiPLx))(( i=L+1,,2L where = 2 (L+ )-L is a scaling parameter. The constant determines the spread of the sigma points around x and is usually set to a small value(less than 1e-4) and the constant is usually set to 0 or 3-L. ixPL))(( is the i th column of the square root matrix (e.g. lower triangular Cholesky factor).These sigma points are propagated through a nonlinear function: yi=f( i ) for i=0,L and the mean and covariance for y are estimated using a weighted sample mean and covariance of the posterior sigma points: PAGE 53 41 LiTiiciyLiimiyyyywPywy20)(20)())(( where the weights w i are defined as follows: )1(2)(0)(0 LwLwcm )(21)()(Lwwcimi for i=1,,2L where is used to incorporate prior knowledge of the x distribution( for Gaussian distributions, =2). The superscripts (m) and (c) indicate that the weights correspond to the computation of the mean and the covariance respectively. A simple example is shown in figure 4.2 for a 2-dimensional system: the left plot shows the true mean and covariance propagation using Monte-Carlo sampling, the center plot shows the results for the unscented transformation, the right plot shows the results for a method using a first order linearization approach as it is done in the extended Kalman filter. The Application to the Kalman Filter The unscented Kalman filter is a straightforward application of the unscented transformation applied to the recursive equation [13]: kx = (optimal prediction of x k ) +K k [z k -(optimal prediction of z k )] The unscented transformation sigma points selection scheme is applied to the state vector to compute the corresponding sigma matrix k . PAGE 54 42 Figure 4.2: The unscented transformation estimates for the mean and the covariance compared to the actual mean and covariance, and to the â€œfirst-orderâ€ estimates. Source: [13]. The unscented Kalman filter equations are given in Table 4.3, Table 4.4 and Table 4.5. One can notice that no explicit computation of Jacobians or Hessians is necessary to implement this algorithm. Furthermore, the overall number of computations is the same order L 3 as the extended Kalman filter. The most costly operation is forming the sample prior covariance matrix . A number of variations for numerical purposes are also possible. For example, the matrix square root, which can be implemented directly using the Cholesky factorization, with order L kP 3 /6. However, the covariance matrices are PAGE 55 43 expressed recursively, and thus the covariance matrix can be computed in only order MxL 2 (M being the size of z k ) by performing a recursive update to the Cholesky factor. Table 4.3:Computation of the sigma points 111111kkkkkkPxPxx Table 4.4:unscented Kalman filter time update equations ),(11kkkkuf Likkimikwx201,)( LiTkkkikkkicikxxwP201,1,)())(( kkkkkkkPxPxx1 )(11kkkkhz Likkimikzwz201,)( Table 4.5:unscented Kalman filter measurement update equations LiTkkkikkkicizzzzzzwPkk201,1,)(~~))(( LiTkkkikkkicizxzzxwPkk201,1,)())(( 1~~kkkkzzzxkPPK )(kkkkkyyKxx TkzzkkkKPKPPkk~~ The weights and are defined as in the unscented transformation. The )(miw )(ciw 1kk points are the a priori estimates of the sigma points and kkzzkz ~ . PAGE 56 44 Results The performance of the unscented Kalman filter applied to moving vehicle tracking has been tested on a set of simulated radar data. We have chosen to work with only 2-dimensional radar data (range and bearing), but the experiment can be easily generalized. The measurement equation becomes: kkkkkkkvxyyxr)arctan(22 where v~N(0,R) is an additive white Gaussian noise. r k is the range at time step k, k is the bearing at time step , x k is referred to as the cross-axis position and y k is referred to as the thrust-axis position. The state equation is the same as for the time-varying Kalman filter. The criteria to analyze the performance of the unscented Kalman filter are the estimated trajectory of the moving vehicle, the estimation residual for range with respect to the number of measurements and the East-West and the North-South position with respect to time. The performance of the unscented Kalman filter is compared with that of the extended Kalman filter. The experimental set is taken from the MATLAB demonstration program radmod.m and is described in Figures 4.3 and 4.4. The extended Kalman filter algorithm is a modified version of the extkalman.m MATLAB program. It has been changed in order to include the acceleration in the state model and the time-varying Kalman filter in the linear part of the algorithm. PAGE 57 45 Figure 4.3: Experimental set for the unscented Kalman filter performance test Figure 4.3: unscented Kalman filter subsystem The aircraft motion adds a slowly varying acceleration with noise, that affects this initial velocity. The initial longitudinal and the cross-axis positions are set to 0 and the initial velocity is set to 400 ft/s. To compare the performance of the unscented Kalman filter and that of the extended Kalman filter, the state noise power, and the measurement noise power are changed. PAGE 58 46 Robustness to State Noise The cross-axis acceleration noise and the thrust-axis acceleration noise variances are set to 10 -3 . The noise variances for the measurements are set to 1. The following figures (Figure 4.5 and Figure 4.6) show the actual and the estimated trajectories in polar coordinates for the extended and the unscented Kalman filters. The actual trajectory is drawn in red and the estimated one in green. One can notice that the trajectory estimated by the unscented Kalman filter is much smoother than the trajectory estimated by the extended Kalman filter. Figure 4.4: Actual and Estimated Trajectories for the extended Kalman filter for a state noise of 10 -3 . PAGE 59 47 Figure 4.5: Actual and Estimated Trajectories for the unscented Kalman filter for a state noise of 10 -3 . This is due to the fact that the noise propagates through the extended Kalman filter and introduces large errors in the true posterior mean and covariance of the state estimate. Besides, the unscented Kalman filter seems to give a much closer estimated trajectory. The following plots (Figure 4.6 and Figure 4.7) show the range estimation residual with respect to the number of measurements for both filters. We can observe that the extended Kalman filter range estimate diverges, while the unscented Kalman filter estimate remains bounded, even if the target moves away. The figures 4.8 and 4.9 show the cross-axis position x (N-S position) and the thrust-axis position y (E-W position). The actual positions are drawn in red, the noisy measurements in green and the estimated positions in blue. PAGE 60 48 Figure 4.6: Range Estimation Residual for the extended Kalman filter for a state noise of 10 -3 . Figure 4.7: Range Estimation Residual for the unscented Kalman filter for a state noise of 10 -3 . PAGE 61 49 Figure 4.8: Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the extended Kalman filter for a state noise of 10 -3 . Figure 4.9: Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the unscented Kalman filter for a state noise of 10 -3 . The extended Kalman filter positions estimates diverges from the actual position. In deed, the acceleration noise is integrated and is propagated through the filter that PAGE 62 50 increases the error, because of the first-order approximation. Nevertheless, the unscented Kalman filter keeps tracking efficiently the cross-axis and thrust-axis positions. Let us increase the state noise power to 10 -1 on both axes. The next set of figures shows the same plots as in figures 4.4 to 4.9, but with this new noise power. Figure 4.10: Actual and Estimated Trajectories for the extended Kalman filter for a state noise of 10 -1 . We observe on all the figures that the extended Kalman filter could not withstand the noise power increase. The estimated trajectory completely diverges from the actual trajectory and is still very noisy, the range estimation residual increases linearly with time and the cross-axis and thrust-axis positions do not correspond at all to the actual positions. PAGE 63 51 Figure 4.11: Actual and Estimated Trajectories for the unscented Kalman filter for a state noise of 10 -1 . Figure 4.12: Range Estimation Residual for the extended Kalman filter for a state noise of 10 -1 PAGE 64 52 Figure 4.12: Range Estimation Residual for the unscented Kalman filter for a state noise of 10 -1 Figure 4.13: Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the extended Kalman filter for a state noise of 10 -1 . PAGE 65 53 Figure 4.14: Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the unscented Kalman filter for a state noise of 10 -1 . Robustness to Measurement Noise The cross-axis acceleration noise and the thrust-axis acceleration noise variances are set to 10 -3 as in figures 4.4 to 4.9. But the noise variances for the measurements are now set to 100. The following plots show the effects of this change of measurement noise. The unscented Kalman filter tracks the object for about 70 seconds. After 70 seconds, the range estimation residual grows linearly, because the state estimates do not fit the actual positions anymore. However, the unscented Kalman filter seems to cope better with the noise power increase. This dramatic change of measurement noise makes the extended Kalman filter completely fail. On the contrary, the unscented Kalman filter withstood this dramatic increase of measurement noise power. PAGE 66 54 Figure 4.15: Actual and Estimated Trajectories for the extended Kalman filter for a measurement noise of 100. Figure 4.16: Actual and Estimated Trajectories for the unscented Kalman filter for a measurement noise of 100. PAGE 67 55 Figure 4.17: Range Estimation Residual for the extended Kalman filter for a measurement noise of 100. Figure 4.18: Range Estimation Residual for the unscented Kalman filter for a measurement noise of 100. PAGE 68 56 Figure 4.19: Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the extended Kalman filter for a measurement noise of 100. Figure 4.20: Cross-Axis Position (N-S position) and the Thrust-Axis Position (E-W position) estimates for the unscented Kalman filter for a measurement noise of 100. PAGE 69 57 The moving object is still tracked, though the estimates are less smooth. The range estimation residual remains bounded and the position estimates seem not to be biased. Conclusion The introduction of radar measurements leads to nonlinear equations that cannot be solved by the classical Kalman filter. The extended Kalman filter is the commonly used method to deal with the nonlinearity in moving vehicle tracking. It takes first-order approximates to the nonlinear equations to obtain a new set of linear state and measurement equations, that can be recursively solved by the Kalman filter. Unfortunately, in radar tracking, the high measurement noise propagates through the approximated filter and makes the algorithm fail. We have introduced in this chapter a new Kalman filter, the unscented Kalman filter that copes with the extended Kalman filter flaws. Coupled with the time-varying Kalman filter, described in the previous chapter, this new filter gives convincing results when it is applied to simulated radar data. PAGE 70 CHAPTER 5 CONCLUSION AND FURTHER RESEARCH Conclusion In the first part of this work, we have introduced a time-varying Kalman filter applied to moving vehicle tracking. The key idea is to consider the accelerations not as inputs to the system, but as states, in the model. Besides, in that model, the accelerations are modeled as a weighted sum of the previous accelerations. A linear fitting is recursively performed using an LMS algorithm. The computed model (state transition matrix, state error covariance matrix) is then used in a classic Kalman filter that has the vehicle position and several accelerations as its outputs. We have demonstrated the effectiveness of this filter on a set of airborne vehicle surveillance videos. After a short period of time, the algorithm becomes efficient, independently of the detection noise. In the second part of this work, we have coupled the time-varying Kalman Filter with a new extension of the Kalman Filter to nonlinear equations, the Unscented Kalman Filter. This new algorithm outperforms the classical Extended Kalman Filter, especially in very noisy environments. We test the combination of the two improved algorithms on simulated radar data. The tests show the robustness of this algorithm to state noise and measurement noise, and reveal that this algorithm is particularly appropriate for target tracking. Further Research In order for the Kalman Filters to be applicable, we make the assumption that the random variables have Gaussian probability distributions. We then consider that, after 58 PAGE 71 59 they propagate through the nonlinear functions, they are still Gaussian. Even if the approximation is accurate to at least the second order with the Unscented Transformation, the assumption is still wrong. To avoid those assumptions in the target tracking problem, the particle filter can be used. The Particle Filter is a sequential Monte Carlo based method that allows for a complete representation of the state distribution, using sequential importance sampling and resampling. It makes no assumption on the form of the probability densities in question, i.e. full nonlinear, non-Gaussian estimation. Nevertheless, the implementation of the Particle Filter is not yet very efficient. Research needs to be done in order for this algorithm to be implemented in hardware, and eventually be used in target tracking purposes. PAGE 72 LIST OF REFERENCES [1] Daum F.E., and Fitzgerald R.J. (1983). Decoupled Kalman Filters for phased array radar tracking. IEEE Transactions on Automatic Control, AC-28, vol.3, pp 269-283. [2] Chang C.B., and Tabaczynski J.A. (1984). Application of state estimation to target tracking. IEEE Transactions on Automatic Control, AC-29, vol.2, pp 98-109. [3] Kalman R.E. (1960). A new approach to linear filtering and prediction problems. Transactions of the ASME-Journal of Basic Engineering, vol.82, Series D, pp 35-45. [4] Hoshino M., Gunji Y., Oho S. and Takano K. (1996). A Kalman filter to estimate direction for automotive navigation. Proc. of the IEEE/SICE/RSJ International Conference on Multisensor Fusion and Integration for Intelligent Systems. pp. 145-150. [5] Gavrilets V., Shterenberg A., Dahleh M.A. and Feron E. (2000). Avionics system for a small unmanned helicopter performing aggressive maneuvers. Proc. of the 19 th Digital Avionics Systems Conferences, DASC, vol.1, pp. 1E2/1-1E2/7. [6] Zarchan P. (2000). Tracking and intercepting spiraling ballistic missiles. Position Location and Navigation Symposium 2000. pp 277-284. [7] Friedland B. (1980). Steady state behaviour of Kalman filter with discreteand continuous-time observations, IEEE Transactions on Automatic Control, AC-25, pp. 988-992. [8] Williams D., Friedland B. and Richman J. (1983). Design of an integrated navigation, guidance, and control system for a tactical missile. Proc. of AIAA Guidance and Control Conference, pp.57-66. [9] Ramachandra K.V. (1988). State estimation of maneuvering targets from noisy radar measurements, IEEE Proceedings, vol.135, Pt.F,no.1 , pp. 82-84. [10] Ramachandra K.V., Mohan B.R. and Geetha B.R.(1993). A three-state Kalman tracker using position and rate measurements. IEEE Transactions on Aerospace and Electronic Systems, vol. 29, no.1, pp. 215-222. 60 PAGE 73 61 [11] Singer R.A (1970). Estimating tracking filter performance for manned maneuvering targets, IEEE Transactions on Aerospace and Electronic Systems, AES-6, pp. 473-483. [12] Julier S.J., and Uhlman J.K. (1997). A new extension of the Kalman filter to nonlinear systems. Proc. of AeroSense: the 11th Int. Symp. on Aerospace/Defence Sensing, Simulation and Controls, pp. 52-56. [13] Wan E.A., and Van der Merwe R. (2000). The unscented Kalman filter for nonlinear estimation. Adaptive Systems for Signal Processing, Communications, and Control Symposium 2000, AS-SPCC, pp. 153-158. PAGE 74 BIOGRAPHICAL SKETCH Nicolas Obolensky was born on June 28, 1977, in Paris, France. He received his Engineering Diploma in electrical engineering with an emphasis on signal processing and control from the National School of Electrical Engineers of Grenoble (ENSIEG) in 2001. Since August 2000, he has been pursuing a Master of Science in electrical and computer engineering at the University of Florida in the area of signal processing. 62 |