Preface xv
Acknowledgments xvii
List of Figures Xix
List of Tables xxv
PART I PRELIMINARIES
1 Introduction 3
1.1 Bayesian Inference 4
1.2 Bayesian Hierarchy of Estimation Methods 5
1.3 Scope of This Text 6
1.3.1 Objective 6
1.3.2 Chapter Overview and Prerequisites 6
1.4 Modeling and Simulation with MATLAB® 8
References 9
2 Preliminary Mathematical Concepts 11
2.1 A Very Brief Overview of Matrix Linear Algebra 11
2.1.1 Vector and Matrix Conventions and Notation 11
2.1.2 Sums and Products 12
2.1.3 Matrix Inversion 13
2.1.4 Block Matrix Inversion 14
2.1.5 Matrix Square Root 15
2.2 Vector Point Generators 16
2.3 Approximating Nonlinear Multidimensional Functions with Multidimensional Arguments 19
2.3.1 Approximating Scalar Nonlinear Functions 19
2.3.2 Approximating Multidimensional Nonlinear Functions 23
2.4 Overview of Multivariate Statistics 29
2.4.1 General Definitions 29
2.4.2 The Gaussian Density 32
References 40
3 General Concepts of Bayesian Estimation 42
3.1 Bayesian Estimation 43
3.2 Point Estimators 43
3.3 Introduction to Recursive Bayesian Filtering of Probability Density Functions 46
3.4 Introduction to Recursive Bayesian Estimation of the State Mean and Covariance 49
3.4.1 State Vector Prediction 50
3.4.2 State Vector Update 51
3.5 Discussion of General Estimation Methods 55
References 55
4 Case Studies: Preliminary Discussions 56
4.1 The Overall Simulation/Estimation/Evaluation Process 57
4.2 A Scenario Simulator for Tracking a Constant Velocity Target Through a DIFAR Buoy Field 58
4.2.1 Ship Dynamics Model 58
4.2.2 Multiple Buoy Observation Model 59
4.2.3 Scenario Specifics 59
4.3 DIFAR Buoy Signal Processing 62
4.4 The DIFAR Likelihood Function 67
References 69
PART II THE GAUSSIAN ASSUMPTION: A FAMILY OF KALMAN FILTER ESTIMATORS
5 The Gaussian Noise Case: Multidimensional Integration of Gaussian-Weighted Distributions 73
5.1 Summary of Important Results From Chapter 3 74
5.2 Derivation of the Kalman Filter Correction (Update) Equations Revisited 76
5.3 The General Bayesian Point Prediction Integrals for Gaussian Densities 78
5.3.1 Refining the Process Through an Affine Transformation 80
5.3.2 General Methodology for Solving Gaussian-Weighted Integrals 82
References 85
6 The Linear Class of Kalman Filters 86
6.1 Linear Dynamic Models 86
6.2 Linear Observation Models 87
6.3 The Linear Kalman Filter 88
6.4 Application of the LKF to DIFAR Buoy Bearing Estimation 88
References 92
7 The Analytical Linearization Class of Kalman Filters: The Extended Kalman Filter 93
7.1 One-Dimensional Consideration 93
7.1.1 One-Dimensional State Prediction 94
7.1.2 One-Dimensional State Estimation Error Variance Prediction 95
7.1.3 One-Dimensional Observation Prediction Equations 96
7.1.4 Transformation of One-Dimensional Prediction Equations 96
7.1.5 The One-Dimensional Linearized EKF Process 98
7.2 Multidimensional Consideration 98
7.2.1 The State Prediction Equation 99
7.2.2 The State Covariance Prediction Equation 100
7.2.3 Observation Prediction Equations 102
7.2.4 Transformation of Multidimensional Prediction Equations 103
7.2.5 The Linearized Multidimensional Extended Kalman Filter Process 105
7.2.6 Second-Order Extended Kalman Filter 105
7.3 An Alternate Derivation of the Multidimensional Covariance Prediction Equations 107
7.4 Application of the EKF to the DIFAR Ship Tracking Case Study 108
7.4.1 The Ship Motion Dynamics Model 108
7.4.2 The DIFAR Buoy Field Observation Model 109
7.4.3 Initialization for All Filters of the Kalman Filter Class 111
7.4.4 Choosing a Value for the Acceleration Noise 112
7.4.5 The EKF Tracking Filter Results 112
References 114
8 The Sigma Point Class: The Finite Difference Kalman Filter 115
8.1 One-Dimensional Finite Difference Kalman Filter 116
8.1.1 One-Dimensional Finite Difference State Prediction 116
8.1.2 One-Dimensional Finite Difference State Variance Prediction 117
8.1.3 One-Dimensional Finite Difference Observation Prediction Equations 118
8.1.4 The One-Dimensional Finite Difference Kalman Filter Process 118
8.1.5 Simplified One-Dimensional Finite Difference Prediction Equations 118
8.2 Multidimensional Finite Difference Kalman Filters 120
8.2.1 Multidimensional Finite Difference State Prediction 120
8.2.2 Multidimensional Finite Difference State Covariance Prediction 123
8.2.3 Multidimensional Finite Difference Observation Prediction Equations 124
8.2.4 The Multidimensional Finite Difference Kalman Filter Process 125
8.3 An Alternate Derivation of the Multidimensional Finite Difference Covariance Prediction Equations 125
References 127
9 The Sigma Point Class: The Unscented Kalman Filter 128
9.1 Introduction to Monomial Cubature Integration Rules 128
9.2 The Unscented Kalman Filter 130
9.2.1 Background 130
9.2.2 The UKF Developed 131
9.2.3 The UKF State Vector Prediction Equation 134
9.2.4 The UKF State Vector Covariance Prediction Equation 134
9.2.5 The UKF Observation Prediction Equations 135
9.2.6 The Unscented Kalman Filter Process 135
9.2.7 An Alternate Version of the Unscented Kalman Filter 135
9.3 Application of the UKF to the DIFAR Ship Tracking Case Study 137
References 138
10 The Sigma Point Class: The Spherical Simplex Kalman Filter 140
10.1 One-Dimensional Spherical Simplex Sigma Points 141
10.2 Two-Dimensional Spherical Simplex Sigma Points 142
10.3 Higher Dimensional Spherical Simplex Sigma Points 144
10.4 The Spherical Simplex Kalman Filter 144
10.5 The Spherical Simplex Kalman Filter Process 145
10.6 Application of the SSKF to the DIFAR Ship Tracking Case Study 146
Reference 147
11 The Sigma Point Class: The Gauss-Hermite Kalman Filter 148
11.1 One-Dimensional Gauss-Hermite Quadrature 149
11.2 One-Dimensional Gauss-Hermite Kalman Filter 153
11.3 Multidimensional Gauss-Hermite Kalman Filter 155
11.4 Sparse Grid Approximation for High Dimension/High Polynomial Order 160
11.5 Application of the GHKF to the DIFAR Ship Tracking Case Study 163
References 163
12 The Monte Carlo Kalman Filter 164
12.1 The Monte Carlo Kalman Filter 167
Reference 167
13 Summary of Gaussian Kalman Filters 168
13.1 Analytical Kalman Filters 168
13.2 Sigma Point Kalman Filters 170
13.3 A More Practical Approach to Utilizing the Family of Kalman Filters 174
References 175
14 Performance Measures for the Family of Kalman Filters 176
14.1 Error Ellipses 176
14.1.1 The Canonical Ellipse 177
14.1.2 Determining the Eigenvalues of P 178
14.1.3 Determining the Error Ellipse Rotation Angle 179
14.1.4 Determination of the Containment Area 180
14.1.5 Parametric Plotting of Error Ellipse 181
14.1.6 Error Ellipse Example 182
14.2 Root Mean Squared Errors 182
14.3 Divergent Tracks 183
14.4 Cramer-Rao Lower Bound 184
14.4.1 The One-Dimensional Case 184
14.4.2 The Multidimensional Case 186
14.4.3 A Recursive Approach to the CRLB 186
14.4.4 The Cramer-Rao Lower Bound for Gaussian Additive Noise 190
14.4.5 The Gaussian Cramer-Rao Lower Bound with Zero Process Noise 191
14.4.6 The Gaussian Cramer-Rao Lower Bound with Linear Models 191
14.5 Performance of Kalman Class DIFAR Track Estimators 192
References 198
PART III MONTE CARLO METHODS
15 Introduction to Monte Carlo Methods 201
15.1 Approximating a Density From a Set of Monte Carlo Samples 202
15.1.1 Generating Samples from a Two-Dimensional Gaussian Mixture Density 202
15.1.2 Approximating a Density by Its Multidimensional Histogram 202
15.1.3 Kernel Density Approximation 204
15.2 General Concepts Importance Sampling 210
15.3 Summary 215
References 216
16 Sequential Importance Sampling Particle Filters 218
16.1 General Concept of Sequential Importance Sampling 218
16.2 Resampling and Regularization (Move) for SIS Particle Filters 222
16.2.1 The Inverse Transform Method 222
16.2.2 SIS Particle Filter with Resampling 226
16.2.3 Regularization 227
16.3 The Bootstrap Particle Filter 230
16.3.1 Application of the BPF to DIFAR Buoy Tracking 231
16.4 The Optimal SIS Particle Filter 233
16.4.1 Gaussian Optimal SIS Particle Filter 235
16.4.2 Locally Linearized Gaussian Optimal SIS Particle Filter 236
16.5 The SIS Auxiliary Particle Filter 238
16.5.1 Application of the APF to DIFAR Buoy Tracking 242
16.6 Approximations to the SIS Auxiliary Particle Filter 243
16.6.1 The Extended Kalman Particle Filter 243
16.6.2 The Unscented Particle Filter 243
16.7 Reducing the Computational Load Through Rao-Blackwellization 245
References 245
17 The Generalized Monte Carlo Particle Filter 247
17.1 The Gaussian Particle Filter 248
17.2 The Combination Particle Filter 250
17.2.1 Application of the CPF-UKF to DIFAR Buoy Tracking 252
17.3 Performance Comparison of All DIFAR Tracking Filters 253
References 255
PART IV ADDITIONAL CASE STUDIES
18 A Spherical Constant Velocity Model for Target Tracking in Three Dimensions 259
18.1 Tracking a Target in Cartesian Coordinates 261
18.1.1 Object Dynamic Motion Model 262
18.1.2 Sensor Data Model 263
18.1.3 GaussianTracking Algorithms for a Cartesian StateVector 264
18.2 Tracking a Target in Spherical Coordinates 265
18.2.1 State Vector Position and Velocity Components in Spherical Coordinates 266
18.2.2 Spherical State Vector Dynamic Equation 267
18.2.3 Observation Equations with a Spherical State Vector 270
18.2.4 GaussianTracking Algorithms for a Spherical StateVector 270
18.3 Implementation of Cartesian and Spherical Tracking Filters 273
18.3.1 Setting Values for q 273
18.3.2 Simulating Radar Observation Data 274
18.3.3 Filter Initialization 276
18.4 Performance Comparison for Various Estimation Methods 278
18.4.1 Characteristics of the Trajectories Used for Performance Analysis 278
18.4.2 Filter Performance Comparisons 282
18.5 Some Observations and Future Considerations 293
APPENDIX 18.A Three-Dimensional Constant Turn Rate Kinematics 294
18.A.1 General Velocity Components for Constant Turn Rate Motion 294
18.A.2 General Position Components for Constant Turn Rate Motion 297
18.A.3 Combined Trajectory Transition Equation 299
18.A.4 Turn Rate Setting Based on a Desired Turn Acceleration 299
APPENDIX 18.B Three-Dimensional Coordinate Transformations 301
18.B.1 Cartesian-to-Spherical Transformation 302
18.B.2 Spherical-to-Cartesian Transformation 305
References 306
19 Tracking a Falling Rigid Body Using Photogrammetry 308
19.1 Introduction 308
19.2 The Process (Dynamic) Model for Rigid Body Motion 311
19.2.1 Dynamic Transition of the Translational Motion of a Rigid Body 311
19.2.2 Dynamic Transition of the Rotational Motion of a Rigid Body 313
19.2.3 Combined Dynamic Process Model 316
19.2.4 The Dynamic Process Noise Models 317
19.3 Components of the Observation Model 318
19.4 Estimation Methods 321
19.4.1 A Nonlinear Least Squares Estimation Method 321
19.4.2 An Unscented Kalman Filter Method 323
19.4.3 Estimation Using the Unscented Combination Particle Filter 325
19.4.4 Initializing the Estimator 326
19.5 The Generation of Synthetic Data 328
19.5.1 Synthetic Rigid Body Feature Points 328
19.5.2 Synthetic Trajectory 328
19.5.3 Synthetic Cameras 333
19.5.4 Synthetic Measurements 333
19.6 Performance Comparison Analysis 334
19.6.1 Filter Performance Comparison Methodology 335
19.6.2 Filter Comparison Results 338
19.6.3 Conclusions and Future Considerations 341
APPENDIX 19.A Quaternions Axis-Angle Vectors and Rotations 342
19.A.1 Conversions Between Rotation Representations 342
19.A.2 Representation of Orientation and Rotation 343
19.A.3 Point Rotations and Frame Rotations 344
References 345
20 Sensor Fusion Using Photogrammetric and Inertial Measurements 346
20.1 Introduction 346
20.2 The Process (Dynamic) Model for Rigid Body Motion 347
20.3 The Sensor Fusion Observational Model 348
20.3.1 The Inertial Measurement Unit Component of the Observation Model 348
20.3.2 The Photogrammetric Component of the Observation Model 350
20.3.3 The Combined Sensor Fusion Observation Model 351
20.4 The Generation of Synthetic Data 352
20.4.1 Synthetic Trajectory 352
20.4.2 Synthetic Cameras 352
20.4.3 Synthetic Measurements 352
20.5 Estimation Methods 354
20.5.1 Initial Value Problem Solver for IMU Data 354
20.6 Performance Comparison Analysis 357
20.6.1 Filter Performance Comparison Methodology 359
20.6.2 Filter Comparison Results 360
20.7 Conclusions 361
20.8 Future Work 362
References 364
Index 367
