Kalman And Bayesian Filters In Python

2015·22.8 MB·English

by  Roger R Labbe Jr

Kalman and Bayesian Filters in Python
Roger R Labbe Jr
March 3, 2015
2
Contents
Preface 9
0.1 Reading Online . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
0.2 PDF Version . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
0.3 Downloading the book . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
0.4 Version 0.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
0.5 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
0.6 Installation and Software Requirements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
0.7 Provided Libraries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
0.8 Thoughts on Python and Coding Math. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
0.9 License. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
0.10 Contact . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
0.11 Resources . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1 The g-h Filter 17
1.1 Building Intuition via Thought Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.2 The g-h Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
1.3 Notation and Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
1.4 Exercise: Write Generic Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
1.4.1 Solution and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
1.5 Choice of g and h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
1.6 Exercise: create measurement function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
1.6.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
1.7 Exercise: Bad Initial Conditions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
1.7.1 Solution and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
1.8 Exercise: Extreme Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
1.8.1 Solution and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
1.9 Exercise: The Effect of Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
1.9.1 Solution and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
1.10 Exercise: Varying g . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
1.10.1 Solution and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
1.11 Varying h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
1.12 Tracking a Train . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
1.13 g-h Filters with FilterPy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
1.14 Final Thoughts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
1.15 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
1.16 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
2 Discrete Bayes Filter 53
2.1 Tracking a Dog . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
2.2 Extracting Information from Multiple Sensor Readings . . . . . . . . . . . . . . . . . . . . . . 55
2.3 Noisy Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
2.4 Incorporating Movement Data. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
3
4 CONTENTS
2.5 Adding Noise to the Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
2.6 Integrating Measurements and Movement Updates . . . . . . . . . . . . . . . . . . . . . . . . 62
2.7 The Effect of Bad Sensor Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
2.8 Drawbacks and Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
2.9 Generalizing to Multiple Dimensions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
2.10 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
2.11 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
3 Least Squares Filters 71
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
4 Gaussian Probabilities 73
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.2 Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
4.3 Gaussian Distributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.4 The Variance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
4.5 Interactive Gaussians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
4.6 Computational Properties of the Gaussian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
4.7 Computing Probabilities with scipy.stats . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
4.8 Fat Tails . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
4.9 Summary and Key Points . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
4.10 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
5 One Dimensional Kalman Filters 85
5.1 One Dimensional Kalman Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
5.2 Tracking A Dog . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
5.3 Math with Gaussians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
5.4 Implementing the Update Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
5.5 Implementing Predictions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
5.5.1 Animating the Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
5.6 Implementation in a Class . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
5.7 Relationship to the g-h Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
5.7.1 Exercise: Modify Variance Values . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
5.8 Introduction to Designing a Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
5.8.1 Animation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
5.9 Exercise(optional): . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
5.9.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
5.9.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
5.10 Explaining the Results - Multi-Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
5.11 More examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
5.11.1 Example: Extreme Amounts of Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
5.11.2 Example: Bad Initial Estimate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
5.11.3 Example: Large Noise and Bad Initial Estimate. . . . . . . . . . . . . . . . . . . . . . 115
5.12 Exercise: Interactive Plots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
5.12.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
5.13 Exercise - Nonlinear Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
5.13.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
5.13.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
5.14 Exercise - Noisy Nonlinear Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
5.14.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
5.14.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
5.15 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
CONTENTS 5
6 Multivariate Kalman Filters 123
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
6.2 Multivariate Normal Distributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
6.3 Multiplying Multidimensional Gaussians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
6.4 Unobserved Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
6.5 Kalman Filter Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
6.6 The Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144
6.6.1 Kalman Equations Expressed as an Algorithm . . . . . . . . . . . . . . . . . . . . . . 145
6.7 Implementation in Python . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
6.8 Tracking a Dog . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
6.8.1 Step 1: Choose the State Variables and Set Initial Conditions . . . . . . . . . . . . . 150
6.8.2 Step 2: Design the State Transition Function . . . . . . . . . . . . . . . . . . . . . . 152
6.8.3 Step 3: Design the Motion Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
6.8.4 Step 4: Design the Measurement Function . . . . . . . . . . . . . . . . . . . . . . . . 153
6.8.5 Step 5: Design the Measurement Noise Matrix . . . . . . . . . . . . . . . . . . . . . . 154
6.8.6 Step 6: Design the Process Noise Matrix . . . . . . . . . . . . . . . . . . . . . . . . . 155
6.9 Implementing the Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
6.10 Compare to Univariate Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
6.10.1 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
6.11 Converting the Multivariate Equations to the Univariate Case . . . . . . . . . . . . . . . . . . 165
6.12 Exercise: Compare to a Filter That Incorporates Velocity . . . . . . . . . . . . . . . . . . . . 167
6.12.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
6.12.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169
6.13 Adjusting the Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169
6.14 A Detailed Examination of the Covariance Matrix . . . . . . . . . . . . . . . . . . . . . . . . 173
6.15 Question: Explain Ellipse Differences . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180
6.15.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180
6.16 Batch Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181
6.17 Smoothing the Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
6.18 Walking Through the KalmanFilter Code (Optional) . . . . . . . . . . . . . . . . . . . . . . . 185
6.19 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 188
7 Kalman Filter Math 189
7.1 Bayesian Probability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189
7.1.1 Bayes’ theorem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 190
7.2 Modeling a Dynamic System that Has Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191
7.3 Modeling Dynamic Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192
7.3.1 Why This is Hard . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193
7.3.2 Finding the Fundamental Matrix for Time Invariant Systems . . . . . . . . . . . . . . 194
7.3.3 Forming First Order Equations from Higher Order Equations . . . . . . . . . . . . . . 196
7.4 Walking Through the Kalman Filter Equations . . . . . . . . . . . . . . . . . . . . . . . . . . 196
7.5 Design of the Process Noise Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199
7.5.1 Continuous White Noise Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199
7.5.2 Piecewise White Noise Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
7.5.3 Using FilterPy to Compute Q . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202
7.5.4 Simplification of Q . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203
7.6 Numeric Integration of Differential Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . 204
7.6.1 Runge Kutta Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204
7.7 Iterative Least Squares for Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 206
7.7.1 Derivation of ILS Equations (Optional) . . . . . . . . . . . . . . . . . . . . . . . . . . 208
7.7.2 Implementing Iterative Least Squares . . . . . . . . . . . . . . . . . . . . . . . . . . . 210
7.8 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213
6 CONTENTS
8 Designing Kalman Filters 215
8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215
8.2 Tracking a Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215
8.2.1 Step 1: Choose the State Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 216
8.2.2 Step 2: Design State Transition Function . . . . . . . . . . . . . . . . . . . . . . . . . 217
8.2.3 Step 3: Design the Motion Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . 217
8.2.4 Step 4: Design the Measurement Function . . . . . . . . . . . . . . . . . . . . . . . . 218
8.2.5 Step 5: Design the Measurement Noise Matrix . . . . . . . . . . . . . . . . . . . . . . 218
8.2.6 Step 6: Design the Process Noise Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . 219
8.2.7 Step 7: Design Initial Conditions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 219
8.3 Implement the Filter Code. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 220
8.4 The Effect of Order. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224
8.4.1 Zero Order Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 226
8.4.2 First Order Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 226
8.4.3 Second Order Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 227
8.4.4 Evaluating the Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 228
8.5 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 244
8.5.1 Exercise: Can you Kalman Filter GPS outputs? . . . . . . . . . . . . . . . . . . . . . 250
8.5.2 Excercise: Prove that the PS improves the filter . . . . . . . . . . . . . . . . . . . . . 255
8.5.3 Exercise: Different Data Rates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257
8.6 Tracking a Ball . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 260
8.6.1 Step 1: Choose the State Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 262
8.6.2 Step 2: Design State Transition Function . . . . . . . . . . . . . . . . . . . . . . . . . 263
8.6.3 Interlude: Test State Transition. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263
8.6.4 Step 3: Design the Motion Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265
8.6.5 Step 4: Design the Measurement Function . . . . . . . . . . . . . . . . . . . . . . . . 265
8.6.6 Step 5: Design the Measurement Noise Matrix . . . . . . . . . . . . . . . . . . . . . . 265
8.6.7 Step 6: Design the Process Noise Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . 266
8.6.8 Step 7: Design the Initial Conditions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 266
8.7 Tracking a Ball in Air . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 268
8.7.1 Implementing Air Drag . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 268
8.8 Tracking Noisy Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 276
9 Nonlinear Filtering 279
9.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279
9.2 The Problem with Nonlinearity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 280
9.3 An Intuitive Look at the Problem. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 281
9.4 The Effect of Nonlinear Functions on Gaussians . . . . . . . . . . . . . . . . . . . . . . . . . . 282
9.5 A 2D Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 286
9.6 The Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 289
9.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 290
10 Unscented Kalman Filters 293
10.1 Choosing Sigma Points . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 294
10.2 Handling the Nonlinearities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 299
10.3 Sigma Point Algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 300
10.4 The Unscented Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 301
10.5 The Unscented Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 301
10.5.1 Predict Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 301
10.5.2 Update Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302
10.6 Using the UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 303
10.7 Tracking a Flying Airplane . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 306
10.7.1 First Attempt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 306
10.7.2 Tracking Manuevering Aircraft . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 311
CONTENTS 7
10.7.3 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314
10.7.4 Multiple Position Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 317
10.8 Exercise: Track a target moving in a circle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 320
10.8.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 320
10.8.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 322
10.9 Exercise: Sensor Position Effects . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 322
10.9.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 322
10.10Exercise: Compute Position Errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323
10.10.1Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323
10.10.2Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324
10.11Exercise: Explain Filter Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324
10.11.1Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324
10.12Exercise: Visualize Sigma Points . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 328
10.12.1Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 328
10.12.2Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 330
10.13Implementation of the UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 330
10.13.1Weights . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 331
10.13.2Sigma Points . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 331
10.13.3Predict Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 334
10.13.4Update Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 334
10.14Full Source from FilterPy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 335
10.15Batch Processing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345
10.16Smoothing the Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 348
10.17Choosing Values for kappa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 350
10.18The Skewed Unscented Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 351
10.19The Scaled Unscented Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 352
10.20Nonlinear State Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 352
10.21References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 352
11 The Extended Kalman Filter 353
11.1 The Problem with Nonlinearity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353
11.2 The Effect of Nonlinear Transfer Functions on Gaussians. . . . . . . . . . . . . . . . . . . . . 354
11.3 The Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 358
11.4 Example: Tracking a Flying Airplane. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 362
11.4.1 Design the State Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363
11.4.2 Design the System Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363
11.4.3 Design the Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364
11.5 Using SymPy to compute Jacobians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 369
11.6 Designing Q . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 369
11.7 Example: A falling Ball . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 370
12 Designing Nonlinear Kalman Filters 373
12.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 373
12.1.1 Kalman Filter with Air Drag . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 373
12.2 Realistic 2D Position Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 374
12.3 Linearizing the Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 380
12.4 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 381
13 Smoothing 383
13.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 383
13.2 Types of Smoothers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 385
13.3 Fixed Point Smoothing. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 386
13.4 Fixed Lag Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 386
13.5 Fixed Interval Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 386
8 CONTENTS
13.6 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 389
14 Adaptive Filtering 391
14.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 391
14.2 Maneuvering Targets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 391
14.3 Detecting a Maneuver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 400
14.4 Adjustable Process Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 402
14.4.1 Continuous Adjustment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 403
14.4.2 Continuous Adjustment - Standard Deviation Version . . . . . . . . . . . . . . . . . . 405
14.5 Fading Memory Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 412
14.6 Noise Level Switching . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 415
14.7 Variable State Dimension . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 415
14.8 Multiple Model Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 415
14.9 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 416
15 H filter 417
∞
16 Ensemble Kalman Filters 419
16.1 The Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 420
16.1.1 Initialize Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 420
16.1.2 Predict Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 421
16.1.3 Update Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 421
16.2 Implementation and Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 423
16.3 Outstanding Questions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 425
16.4 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 426
A Installation, Python, Numpy, and filterpy 427
A.1 Installing the SciPy Stack . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 427
A.1.1 Manual Install of the SciPy stack . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 428
A.2 Installing/downloading and running the book . . . . . . . . . . . . . . . . . . . . . . . . . . . 429
A.3 Using IPython Notebook. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 429
A.4 SymPy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 430
B Symbology 433
B.1 State . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433
B.2 State at step n . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433
B.3 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433
B.4 measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433
B.5 control transition Matrix. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433
C Nomenclature 435
C.1 Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
C.1.1 Brookner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
C.1.2 Gelb . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
C.1.3 Brown . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
C.1.4 Zarchan . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435
C.1.5 Wikipedia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 436
C.1.6 Labbe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 436
Preface
Introductory textbook for Kalman filters and Bayesian filters. All code is written in Python, and the book
itself is written in IPython Notebook so that you can run and modify the code in the book in place, seeing
the results inside the book. What better way to learn?
0.1 Reading Online
You may access this book via nbviewer at any time by using this address: Read Online Now
The quickest way to get starting with reading the book is to read it online using the link above. The
book is written as a collection of IPython Notebooks, an interactive, browser based system that allows you
tocombinetext,Python,andmathintoyourbrowser. Thewebsitehttp://nbviewer.orgprovidesanIPython
Notebook server that renders notebooks stored at github (or elsewhere). The rendering is done in real time
whenyouloadthebook. Ifyoureadmybooktoday,andthenImakeachangetomorrow,whenyougoback
tomorrowyouwillseethatchange. Perhapsmoreimportantly,thebookusesanimationstodemonstratehow
the algorithms perform over time. The PDF version of the book, discussed in the next paragraph, cannot
show the animations.
TheprefaceavailablefromthelinkabovehasalltheinformationinthisREADMEandmore, sofeelfree
to follow the link now.
IperiodicallygenerateaPDFofthebookfromtheNotebooks. Idonotdothisforeverycheckin, sothe
PDF will usually lag the content in github and on nbviewer.org. However, I do generate it whenever I make
a substantial change. Of course, you will not be able to run and modify the code in the notebooks, nor will
you be able to see the animations.
0.2 PDF Version
I periodically generate a PDF of the book from the Notebooks. I do not do this for every check in, so the
PDF will usually lag the content in github and on nbviewer.org. However, I do generate it whenever I make
a substantial change. Of course, you will not be able to run and modify the code in the notebooks, nor will
you be able to see the animations.
PDF Version of the book
0.3 Downloading the book
However, this book is intended to be interactive and I recommend using it in that form. If you install
IPython on your computer and then clone this book you will be able to run all of the code in the book
yourself. You can perform experiments, see how filters react to different data, see how different filters react
to the same data, and so on. I find this sort of immediate feedback both vital and invigorating. You do not
have to wonder “what happens if”. Try it and see!
The github pages for this project are at
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
9
10 CONTENTS
You can clone it to your hard drive with the command
git clone https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python.git
This will create a directory named Kalman-and-Bayesian-Filters-in-Python. Navigate to the direc-
tory, and run IPython notebook with the command
ipython notebook
This will open a browser window showing the contents of the base directory. The book is organized
into chapters. To read Chapter 2, click on the link for chapter 2. This will cause the browser to open that
subdirectory. IneachsubdirectorytherewillbeoneormoreIPythonNotebooks(allnotebookshavea.ipynb
file extension). The chapter contents are in the notebook with the same name as the chapter name. There
are sometimes supporting notebooks for doing things like generating animations that are displayed in the
chapter. These are not intended to be read by the end user, but of course if you are curious as to how an
animation is made go ahead and take a look.
Thisisadmittedlyasomewhatcumbersomeinterfacetoabook; Iamfollowinginthefootstepsofseveral
other projects that are somewhat re-purposing IPython Notebook to generate entire books. I feel the slight
annoyances have a huge payoff - instead of having to download a separate code base and run it in an IDE
while you try to read a book, all of the code and text is in one place. If you want to alter the code, you
may do so and immediately see the effects of your change. If you find a bug, you can make a fix, and push
it back to my repository so that everyone in the world benefits. And, of course, you will never encounter a
problem I face all the time with traditional books - the book and the code are out of sync with each other,
and you are left scratching your head as to which source to trust.
0.4 Version 0.0
Not ready for public consumption. In development.
author’s note: The chapter on g-h filters is fairly complete as far as planned content goes. The
content for the discrete Bayesian chapter, chapter 2, is also fairly complete. After that I have
questionsinmymindastothebestwaytopresentthestatisticsneededtounderstandthefilters.
I try to avoid the ‘dump a semester of math into 4 pages’ approach of most textbooks, but then
again perhaps I put things off a bit too long. In any case, the subsequent chapters are due a
strong editing cycle where I decide how to best develop these concepts. Otherwise I am pretty
happy with the content for the one dimensional and multidimensional Kalman filter chapters. I
know the code works, I am using it in real world projects at work, but there are areas where the
content about the covariance matrices is pretty bad. The implementation is fine, the description
is poor. Sorry. It will be corrected.
Beyond that the chapters are much more in a state of flux. Reader beware. My writing method-
ology is to just vomit out whatever is in my head, just to get material, and then go back and
think through presentation, test code, refine, and so on. Whatever is checked in in these later
chapters may be wrong and not ready for your use.
Finally, nothing has been spell checked or proof read yet. I with IPython Notebook had spell
check, but it doesn’t seem to.
0.5 Motivation
This is a book for programmers that have a need or interest in Kalman filtering. The motivation for this
book came out of my desire for a gentle introduction to Kalman filtering. I’m a software engineer that
spentalmosttwodecadesintheavionicsfield,andsoIhavealwaysbeen‘bumpingelbows’withtheKalman
filter, but never implemented one myself. They always has a fearsome reputation for difficulty, and I did
not have the requisite education. Everyone I met that did implement them had multiple graduate courses
on the topic and extensive industrial experience with them. As I moved into solving tracking problems with