PDF Archive

Easily share your PDF documents with your contacts, on the Web and Social Networks.

Send a file File manager PDF Toolbox Search Help Contact



Kalman and Bayesian Filters in Python .pdf



Original filename: Kalman_and_Bayesian_Filters_in_Python.pdf

This PDF 1.5 document has been generated by LaTeX with hyperref package / pdfTeX-1.40.15, and has been sent on pdf-archive.com on 22/05/2015 at 21:36, from IP address 40.140.x.x. The current document download page has been viewed 4543 times.
File size: 7.2 MB (402 pages).
Privacy: public file




Download original PDF file









Document preview


Kalman and Bayesian Filters in Python
Roger R Labbe Jr
May 22, 2015

2

Contents
Preface
0.1 Kalman and Bayesian Filters . . . . . .
0.2 Motivation . . . . . . . . . . . . . . . .
0.3 Reading Online . . . . . . . . . . . . . .
0.4 PDF Version . . . . . . . . . . . . . . .
0.5 Downloading the book . . . . . . . . . .
0.6 Installation and Software Requirements
0.7 Provided Libraries . . . . . . . . . . . .
0.8 Thoughts on Python and Coding Math .
0.9 License . . . . . . . . . . . . . . . . . . .
0.10 Contact . . . . . . . . . . . . . . . . . .
0.11 Resources . . . . . . . . . . . . . . . . .
1 The
1.1
1.2
1.3
1.4

1.5
1.6
1.7
1.8
1.9
1.10
1.11
1.12
1.13
1.14
1.15
1.16
1.17
1.18

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

11
11
12
13
13
13
14
15
15
17
17
17

g-h Filter
Building Intuition via Thought Experiments
The g-h Filter . . . . . . . . . . . . . . . . .
Notation and Nomenclature . . . . . . . . .
NumPy arrays . . . . . . . . . . . . . . . .
1.4.1 Excercise - Create arrays . . . . . .
1.4.2 Solution . . . . . . . . . . . . . . . .
Exercise: Write Generic Algorithm . . . . .
1.5.1 Solution and Discussion . . . . . . .
Choice of g and h . . . . . . . . . . . . . . .
Exercise: create measurement function . . .
1.7.1 Solution . . . . . . . . . . . . . . . .
Exercise: Bad Initial Conditions . . . . . .
1.8.1 Solution and Discussion . . . . . . .
Exercise: Extreme Noise . . . . . . . . . . .
1.9.1 Solution and Discussion . . . . . . .
Exercise: The Effect of Acceleration . . . .
1.10.1 Solution and Discussion . . . . . . .
Exercise: Varying g . . . . . . . . . . . . . .
1.11.1 Solution and Discussion . . . . . . .
Varying h . . . . . . . . . . . . . . . . . . .
Interactive Example . . . . . . . . . . . . .
Tracking a Train . . . . . . . . . . . . . . .
g-h Filters with FilterPy . . . . . . . . . . .
Final Thoughts . . . . . . . . . . . . . . . .
Summary . . . . . . . . . . . . . . . . . . .
References . . . . . . . . . . . . . . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

19
19
30
31
32
33
33
34
34
35
35
36
36
36
37
37
37
38
38
38
41
41
42
46
46
47
47

.
.
.
.
.
.
.
.
.
.
.

3

4

CONTENTS

2 Discrete Bayes Filter
2.1 Tracking a Dog . . . . . . . . . . . . . . . . . . . . . .
2.2 Extracting Information from Multiple Sensor Readings
2.3 Noisy Sensors . . . . . . . . . . . . . . . . . . . . . . .
2.4 Incorporating Movement Data . . . . . . . . . . . . . .
2.5 Adding Noise to the Prediction . . . . . . . . . . . . .
2.6 Generalizing with Convolution . . . . . . . . . . . . .
2.7 Integrating Measurements and Movement Updates . .
2.8 The Effect of Bad Sensor Data . . . . . . . . . . . . .
2.9 Drawbacks and Limitations . . . . . . . . . . . . . . .
2.10 Tracking and Control . . . . . . . . . . . . . . . . . . .
2.10.1 Simulating the Train Behavior . . . . . . . . .
2.11 Bayes Theorem . . . . . . . . . . . . . . . . . . . . . .
2.12 Total Probability Theorem . . . . . . . . . . . . . . .
2.13 Summary . . . . . . . . . . . . . . . . . . . . . . . . .
2.14 References . . . . . . . . . . . . . . . . . . . . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

49
49
50
51
53
54
56
57
60
61
62
63
66
67
68
69

3 Least Squares Filters
71
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
4 Gaussian Probabilities
4.1 Introduction . . . . . . . . . . . . . . . . . . . . .
4.2 Mean, Variance, and Standard Deviations . . . .
4.2.1 Random Variables . . . . . . . . . . . . .
4.2.2 The Mean of a Random Variable . . . . .
4.2.3 Standard Deviation of a Random Variable
4.2.4 Variance of a Random Variable . . . . . .
4.2.5 Why the Square of the Differences . . . .
4.3 Gaussians . . . . . . . . . . . . . . . . . . . . . .
4.4 Nomenclature . . . . . . . . . . . . . . . . . . . .
4.5 Gaussian Distributions . . . . . . . . . . . . . . .
4.6 The Variance . . . . . . . . . . . . . . . . . . . .
4.7 Interactive Gaussians . . . . . . . . . . . . . . . .
4.8 Computational Properties of the Gaussian . . . .
4.9 Computing Probabilities with scipy.stats . . . . .
4.10 Fat Tails . . . . . . . . . . . . . . . . . . . . . . .
4.11 Summary and Key Points . . . . . . . . . . . . .
4.12 References . . . . . . . . . . . . . . . . . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

73
73
73
73
73
74
77
77
78
79
80
81
83
84
84
85
88
88

5 One Dimensional Kalman Filters
5.1 One Dimensional Kalman Filters . . . .
5.2 Tracking A Dog . . . . . . . . . . . . . .
5.3 Math with Gaussians . . . . . . . . . . .
5.4 Implementing the Update Step . . . . .
5.5 Implementing Predictions . . . . . . . .
5.5.1 Animating the Tracking . . . . .
5.6 Implementation in a Class . . . . . . . .
5.7 Relationship to the g-h Filter . . . . . .
5.7.1 Exercise: Modify Variance Values
5.8 Introduction to Designing a Filter . . .
5.8.1 Animation . . . . . . . . . . . . .
5.9 Exercise(optional): . . . . . . . . . . . .
5.9.1 Solution . . . . . . . . . . . . . .
5.9.2 Discussion . . . . . . . . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

89
89
89
94
99
101
105
106
107
108
108
111
111
112
112

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

CONTENTS
5.10 Explaining the Results - Multi-Sensor Fusion . . . . . .
5.11 More examples . . . . . . . . . . . . . . . . . . . . . . .
5.11.1 Example: Extreme Amounts of Noise . . . . . .
5.11.2 Example: Bad Initial Estimate . . . . . . . . . .
5.11.3 Example: Large Noise and Bad Initial Estimate .
5.12 Exercise: Interactive Plots . . . . . . . . . . . . . . . . .
5.12.1 Solution . . . . . . . . . . . . . . . . . . . . . . .
5.13 Exercise - Nonlinear Systems . . . . . . . . . . . . . . .
5.13.1 Solution . . . . . . . . . . . . . . . . . . . . . . .
5.13.2 Discussion . . . . . . . . . . . . . . . . . . . . . .
5.14 Exercise - Noisy Nonlinear Systems . . . . . . . . . . . .
5.14.1 Solution . . . . . . . . . . . . . . . . . . . . . . .
5.14.2 Discussion . . . . . . . . . . . . . . . . . . . . . .
5.15 Summary . . . . . . . . . . . . . . . . . . . . . . . . . .

5
.
.
.
.
.
.
.
.
.
.
.
.
.
.

6 Multivariate Kalman Filters
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . .
6.2 Multivariate Normal Distributions . . . . . . . . . . . . .
6.3 Computing Means and Covariances . . . . . . . . . . . . .
6.3.1 Computing the Mean . . . . . . . . . . . . . . . .
6.3.2 Expected Value . . . . . . . . . . . . . . . . . . . .
6.3.3 Computing the Covariance . . . . . . . . . . . . .
6.4 Multiplying Multidimensional Gaussians . . . . . . . . . .
6.5 Unobserved Variables . . . . . . . . . . . . . . . . . . . .
6.6 Kalman Filter Algorithm . . . . . . . . . . . . . . . . . .
6.7 The Equations . . . . . . . . . . . . . . . . . . . . . . . .
6.7.1 Kalman Equations Expressed as an Algorithm . .
6.8 Implementation in Python . . . . . . . . . . . . . . . . . .
6.9 Tracking a Dog . . . . . . . . . . . . . . . . . . . . . . . .
6.9.1 Step 1: Choose the State Variables and Set Initial
6.9.2 Step 2: Design the State Transition Function . .
6.9.3 Step 3: Design the Control Function . . . . . . .
6.9.4 Step 4: Design the Measurement Function . . . .
6.9.5 Step 5: Design the Measurement Noise Matrix . .
6.9.6 Step 6: Design the Process Noise Matrix . . . . .
6.10 Implementing the Kalman Filter . . . . . . . . . . . . . .
6.11 Exercise: Show Effect of Unobserved Variables . . . . . .
6.11.1 Solution . . . . . . . . . . . . . . . . . . . . . . . .
6.11.2 Discussion . . . . . . . . . . . . . . . . . . . . . . .
6.12 Adjusting the Filter . . . . . . . . . . . . . . . . . . . . .
6.13 A Detailed Examination of the Covariance Matrix . . . .
6.14 Question: Explain Ellipse Differences . . . . . . . . . . . .
6.14.1 Solution . . . . . . . . . . . . . . . . . . . . . . . .
6.15 The Update Equations . . . . . . . . . . . . . . . . . . . .
6.16 Batch Processing . . . . . . . . . . . . . . . . . . . . . . .
6.17 Smoothing the Results . . . . . . . . . . . . . . . . . . . .
6.18 References . . . . . . . . . . . . . . . . . . . . . . . . . . .
7 Kalman Filter Math
7.1 Bayesian Probability . . . . . . . . . . . . . . . . . . . .
7.1.1 Bayes’ theorem . . . . . . . . . . . . . . . . . . .
7.2 Modeling a Dynamic System that Has Noise . . . . . . .
7.3 Walking Through the Kalman Filter Equations . . . . .
7.4 Converting the Multivariate Equations to the Univariate

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

112
114
114
115
116
118
119
120
120
121
121
121
122
122

. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
Conditions
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

123
123
124
131
131
132
132
135
141
145
146
146
147
150
150
151
153
153
154
155
159
164
165
166
166
170
176
176
177
178
179
181

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

183
183
184
185
186
189

. . .
. . .
. . .
. . .
Case

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

6

CONTENTS
7.4.1 Compare Performance to Univariate Kalman Filter . . . . . .
Modeling Dynamic Systems . . . . . . . . . . . . . . . . . . . . . . .
7.5.1 Why This is Hard . . . . . . . . . . . . . . . . . . . . . . . .
7.5.2 Finding the Fundamental Matrix for Time Invariant Systems
7.5.3 Taylor Series Expansion . . . . . . . . . . . . . . . . . . . . .
7.5.4 Linear Time Invariant Theory . . . . . . . . . . . . . . . . . .
7.5.5 Numerical Solutions . . . . . . . . . . . . . . . . . . . . . . .
7.5.6 Forming First Order Equations from Higher Order Equations
7.6 Design of the Process Noise Matrix . . . . . . . . . . . . . . . . . . .
7.6.1 Continuous White Noise Model . . . . . . . . . . . . . . . . .
7.6.2 Piecewise White Noise Model . . . . . . . . . . . . . . . . . .
7.6.3 Using FilterPy to Compute Q . . . . . . . . . . . . . . . . . .
7.6.4 Simplification of Q . . . . . . . . . . . . . . . . . . . . . . . .
7.7 Numeric Integration of Differential Equations . . . . . . . . . . . . .
7.8 Euler’s Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.8.1 Runge Kutta Methods . . . . . . . . . . . . . . . . . . . . . .
7.9 Iterative Least Squares for Sensor Fusion . . . . . . . . . . . . . . .
7.9.1 Derivation of ILS Equations (Optional) . . . . . . . . . . . .
7.9.2 Implementing Iterative Least Squares . . . . . . . . . . . . .
7.10 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.5

8 Designing Kalman Filters
8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . .
8.2 Tracking a Robot . . . . . . . . . . . . . . . . . . . . . .
8.2.1 Step 1: Choose the State Variables . . . . . . . .
8.2.2 Step 2: Design State Transition Function . . . .
8.2.3 Step 3: Design the Motion Function . . . . . . .
8.2.4 Step 4: Design the Measurement Function . . .
8.2.5 Step 5: Design the Measurement Noise Matrix .
8.2.6 Step 6: Design the Process Noise Matrix . . . . .
8.2.7 Step 7: Design Initial Conditions . . . . . . . .
8.3 Implement the Filter Code . . . . . . . . . . . . . . . . .
8.4 The Effect of Order . . . . . . . . . . . . . . . . . . . . .
8.4.1 Zero Order Kalman Filter . . . . . . . . . . . . .
8.4.2 First Order Kalman Filter . . . . . . . . . . . . .
8.4.3 Second Order Kalman Filter . . . . . . . . . . .
8.4.4 Evaluating the Performance . . . . . . . . . . . .
8.5 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . .
8.5.1 Exercise: Can you Kalman Filter GPS outputs?
8.5.2 Excercise: Prove that the PS improves the filter
8.5.3 Exercise: Different Data Rates . . . . . . . . . .
8.6 Tracking a Ball . . . . . . . . . . . . . . . . . . . . . . .
8.6.1 Step 1: Choose the State Variables . . . . . . . .
8.6.2 Step 2: Design State Transition Function . . . .
8.6.3 Step 3: Design the Control Input Function . . .
8.6.4 Step 4: Design the Measurement Function . . .
8.6.5 Step 5: Design the Measurement Noise Matrix .
8.6.6 Step 6: Design the Process Noise Matrix . . . . .
8.6.7 Step 7: Design the Initial Conditions . . . . . . .
8.7 Tracking a Ball in Air . . . . . . . . . . . . . . . . . . .
8.7.1 Implementing Air Drag . . . . . . . . . . . . . .
8.8 Tracking Noisy Data . . . . . . . . . . . . . . . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

191
194
194
195
196
198
198
199
200
200
202
203
204
205
206
208
210
212
214
217

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

219
219
219
220
221
221
221
222
223
223
224
227
229
230
231
232
245
251
254
256
258
260
261
262
263
263
263
263
265
265
271

CONTENTS
9 Nonlinear Filtering
9.1 Introduction . . . . . . . . . . . . . .
9.2 The Problem with Nonlinearity . . .
9.3 An Intuitive Look at the Problem . .
9.4 The Effect of Nonlinear Functions on
9.5 A 2D Example . . . . . . . . . . . .
9.6 The Algorithms . . . . . . . . . . . .
9.7 Summary . . . . . . . . . . . . . . .

7

. . . . . .
. . . . . .
. . . . . .
Gaussians
. . . . . .
. . . . . .
. . . . . .

10 Unscented Kalman Filters
10.1 Choosing Sigma Points . . . . . . . . . . .
10.2 Handling the Nonlinearities . . . . . . . .
10.3 Sigma Point Algorithm . . . . . . . . . . .
10.4 The Unscented Transform . . . . . . . . .
10.5 The Unscented Filter . . . . . . . . . . . .
10.5.1 Predict Step . . . . . . . . . . . .
10.5.2 Update Step . . . . . . . . . . . .
10.6 Using the UKF . . . . . . . . . . . . . . .
10.7 Tracking a Flying Airplane . . . . . . . .
10.7.1 First Attempt . . . . . . . . . . . .
10.7.2 Tracking Manuevering Aircraft . .
10.7.3 Sensor Fusion . . . . . . . . . . . .
10.7.4 Multiple Position Sensors . . . . .
10.8 Exercise: Track a target moving in a circle
10.8.1 Solution . . . . . . . . . . . . . . .
10.8.2 Discussion . . . . . . . . . . . . . .
10.9 Exercise: Sensor Position Effects . . . . .
10.9.1 Solution . . . . . . . . . . . . . . .
10.10Exercise: Compute Position Errors . . . .
10.10.1 Solution . . . . . . . . . . . . . . .
10.10.2 Discussion . . . . . . . . . . . . . .
10.11Exercise: Explain Filter Performance . . .
10.11.1 Solution . . . . . . . . . . . . . . .
10.12Exercise: Visualize Sigma Points . . . . .
10.12.1 Solution . . . . . . . . . . . . . . .
10.12.2 Discussion . . . . . . . . . . . . . .
10.13Implementation of the UKF . . . . . . . .
10.13.1 Weights . . . . . . . . . . . . . . .
10.13.2 Sigma Points . . . . . . . . . . . .
10.13.3 Predict Step . . . . . . . . . . . .
10.13.4 Update Step . . . . . . . . . . . .
10.14Batch Processing . . . . . . . . . . . . . .
10.15Smoothing the Results . . . . . . . . . . .
10.16Choosing Values for kappa . . . . . . . . .
10.17The Skewed Unscented Transform . . . .
10.18The Scaled Unscented Transform . . . . .
10.19Nonlinear State Variables . . . . . . . . .
10.20References . . . . . . . . . . . . . . . . . .

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

273
273
274
275
276
281
284
285

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

287
288
293
294
294
294
295
295
296
299
299
305
307
309
313
313
314
315
315
315
316
316
317
317
320
320
322
322
322
323
326
326
327
330
332
333
333
333
333

8

CONTENTS

11 The
11.1
11.2
11.3

Extended Kalman Filter
Linearizing the System Model . . . . .
Linearizing the Kalman Filter . . . . .
Example: Tracking a Flying Airplane .
11.3.1 Design the State Variables . . .
11.3.2 Design the System Model . . .
11.3.3 Design the Measurement Model
11.4 Using SymPy to compute Jacobians .
11.5 Designing Q . . . . . . . . . . . . . . .
11.6 Example: A falling Ball . . . . . . . .

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.

335
335
337
338
339
339
340
345
346
346

12 Designing Nonlinear Kalman Filters
12.1 Introduction . . . . . . . . . . . . . .
12.1.1 Kalman Filter with Air Drag
12.2 Realistic 2D Position Sensors . . . .
12.3 Linearizing the Kalman Filter . . . .
12.4 References . . . . . . . . . . . . . . .

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

.
.
.
.
.

349
349
349
350
356
357

13 Smoothing
13.1 Introduction . . . . . . . .
13.2 Types of Smoothers . . .
13.3 Fixed Point Smoothing . .
13.4 Fixed Lag Smoothing . .
13.5 Fixed Interval Smoothing
13.6 References . . . . . . . . .

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

359
359
361
361
361
362
364

. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
Version
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

.
.
.
.
.
.
.
.
.
.
.

365
365
365
373
375
375
378
384
387
387
387
388

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

14 Adaptive Filtering
14.1 Introduction . . . . . . . . . . . . . . . . .
14.2 Maneuvering Targets . . . . . . . . . . . .
14.3 Detecting a Maneuver . . . . . . . . . . .
14.4 Adjustable Process Noise . . . . . . . . .
14.4.1 Continuous Adjustment . . . . . .
14.4.2 Continuous Adjustment - Standard
14.5 Fading Memory Filter . . . . . . . . . . .
14.6 Noise Level Switching . . . . . . . . . . .
14.7 Variable State Dimension . . . . . . . . .
14.8 Multiple Model Estimation . . . . . . . .
14.9 References . . . . . . . . . . . . . . . . . .

. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
Deviation
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .

A Installation, Python, NumPy, and FilterPy
A.1 Installing the SciPy Stack . . . . . . . . . . .
A.2 Installing FilterPy . . . . . . . . . . . . . . .
A.2.1 Manual Install of the SciPy stack . . .
A.3 Installing/downloading and running the book
A.4 Using IPython Notebook . . . . . . . . . . . .
A.5 SymPy . . . . . . . . . . . . . . . . . . . . . .

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

.
.
.
.
.
.

389
389
390
390
391
391
392

B Symbology
B.1 State . . . . . . .
B.2 State at step n .
B.3 Prediction . . . .
B.4 measurement . .
B.5 control transition
B.6 Nomenclature . .
B.6.1 Equations

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

395
395
395
395
395
395
396
396

. . . . .
. . . . .
. . . . .
. . . . .
Matrix .
. . . . .
. . . . .

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

.
.
.
.
.
.
.

CONTENTS

9

B.6.2 Wikipedia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 396
B.6.3 Labbe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 397
C Walking through the Kalman Filter code

399


Related documents


PDF Document ijeas0405016
PDF Document 2i18 ijaet0118674 v6 iss6 2335 2341
PDF Document 45i15 ijaet0715656 v6 iss3 1424to1430
PDF Document ijetr2118
PDF Document 29i16 ijaet0916882 v6 iss4 1703to1711
PDF Document untitled pdf document 32


Related keywords