Contents
Preface xiii
About the Authors xvii
Abbreviations xix
Symbol Description xxi
1 Introduction 1
1.1 Nonlinear systems . . . . . . . . . . . . . . . . . . . . . . . . 2
1.1.1 Continuous time state space model . . . . . . . . . . . 2
1.1.2 Discrete time state space model . . . . . . . . . . . . . 3
1.2 Discrete time systems with noises . . . . . . . . . . . . . . . 5
1.2.1 Solution of discrete time LTI system . . . . . . . . . . 6
1.2.2 States as a Markov process . . . . . . . . . . . . . . . 6
1.3 Stochastic _ltering problem . . . . . . . . . . . . . . . . . . . 7
1.4 Maximum likelihood and maximum a posterori estimate . . . 8
1.4.1 Maximum likelihood (ML) estimator . . . . . . . . . . 8
1.4.2 Maximum a posteriori (MAP) estimate . . . . . . . . 9
1.5 Bayesian framework of _ltering . . . . . . . . . . . . . . . . . 9
1.5.1 Bayesian statistics . . . . . . . . . . . . . . . . . . . . 9
1.5.2 Recursive Bayesian _ltering: a conceptual solution . . . 10
1.6 Particle _lter . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.6.1 Importance sampling . . . . . . . . . . . . . . . . . . . 12
1.6.2 Resampling . . . . . . . . . . . . . . . . . . . . . . . . 15
1.7 Gaussian _lter . . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.7.1 Propagation of mean and covariance of a linear system . . . . . . . 17
1.7.2 Nonlinear _lter with Gaussian approximations . . . . 19
1.8 Performance measure . . . . . . . . . . . . . . . . . . . . . . 22
1.8.1 When truth is known . . . . . . . . . . . . . . . . . . 22
1.8.2 When truth is unknown . . . . . . . . . . . . . . . . . 23
1.9 A few applications . . . . . . . . . . . . . . . . . . . . . . . . 23
1.9.1 Target tracking . . . . . . . . . . . . . . . . . . . . . . 23
1.9.2 Navigation . . . . . . . . . . . . . . . . . . . . . . . . 24
1.9.3 Process control . . . . . . . . . . . . . . . . . . . . . . 24
1.9.4 Weather prediction . . . . . . . . . . . . . . . . . . . . 24
1.9.5 Estimating state-of-charge (SoC) . . . . . . . . . . . . 24
1.10 Prerequisites . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
1.11 Organization of chapters . . . . . . . . . . . . . . . . . . . . 25
2 The Kalman _lter and the extended Kalman _lter 27
2.1 Linear Gaussian case (the Kalman _lter) . . . . . . . . . . . 27
2.1.1 Kalman _lter: a brief history . . . . . . . . . . . . . . 27
2.1.2 Assumptions . . . . . . . . . . . . . . . . . . . . . . . 28
2.1.3 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.1.4 Properties: convergence and stability . . . . . . . . . . 31
2.1.5 Numerical issues . . . . . . . . . . . . . . . . . . . . . 32
2.1.6 The information _lter . . . . . . . . . . . . . . . . . . 33
2.1.7 Consistency of state estimators . . . . . . . . . . . . . 34
2.1.8 Simulation example for the Kalman _lter . . . . . . . 35
2.1.9 MATLABr-based _ltering exercises . . . . . . . . . . 37
2.2 The extended Kalman _lter (EKF) . . . . . . . . . . . . . . 38
2.2.1 Simulation example for the EKF . . . . . . . . . . . . 40
2.3 Important variants of the EKF . . . . . . . . . . . . . . . . . 43
2.3.1 The iterated EKF (IEKF) . . . . . . . . . . . . . . . . 43
2.3.2 The second order EKF (SEKF) . . . . . . . . . . . . . 45
2.3.3 Divided di_erence Kalman _lter (DDKF) . . . . . . . 45
2.3.4 MATLAB-based _ltering exercises . . . . . . . . . . . 49
2.4 Alternative approaches towards nonlinear _ltering . . . . . . 49
2.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
3 Unscented Kalman _lter 51
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
3.2 Sigma point generation . . . . . . . . . . . . . . . . . . . . . 52
3.3 Basic UKF algorithm . . . . . . . . . . . . . . . . . . . . . . 54
3.3.1 Simulation example for the unscented Kalman _lter . . . . . . . . . . . 56
3.4 Important variants of the UKF . . . . . . . . . . . . . . . . . 60
3.4.1 Spherical simplex unscented transformation . . . . . . 60
3.4.2 Sigma point _lter with 4n + 1 points . . . . . . . . . . 61
3.4.3 MATLAB-based _ltering exercises . . . . . . . . . . . 64
3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
4 Filters based on cubature and quadrature points 65
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
4.2 Spherical cubature rule of integration . . . . . . . . . . . . . 66
4.3 Gauss-Laguerre rule of integration . . . . . . . . . . . . . . . 67
4.4 Cubature Kalman _lter . . . . . . . . . . . . . . . . . . . . . 68
4.5 Cubature quadrature Kalman _lter . . . . . . . . . . . . . . 70
4.5.1 Calculation of cubature quadrature (CQ) points . . . 70
4.5.2 CQKF algorithm . . . . . . . . . . . . . . . . . . . . . 71
4.6 Square root cubature quadrature Kalman _lter . . . . . . . . 75
4.7 High-degree (odd) cubature quadrature Kalman _lter . . . . 77
4.7.1 Approach . . . . . . . . . . . . . . . . . . . . . . . . . 77
4.7.2 High-degree cubature rule . . . . . . . . . . . . . . . . 77
4.7.3 High-degree cubature quadrature rule . . . . . . . . . 79
4.7.4 Calculation of HDCQ points and weights . . . . . . . 80
4.7.5 Illustrations . . . . . . . . . . . . . . . . . . . . . . . . 80
4.7.6 High-degree cubature quadrature Kalman _lter . . . . 86
4.8 Simulation examples . . . . . . . . . . . . . . . . . . . . . . . 87
4.8.1 Problem 1 . . . . . . . . . . . . . . . . . . . . . . . . . 87
4.8.2 Problem 2 . . . . . . . . . . . . . . . . . . . . . . . . . 91
4.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
5 Gauss-Hermite _lter 95
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
5.2 Gauss-Hermite rule of integration . . . . . . . . . . . . . . . 96
5.2.1 Single dimension . . . . . . . . . . . . . . . . . . . . . 96
5.2.2 Multidimensional integral . . . . . . . . . . . . . . . . 97
5.3 Sparse-grid Gauss-Hermite _lter (SGHF) . . . . . . . . . . . 99
5.3.1 Smolyak’s rule . . . . . . . . . . . . . . . . . . . . . . 100
5.4 Generation of points using moment matching method . . . . 104
5.5 Simulation examples . . . . . . . . . . . . . . . . . . . . . . . 105
5.5.1 Tracking an aircraft . . . . . . . . . . . . . . . . . . . 105
5.6 Multiple sparse-grid Gauss-Hermite _lter (MSGHF) . . . . . 109
5.6.1 State-space partitioning . . . . . . . . . . . . . . . . . 109
5.6.2 Bayesian _ltering formulation for multiple
approach . . . . . . . . . . . . . . . . . . . . . . . . . 110
5.6.3 Algorithm of MSGHF . . . . . . . . . . . . . . . . . . 111
5.6.4 Simulation example . . . . . . . . . . . . . . . . . . . 113
5.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116
6 Gaussian sum _lters 117
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
6.2 Gaussian sum approximation . . . . . . . . . . . . . . . . . . 118
6.2.1 Theoretical foundation . . . . . . . . . . . . . . . . . . 118
6.2.2 Implementation . . . . . . . . . . . . . . . . . . . . . . 120
6.2.3 Multidimensional systems . . . . . . . . . . . . . . . . 121
6.3 Gaussian sum _lter . . . . . . . . . . . . . . . . . . . . . . . 122
6.3.1 Time update . . . . . . . . . . . . . . . . . . . . . . . 122
6.3.2 Measurement update . . . . . . . . . . . . . . . . . . . 123
6.4 Adaptive Gaussian sum _ltering . . . . . . . . . . . . . . . . 124
6.5 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . 125
6.5.1 Problem 1: Single dimensional nonlinear system . . . . 125
6.5.2 RADAR target tracking problem . . . . . . . . . . . . 129
6.5.3 Estimation of harmonics . . . . . . . . . . . . . . . . . 133
6.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
7 Quadrature _lters with randomly delayed measurements 139
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
7.2 Kalman _lter for one step randomly delayed
measurements . . . . . . . . . . . . . . . . . . . . . . . . . . 140
7.3 Nonlinear _lters for one step randomly delayed
measurements . . . . . . . . . . . . . . . . . . . . . . . . . . 143
7.3.1 Assumptions . . . . . . . . . . . . . . . . . . . . . . . 144
7.3.2 Measurement noise estimation . . . . . . . . . . . . . 144
7.3.3 State estimation . . . . . . . . . . . . . . . . . . . . . 145
7.4 Nonlinear _lter for any arbitrary step randomly
delayed measurement . . . . . . . . . . . . . . . . . . . . . . 146
7.4.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . 153
7.5 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
7.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
8 Continuous-discrete _ltering 159
8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
8.2 Continuous time _ltering . . . . . . . . . . . . . . . . . . . . 160
8.2.1 Continuous _lter for a linear Gaussian system . . . . . 161
8.2.2 Nonlinear continuous time system . . . . . . . . . . . 167
8.2.2.1 The extended Kalman-Bucy _lter . . . . . . 167
8.3 Continuous-discrete _ltering . . . . . . . . . . . . . . . . . . 168
8.3.1 Nonlinear continuous time process model . . . . . . . 171
8.3.2 Discretization of process model using
Runge-Kutta method . . . . . . . . . . . . . . . . . . 172
8.3.3 Discretization using Ito-Taylor expansion of
order 1.5 . . . . . . . . . . . . . . . . . . . . . . . . . 172
8.3.4 Continuous-discrete _lter with deterministic
sample points . . . . . . . . . . . . . . . . . . . . . . . 174
8.4 Simulation examples . . . . . . . . . . . . . . . . . . . . . . . 176
8.4.1 Single dimensional _ltering problem . . . . . . . . . . 176
8.4.2 Estimation of harmonics . . . . . . . . . . . . . . . . . 177
8.4.3 RADAR target tracking problem . . . . . . . . . . . . 179
8.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
9 Case studies 187
9.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 187
9.2 Bearing only underwater target tracking problem . . . . . . 188
9.3 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . 189
9.3.1 Tracking scenarios . . . . . . . . . . . . . . . . . . . . 190
9.4 Shifted Rayleigh _lter (SRF) . . . . . . . . . . . . . . . . . . 191
9.5 Gaussian sum shifted Rayleigh _lter (GS-SRF) . . . . . . . . 193
9.5.1 Bearing density . . . . . . . . . . . . . . . . . . . . . . 194
9.6 Continuous-discrete shifted Rayleigh _lter
(CD-SRF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194
9.6.1 Time update of CD-SRF . . . . . . . . . . . . . . . . . 196
9.7 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . 196
9.7.1 Filter initialization . . . . . . . . . . . . . . . . . . . . 199
9.7.2 Performance criteria . . . . . . . . . . . . . . . . . . . 201
9.7.3 Performance analysis of Gaussian sum _lters . . . . . 201
9.7.4 Performance analysis of continuous-discrete _lters . . . . . . . 211
9.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215
9.9 Tracking of a ballistic target . . . . . . . . . . . . . . . . . . 216
9.10 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . 219
9.10.1 Process model . . . . . . . . . . . . . . . . . . . . . . 219
9.10.1.1 Process model in discrete domain . . . . . . 219
9.10.1.2 Process model in continuous time
domain . . . . . . . . . . . . . . . . . . . . . 220
9.10.2 Seeker measurement model . . . . . . . . . . . . . . . 220
9.10.3 Target acceleration model . . . . . . . . . . . . . . . . 223
9.11 Proportional navigation guidance (PNG) law . . . . . . . . . 225
9.12 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . 226
9.12.1 Performance of adaptive Gaussian sum _lters . . . . . 228
9.12.2 Performance of continuous-discrete _lters . . . . . . . 229
9.13 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 230
Bibliography 235
Index 251
Preface
This book deals with nonlinear state estimation. It is well known that, for a linear system and additive Gaussian noise, an optimal solution is available for the state estimation problem. This well known solution is known as the Kalman _lter. However, if the systems are nonlinear, the posterior and the prior probability density functions (pdfs) are no longer Gaussian. For such systems, no optimal solution is available in general. The primitive approach is to linearize the system and apply the Kalman _lter. The method is known as the extended Kalman _lter (EKF). However, the estimate fails to converge in many cases if the system is highly nonlinear. To overcome the limitations associated with the extended Kalman _lter, many techniques are proposed.
All the post-EKF techniques could be divided into two categories, namely (i) the estimation with probabilistic sample points and (ii) the estimation with deterministic sample points. The probabilistic sample point methods approximately reconstruct the posterior and the prior pdfs with the help of many points in the state space (also known as particles) sampled from an appropriate probability distribution and their associated probability weights. On the other hand, deterministic sample point techniques approximate the posterior and the prior pdfs with a multidimensional Gaussian distribution and calculate the mean and covariance with a few wisely chosen points and weights.
For this reason, they are also called Gaussian _lters. They are popular in real time applications due to their ease of implementation and faster execution, when compared to the techniques based on probabilistic sample points. There are good books on _ltering with probabilistic sample points, i.e., particle _ltering. However, the same is not true for approximate Gaussian _lters. Moreover, over the last few years there is considerable development on the said topic. This motivates us to write a book which presents a complete coverage of the Bayesian estimation with deterministic sample points. The purpose of the book is to educate the readers about all the available Gaussian estimators. Learning of various available methods becomes essential for a designer as in _ltering there is no `holy grail’ which will always provide the best result irrespective of the problems encountered. In other words, the best choice of estimator is highly problem speci_c.
There are prerequisites to understand the material presented in this book. These include (i) understanding of linear algebra and linear systems (ii) Bayesian probability theory, (iii) state space analysis. Assuming the readers are exposed to the above prerequisites, the book starts with the conceptual solution of the nonlinear estimation problems and describes all the Gaussian _lters in depth with rigorous mathematical analysis.
The style of writing is suitable for engineers and scientists. The material of the book is presented with the emphasis on key ideas, underlying assumptions behind them, algorithms, and properties. In this book, readers will get a comprehensive idea and understanding about the approximate solutions of the nonlinear estimation problem. The designers, who want to implement the _lters, will bene_t from the algorithms, ow charts and MATLABr code provided in the book. Rigorous, state of the art mathematical treatment will also be provided where relevant, for the analyst who wants to analyze the algorithm in depth for deeper understanding and further contribution. Further, beginners can verify their understanding with the help of numerical illustrations and MATLAB codes.
The book contains nine chapters. It starts with the formulation of the state estimation problem and the conceptual solution of it. Chapter 2 provides an optimal solution of the problem for a linear system and Gaussian noises. Further, it provides a detailed overview of several nonlinear estimators available in the literature. The next chapter deals with the unscented Kalman _lter. Chapters 4 and 5 describe cubature and quadrature based Kalman _lters, the Gauss-Hermite _lter and their variants respectively. The next chapter presents the Gaussian sum _lter, where the prior and the posterior pdfs are approximated with the weighted sum of several Gaussian pdfs. Chapter 7 considers the problem where measurements are randomly delayed. Such _lters are _nding more and more applications in networked control systems. Chapter 8 presents an estimation method for the continuous-discrete system. Such systems naturally arise because process equations are in continuous time domain as they are modeled from physical laws and the measurement equations are in discrete time domain as they arrive from the sampled sensor measurement. Finally, in the last chapter two case studies namely (i) bearing only underwater target tracking and (ii) tracking a ballistic target on reentry have been considered.
All the Gaussian _lters are applied to them and results are compared. Readers are suggested to start with the _rst two chapters because the rest of the book depends on them. Next, the reader can either read all the chapters from 3 to 6, or any of them (based on necessity). In other words, Chapters 3-6 are not dependent on one another. However, to read Chapters 7 to 9 understanding of the previous chapters is required.
This book is an outcome of many years of our research work, which was carried out with the active participation of our PhD students. We are thankful to them. Particularly, we would like to express our special appreciation and thanks to Dr Rahul Radhakrishnan and Dr Abhinoy Kumar Singh. Further, we thank anonymous reviewers, who reviewed our book proposal, for their constructive comments which help to uplift the quality of the book. We acknowledge the help of Mr Rajesh Kumar for drawing some of the _gures included in the book. Finally, we would like to acknowledge with gratitude, the support and love of our families who all help us to move forward and this book would not have been possible without them.
We hope that the book will make signi_cant contribution in the literature of Bayesian estimation and the readers will appreciate the e_ort. Further, it is anticipated that the book will open up many new avenues of both theoretical and applied research in various _elds of science and technology.