daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Book format changes.

Browse Source 
Moved book_format out of code to simplify importing it into the
notebooks. Added code to it to suppress future warnings from
matplotlib 1.4.3, and to set the numpy print precision to 3
decimal places.
This commit is contained in:
Roger Labbe 2015-11-25 13:29:40 -08:00
parent 7cd8e11b57
commit c0fe5e7de8
25 changed files with 4415 additions and 3745 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
00-Preface.ipynb
View File

@ -270,8 +270,6 @@
],
"source": [
"#format the book\n",
"import sys\n",
"sys.path.insert(0,'./code')\n",
"from book_format import load_style\n",
"load_style() "
]

98
01-g-h-filter.ipynb
View File

File diff suppressed because one or more lines are too long

121
02-Discrete-Bayes.ipynb
View File

File diff suppressed because one or more lines are too long

50
03-Gaussians.ipynb
View File

File diff suppressed because one or more lines are too long

243
04-One-Dimensional-Kalman-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

616
05-Multivariate-Gaussians.ipynb
View File

File diff suppressed because one or more lines are too long

188
06-Multivariate-Kalman-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

114
07-Kalman-Filter-Math.ipynb
View File

File diff suppressed because one or more lines are too long

331
08-Designing-Kalman-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

70
09-Nonlinear-Filtering.ipynb
View File

File diff suppressed because one or more lines are too long

153
10-Unscented-Kalman-Filter.ipynb
View File

File diff suppressed because one or more lines are too long

84
11-Extended-Kalman-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

97
12-Particle-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

52
13-Smoothing.ipynb
View File

File diff suppressed because one or more lines are too long

125
14-Adaptive-Filtering.ipynb
View File

File diff suppressed because one or more lines are too long

16
Appendix-A-Installation.ipynb
View File

@ -37,6 +37,7 @@
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
"@import url('http://fonts.googleapis.com/css?family=Vollkorn');\n",
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"@import url('http://fonts.googleapis.com/css?family=Fira_sans');\n",
"\n",
" div.cell{\n",
" width: 900px;\n",
@ -127,9 +128,10 @@
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Arimo',verdana,arial,sans-serif;\n",
" line-height: 125%;\n",
" font-size: 120%;\n",
" font-family: 'Fira sans', verdana,arial,sans-serif;\n",
" line-height: 150%;\n",
" font-size: 110%;\n",
" font-weight: 400;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
@ -242,7 +244,7 @@
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\"]\n",
" extensions: [\"AMSmath.js\", \"autobold.js\"]\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
@ -271,12 +273,8 @@
"source": [
"#format the book\n",
"%matplotlib inline\n",
"%load_ext autoreload\n",
"%autoreload 2 \n",
"from __future__ import division, print_function\n",
"import sys\n",
"sys.path.insert(0,'./code')\n",
"from book_format import load_style, set_figsize, figsize\n",
"from book_format import load_style\n",
"load_style()"
]
},

810
Appendix-B-Symbols-and-Notations.ipynb
View File

@ -1,405 +1,405 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"[Table of Contents](http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb)"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
"<style>\n",
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
"@import url('http://fonts.googleapis.com/css?family=Vollkorn');\n",
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"\n",
" div.cell{\n",
" width: 900px;\n",
" margin-left: 0% !important;\n",
" margin-right: auto;\n",
" }\n",
" div.text_cell code {\n",
" background: transparent;\n",
" color: #000000;\n",
" font-weight: 600;\n",
" font-size: 11pt;\n",
" font-style: bold;\n",
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
" }\n",
" h1 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
"\t}\n",
"\t\n",
" div.input_area {\n",
" background: #F6F6F9;\n",
" border: 1px solid #586e75;\n",
" }\n",
"\n",
" .text_cell_render h1 {\n",
" font-weight: 200;\n",
" font-size: 30pt;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" text-align: left;\n",
" }\n",
" .text_cell_render h2 {\n",
" font-weight: 200;\n",
" font-size: 16pt;\n",
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h3 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h3 {\n",
" font-weight: 200;\n",
" font-size: 14pt;\n",
" line-height: 100%;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 2em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" }\n",
" h4 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h4 {\n",
" font-weight: 100;\n",
" font-size: 14pt;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" h5 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h5 {\n",
" font-weight: 200;\n",
" font-style: normal;\n",
" color: #1d3b84;\n",
" font-size: 16pt;\n",
" margin-bottom: 0em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Arimo',verdana,arial,sans-serif;\n",
" line-height: 125%;\n",
" font-size: 120%;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" div.output_subarea.output_text.output_pyout {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_subarea.output_stream.output_stdout.output_text {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_wrapper{\n",
" margin-top:0.2em;\n",
" margin-bottom:0.2em;\n",
"}\n",
"\n",
" code{\n",
" font-size: 70%;\n",
" }\n",
" .rendered_html code{\n",
" background-color: transparent;\n",
" }\n",
" ul{\n",
" margin: 2em;\n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li li{\n",
" padding-left: 0.2em; \n",
" margin-bottom: 0.2em; \n",
" margin-top: 0.2em; \n",
" }\n",
" ol{\n",
" margin: 2em;\n",
" }\n",
" ol li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.2em; \n",
" }\n",
" a:link{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:visited{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:hover{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:focus{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:active{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" .rendered_html :link {\n",
" text-decoration: underline; \n",
" }\n",
" .rendered_html :hover {\n",
" text-decoration: none; \n",
" }\n",
" .rendered_html :visited {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :focus {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :active {\n",
" text-decoration: none;\n",
" }\n",
" .warning{\n",
" color: rgb( 240, 20, 20 )\n",
" } \n",
" hr {\n",
" color: #f3f3f3;\n",
" background-color: #f3f3f3;\n",
" height: 1px;\n",
" }\n",
" blockquote{\n",
" display:block;\n",
" background: #fcfcfc;\n",
" border-left: 5px solid #c76c0c;\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" width:680px;\n",
" padding: 10px 10px 10px 10px;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" blockquote p {\n",
" margin-bottom: 0;\n",
" line-height: 125%;\n",
" font-size: 100%;\n",
" }\n",
"</style>\n",
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\"]\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
" },\n",
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
" \"HTML-CSS\": {\n",
" scale:100,\n",
" availableFonts: [],\n",
" preferredFont:null,\n",
" webFont: \"TeX\",\n",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 1,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"#format the book\n",
"import sys\n",
"sys.path.insert(0,'./code')\n",
"from book_format import load_style\n",
"load_style()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Symbols and Notations\n",
"\n",
"Here is a collection of the notation used by various authors for the linear Kalman filter. I have ordered them in the same order so that you can compare and contrast the choices. I use the original variable names; I did not try to normalize them."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Labbe\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\overline{\\mathbf{x}} &= \\mathbf{F}\\hat{\\mathbf{x}} + \\mathbf{B}\\mathbf{u} \\\\\n",
"\\overline{\\mathbf{P}} &= \\mathbf{F} \\mathbf{P}\\mathbf{F}^\\mathsf{T} + \\textbf{Q} \\\\ \\\\\n",
"\\mathbf{y} &= \\mathbf{z} - \\mathbf{H}\\overline{\\textbf{x}} \\\\\n",
"\\mathbf{S} &= \\mathbf{H}\\overline{\\mathbf{P}}\\mathbf{H}^\\mathsf{T} + \\mathbf{R} \\\\\n",
"\\mathbf{K} &= \\overline{\\mathbf{P}}\\mathbf{H}^\\mathsf{T}\\mathbf{S}^{-1} \\\\\n",
"\\hat{\\textbf{x}} &= \\overline{\\mathbf{x}} +\\mathbf{K}\\mathbf{y} \\\\\n",
"\\mathbf{P} &= (\\mathbf{I}-\\mathbf{K}\\mathbf{H})\\overline{\\mathbf{P}}\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Wikipedia\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{\\textbf{x}}_{k\\mid k-1} &= \\textbf{F}_{k}\\hat{\\textbf{x}}_{k-1\\mid k-1} + \\textbf{B}_{k} \\textbf{u}_{k} \\\\\n",
"\\textbf{P}_{k\\mid k-1} &= \\textbf{F}_{k} \\textbf{P}_{k-1\\mid k-1} \\textbf{F}_{k}^{\\textsf{T}} + \\textbf{Q}_{k}\\\\\n",
"\\tilde{\\textbf{y}}_k &= \\textbf{z}_k - \\textbf{H}_k\\hat{\\textbf{x}}_{k\\mid k-1} \\\\\n",
"\\textbf{S}_k &= \\textbf{H}_k \\textbf{P}_{k\\mid k-1} \\textbf{H}_k^\\textsf{T} + \\textbf{R}_k \\\\\n",
"\\textbf{K}_k &= \\textbf{P}_{k\\mid k-1}\\textbf{H}_k^\\textsf{T}\\textbf{S}_k^{-1} \\\\\n",
"\\hat{\\textbf{x}}_{k\\mid k} &= \\hat{\\textbf{x}}_{k\\mid k-1} + \\textbf{K}_k\\tilde{\\textbf{y}}_k \\\\\n",
"\\textbf{P}_{k|k} &= (I - \\textbf{K}_k \\textbf{H}_k) \\textbf{P}_{k|k-1}\n",
"\\end{aligned}$$\n",
"\n",
"## Brookner\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"X^*_{n+1,n} &= \\Phi X^*_{n,n} \\\\\n",
"X^*_{n,n} &= X^*_{n,n-1} +H_n(Y_n - MX^*_{n,n-1}) \\\\\n",
"H_n &= S^*_{n,n-1}M^\\mathsf{T}[R_n + MS^*_{n,n-1}M^\\mathsf{T}]^{-1} \\\\\n",
"S^*_{n,n-1} &= \\Phi S^*_{n-1,n-1}\\Phi^\\mathsf{T} + Q_n \\\\\n",
"S^*_{n-1,n-1} &= (I-H_{n-1}M)S^*_{n-1,n-2}\n",
"\\end{aligned}$$\n",
"\n",
"## Gelb\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\underline{\\hat{x}}_k(-) &= \\Phi_{k-1} \\underline{\\hat{x}}_{k-1}(+) \\\\\n",
"\\underline{\\hat{x}}_k(+) &= \\underline{\\hat{x}}_k(-) +K_k[Z_k - H_k\\underline{\\hat{x}}_k(-)] \\\\\n",
"K_k &= P_k(-)H_k^\\mathsf{T}[H_kP_k(-)H_k^\\mathsf{T} + R_k]^{-1}\\\\\n",
"P_k(+) &= \\Phi_{k-1} P_{k-1}(+)\\Phi_{k-1}^\\mathsf{T} + Q_{k-1} \\\\\n",
"P_k(-) &= (I-K_kH_k)P_k(-)\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Brown\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{\\textbf{x}}^-_{k+1} &= \\mathbf{\\phi}_{k}\\hat{\\textbf{x}}_{k} \\\\\n",
"\\hat{\\textbf{x}}_k &= \\hat{\\textbf{x}}^-_k +\\textbf{K}_k[\\textbf{z}_k - \\textbf{H}_k\\hat{\\textbf{}x}^-_k] \\\\\n",
"\\textbf{K}_k &= \\textbf{P}^-_k\\textbf{H}_k^\\mathsf{T}[\\textbf{H}_k\\textbf{P}^-_k\\textbf{H}_k^T + \\textbf{R}_k]^{-1}\\\\\n",
"\\textbf{P}^-_{k+1} &= \\mathbf{\\phi}_k \\textbf{P}_k\\mathbf{\\phi}_k^\\mathsf{T} + \\textbf{Q}_{k} \\\\\n",
"\\mathbf{P}_k &= (\\mathbf{I}-\\mathbf{K}_k\\mathbf{H}_k)\\mathbf{P}^-_k\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Zarchan\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{x}_{k} &= \\Phi_{k}\\hat{x}_{k-1} + G_ku_{k-1} + K_k[z_k - H\\Phi_{k}\\hat{x}_{k-1} - HG_ku_{k-1} ] \\\\\n",
"M_{k} &= \\Phi_k P_{k-1}\\phi_k^\\mathsf{T} + Q_{k} \\\\\n",
"K_k &= M_kH^\\mathsf{T}[HM_kH^\\mathsf{T} + R_k]^{-1}\\\\\n",
"P_k &= (I-K_kH)M_k\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Bar-Shalom\n",
"\n",
"## Thrun"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Terminology\n",
"\n",
"### Bar-Shalom\n",
"\n",
"* State-space\n",
"* x: state vector\n",
"* u: input vector\n",
"* Process noise or plant noise\n",
"* system matrix: A (for $\\dot(x) = Ax + Bu + Dv$)\n",
"* F: state transition matrix\n",
"* H : Measurement matrix\n",
"* y : measurement residual\n",
"* $\\overline{P}$ : state prediction covariance\n",
"* P: updated state covariance\n",
"* S: innovatin covariance\n",
"* K(W): filter gain\n",
"* $\\hat{x}$ updated state estimate\n",
"\n",
"\n",
"smoothed state = retrodicted state\n",
"\n",
"x0 = initial estimate\n",
"\n",
"P0 : initial covariance\n"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.4.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"[Table of Contents](http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb)"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
"<style>\n",
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
"@import url('http://fonts.googleapis.com/css?family=Vollkorn');\n",
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"@import url('http://fonts.googleapis.com/css?family=Fira_sans');\n",
"\n",
" div.cell{\n",
" width: 900px;\n",
" margin-left: 0% !important;\n",
" margin-right: auto;\n",
" }\n",
" div.text_cell code {\n",
" background: transparent;\n",
" color: #000000;\n",
" font-weight: 600;\n",
" font-size: 11pt;\n",
" font-style: bold;\n",
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
" }\n",
" h1 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
"\t}\n",
"\t\n",
" div.input_area {\n",
" background: #F6F6F9;\n",
" border: 1px solid #586e75;\n",
" }\n",
"\n",
" .text_cell_render h1 {\n",
" font-weight: 200;\n",
" font-size: 30pt;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" text-align: left;\n",
" }\n",
" .text_cell_render h2 {\n",
" font-weight: 200;\n",
" font-size: 16pt;\n",
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h3 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h3 {\n",
" font-weight: 200;\n",
" font-size: 14pt;\n",
" line-height: 100%;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 2em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" }\n",
" h4 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h4 {\n",
" font-weight: 100;\n",
" font-size: 14pt;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" h5 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h5 {\n",
" font-weight: 200;\n",
" font-style: normal;\n",
" color: #1d3b84;\n",
" font-size: 16pt;\n",
" margin-bottom: 0em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Fira sans', verdana,arial,sans-serif;\n",
" line-height: 150%;\n",
" font-size: 110%;\n",
" font-weight: 400;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" div.output_subarea.output_text.output_pyout {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_subarea.output_stream.output_stdout.output_text {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_wrapper{\n",
" margin-top:0.2em;\n",
" margin-bottom:0.2em;\n",
"}\n",
"\n",
" code{\n",
" font-size: 70%;\n",
" }\n",
" .rendered_html code{\n",
" background-color: transparent;\n",
" }\n",
" ul{\n",
" margin: 2em;\n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li li{\n",
" padding-left: 0.2em; \n",
" margin-bottom: 0.2em; \n",
" margin-top: 0.2em; \n",
" }\n",
" ol{\n",
" margin: 2em;\n",
" }\n",
" ol li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.2em; \n",
" }\n",
" a:link{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:visited{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:hover{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:focus{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:active{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" .rendered_html :link {\n",
" text-decoration: underline; \n",
" }\n",
" .rendered_html :hover {\n",
" text-decoration: none; \n",
" }\n",
" .rendered_html :visited {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :focus {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :active {\n",
" text-decoration: none;\n",
" }\n",
" .warning{\n",
" color: rgb( 240, 20, 20 )\n",
" } \n",
" hr {\n",
" color: #f3f3f3;\n",
" background-color: #f3f3f3;\n",
" height: 1px;\n",
" }\n",
" blockquote{\n",
" display:block;\n",
" background: #fcfcfc;\n",
" border-left: 5px solid #c76c0c;\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" width:680px;\n",
" padding: 10px 10px 10px 10px;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" blockquote p {\n",
" margin-bottom: 0;\n",
" line-height: 125%;\n",
" font-size: 100%;\n",
" }\n",
"</style>\n",
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\", \"autobold.js\"]\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
" },\n",
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
" \"HTML-CSS\": {\n",
" scale:100,\n",
" availableFonts: [],\n",
" preferredFont:null,\n",
" webFont: \"TeX\",\n",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 1,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"#format the book\n",
"from book_format import load_style\n",
"load_style()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Symbols and Notations\n",
"\n",
"Here is a collection of the notation used by various authors for the linear Kalman filter. I have ordered them in the same order so that you can compare and contrast the choices. I use the original variable names; I did not try to normalize them."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Labbe\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\overline{\\mathbf{x}} &= \\mathbf{F}\\hat{\\mathbf{x}} + \\mathbf{B}\\mathbf{u} \\\\\n",
"\\overline{\\mathbf{P}} &= \\mathbf{F} \\mathbf{P}\\mathbf{F}^\\mathsf{T} + \\textbf{Q} \\\\ \\\\\n",
"\\mathbf{y} &= \\mathbf{z} - \\mathbf{H}\\overline{\\textbf{x}} \\\\\n",
"\\mathbf{S} &= \\mathbf{H}\\overline{\\mathbf{P}}\\mathbf{H}^\\mathsf{T} + \\mathbf{R} \\\\\n",
"\\mathbf{K} &= \\overline{\\mathbf{P}}\\mathbf{H}^\\mathsf{T}\\mathbf{S}^{-1} \\\\\n",
"\\hat{\\textbf{x}} &= \\overline{\\mathbf{x}} +\\mathbf{K}\\mathbf{y} \\\\\n",
"\\mathbf{P} &= (\\mathbf{I}-\\mathbf{K}\\mathbf{H})\\overline{\\mathbf{P}}\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Wikipedia\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{\\textbf{x}}_{k\\mid k-1} &= \\textbf{F}_{k}\\hat{\\textbf{x}}_{k-1\\mid k-1} + \\textbf{B}_{k} \\textbf{u}_{k} \\\\\n",
"\\textbf{P}_{k\\mid k-1} &= \\textbf{F}_{k} \\textbf{P}_{k-1\\mid k-1} \\textbf{F}_{k}^{\\textsf{T}} + \\textbf{Q}_{k}\\\\\n",
"\\tilde{\\textbf{y}}_k &= \\textbf{z}_k - \\textbf{H}_k\\hat{\\textbf{x}}_{k\\mid k-1} \\\\\n",
"\\textbf{S}_k &= \\textbf{H}_k \\textbf{P}_{k\\mid k-1} \\textbf{H}_k^\\textsf{T} + \\textbf{R}_k \\\\\n",
"\\textbf{K}_k &= \\textbf{P}_{k\\mid k-1}\\textbf{H}_k^\\textsf{T}\\textbf{S}_k^{-1} \\\\\n",
"\\hat{\\textbf{x}}_{k\\mid k} &= \\hat{\\textbf{x}}_{k\\mid k-1} + \\textbf{K}_k\\tilde{\\textbf{y}}_k \\\\\n",
"\\textbf{P}_{k|k} &= (I - \\textbf{K}_k \\textbf{H}_k) \\textbf{P}_{k|k-1}\n",
"\\end{aligned}$$\n",
"\n",
"## Brookner\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"X^*_{n+1,n} &= \\Phi X^*_{n,n} \\\\\n",
"X^*_{n,n} &= X^*_{n,n-1} +H_n(Y_n - MX^*_{n,n-1}) \\\\\n",
"H_n &= S^*_{n,n-1}M^\\mathsf{T}[R_n + MS^*_{n,n-1}M^\\mathsf{T}]^{-1} \\\\\n",
"S^*_{n,n-1} &= \\Phi S^*_{n-1,n-1}\\Phi^\\mathsf{T} + Q_n \\\\\n",
"S^*_{n-1,n-1} &= (I-H_{n-1}M)S^*_{n-1,n-2}\n",
"\\end{aligned}$$\n",
"\n",
"## Gelb\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\underline{\\hat{x}}_k(-) &= \\Phi_{k-1} \\underline{\\hat{x}}_{k-1}(+) \\\\\n",
"\\underline{\\hat{x}}_k(+) &= \\underline{\\hat{x}}_k(-) +K_k[Z_k - H_k\\underline{\\hat{x}}_k(-)] \\\\\n",
"K_k &= P_k(-)H_k^\\mathsf{T}[H_kP_k(-)H_k^\\mathsf{T} + R_k]^{-1}\\\\\n",
"P_k(+) &= \\Phi_{k-1} P_{k-1}(+)\\Phi_{k-1}^\\mathsf{T} + Q_{k-1} \\\\\n",
"P_k(-) &= (I-K_kH_k)P_k(-)\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Brown\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{\\textbf{x}}^-_{k+1} &= \\mathbf{\\phi}_{k}\\hat{\\textbf{x}}_{k} \\\\\n",
"\\hat{\\textbf{x}}_k &= \\hat{\\textbf{x}}^-_k +\\textbf{K}_k[\\textbf{z}_k - \\textbf{H}_k\\hat{\\textbf{}x}^-_k] \\\\\n",
"\\textbf{K}_k &= \\textbf{P}^-_k\\textbf{H}_k^\\mathsf{T}[\\textbf{H}_k\\textbf{P}^-_k\\textbf{H}_k^T + \\textbf{R}_k]^{-1}\\\\\n",
"\\textbf{P}^-_{k+1} &= \\mathbf{\\phi}_k \\textbf{P}_k\\mathbf{\\phi}_k^\\mathsf{T} + \\textbf{Q}_{k} \\\\\n",
"\\mathbf{P}_k &= (\\mathbf{I}-\\mathbf{K}_k\\mathbf{H}_k)\\mathbf{P}^-_k\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Zarchan\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{x}_{k} &= \\Phi_{k}\\hat{x}_{k-1} + G_ku_{k-1} + K_k[z_k - H\\Phi_{k}\\hat{x}_{k-1} - HG_ku_{k-1} ] \\\\\n",
"M_{k} &= \\Phi_k P_{k-1}\\phi_k^\\mathsf{T} + Q_{k} \\\\\n",
"K_k &= M_kH^\\mathsf{T}[HM_kH^\\mathsf{T} + R_k]^{-1}\\\\\n",
"P_k &= (I-K_kH)M_k\n",
"\\end{aligned}$$\n",
"\n",
"\n",
"## Bar-Shalom\n",
"\n",
"## Thrun"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Terminology\n",
"\n",
"### Bar-Shalom\n",
"\n",
"* State-space\n",
"* x: state vector\n",
"* u: input vector\n",
"* Process noise or plant noise\n",
"* system matrix: A (for $\\dot(x) = Ax + Bu + Dv$)\n",
"* F: state transition matrix\n",
"* H : Measurement matrix\n",
"* y : measurement residual\n",
"* $\\overline{P}$ : state prediction covariance\n",
"* P: updated state covariance\n",
"* S: innovatin covariance\n",
"* K(W): filter gain\n",
"* $\\hat{x}$ updated state estimate\n",
"\n",
"\n",
"smoothed state = retrodicted state\n",
"\n",
"x0 = initial estimate\n",
"\n",
"P0 : initial covariance\n"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.5.0"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

964
Appendix-C-Walking-Through-KF-Code.ipynb
View File

@ -1,483 +1,481 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"[Table of Contents](http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Walking through the Kalman Filter code"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
"<style>\n",
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
"@import url('http://fonts.googleapis.com/css?family=Vollkorn');\n",
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"\n",
" div.cell{\n",
" width: 900px;\n",
" margin-left: 0% !important;\n",
" margin-right: auto;\n",
" }\n",
" div.text_cell code {\n",
" background: transparent;\n",
" color: #000000;\n",
" font-weight: 600;\n",
" font-size: 11pt;\n",
" font-style: bold;\n",
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
" }\n",
" h1 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
"\t}\n",
"\t\n",
" div.input_area {\n",
" background: #F6F6F9;\n",
" border: 1px solid #586e75;\n",
" }\n",
"\n",
" .text_cell_render h1 {\n",
" font-weight: 200;\n",
" font-size: 30pt;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" text-align: left;\n",
" }\n",
" .text_cell_render h2 {\n",
" font-weight: 200;\n",
" font-size: 16pt;\n",
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h3 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h3 {\n",
" font-weight: 200;\n",
" font-size: 14pt;\n",
" line-height: 100%;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 2em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" }\n",
" h4 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h4 {\n",
" font-weight: 100;\n",
" font-size: 14pt;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" h5 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h5 {\n",
" font-weight: 200;\n",
" font-style: normal;\n",
" color: #1d3b84;\n",
" font-size: 16pt;\n",
" margin-bottom: 0em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Arimo',verdana,arial,sans-serif;\n",
" line-height: 125%;\n",
" font-size: 120%;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" div.output_subarea.output_text.output_pyout {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_subarea.output_stream.output_stdout.output_text {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_wrapper{\n",
" margin-top:0.2em;\n",
" margin-bottom:0.2em;\n",
"}\n",
"\n",
" code{\n",
" font-size: 70%;\n",
" }\n",
" .rendered_html code{\n",
" background-color: transparent;\n",
" }\n",
" ul{\n",
" margin: 2em;\n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li li{\n",
" padding-left: 0.2em; \n",
" margin-bottom: 0.2em; \n",
" margin-top: 0.2em; \n",
" }\n",
" ol{\n",
" margin: 2em;\n",
" }\n",
" ol li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.2em; \n",
" }\n",
" a:link{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:visited{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:hover{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:focus{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:active{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" .rendered_html :link {\n",
" text-decoration: underline; \n",
" }\n",
" .rendered_html :hover {\n",
" text-decoration: none; \n",
" }\n",
" .rendered_html :visited {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :focus {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :active {\n",
" text-decoration: none;\n",
" }\n",
" .warning{\n",
" color: rgb( 240, 20, 20 )\n",
" } \n",
" hr {\n",
" color: #f3f3f3;\n",
" background-color: #f3f3f3;\n",
" height: 1px;\n",
" }\n",
" blockquote{\n",
" display:block;\n",
" background: #fcfcfc;\n",
" border-left: 5px solid #c76c0c;\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" width:680px;\n",
" padding: 10px 10px 10px 10px;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" blockquote p {\n",
" margin-bottom: 0;\n",
" line-height: 125%;\n",
" font-size: 100%;\n",
" }\n",
"</style>\n",
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\"]\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
" },\n",
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
" \"HTML-CSS\": {\n",
" scale:100,\n",
" availableFonts: [],\n",
" preferredFont:null,\n",
" webFont: \"TeX\",\n",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 1,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"#format the book\n",
"%matplotlib inline\n",
"%load_ext autoreload\n",
"%autoreload 2 \n",
"from __future__ import division, print_function\n",
"import sys\n",
"sys.path.insert(0,'./code')\n",
"from book_format import load_style, set_figsize, figsize\n",
"load_style()"
]
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": [
"** author's note: this code is somewhat old. This section needs to be edited; I would not pay a lot of attention to it right now. **\n",
"\n",
"The kalman filter code that we are using is implemented in my Python library `FilterPy`. If you are interested in the full implementation of the filter you should look in `filterpy\\kalman\\kalman_filter.py`. In the following I will present a simplified implementation of the same code. The code in the library handles issues that are beyond the scope of this chapter, such as numerical stability and support for the extended Kalman filter, subject of a later chapter. \n",
"\n",
"The code is implemented as the class `KalmanFilter`. Some Python programmers are not a fan of object oriented (OO) Python, and eschew classes. I do not intend to enter into that battle other than to say that I have often seen OO abused. Here I use the class to encapsulate the data that is pertinent to the filter so that you do not have to store and pass around a half dozen variables everywhere.\n",
"\n",
"The method `__init__()` is used by Python to create the object. Here is the method \n",
"\n",
" def __init__(self, dim_x, dim_z):\n",
" \"\"\" Create a Kalman filter. You are responsible for setting the \n",
" various state variables to reasonable values; the defaults below will\n",
" not give you a functional filter.\n",
" \n",
" Parameters\n",
" ----------\n",
" dim_x : int\n",
" Number of state variables for the Kalman filter. For example, if\n",
" you are tracking the position and velocity of an object in two\n",
" dimensions, dim_x would be 4.\n",
" \n",
" This is used to set the default size of P, Q, and u\n",
" \n",
" dim_z : int\n",
" Number of of measurement inputs. For example, if the sensor\n",
" provides you with position in (x,y), dim_z would be 2. \n",
" \"\"\"\n",
" \n",
" self.dim_x = dim_x\n",
" self.dim_z = dim_z\n",
"\n",
" self.x = np.zeros((dim_x, 1)) # state\n",
" self.P = np.eye(dim_x) # uncertainty covariance\n",
" self.Q = np.eye(dim_x) # process uncertainty\n",
" self.u = 0 # control input vector\n",
" self.B = np.zeros((dim_x, 1))\n",
" self.F = 0 # state transition matrix\n",
" self.H = 0 # Measurement function\n",
" self.R = np.eye(dim_z) # state uncertainty\n",
"\n",
" # identity matrix. Do not alter this.\n",
" self._I = np.eye(dim_x)\n",
"\n",
"More than anything this method exists to document for you what the variable names are in the filter. To do anything useful with this filter you will have to modify most of these values. Some are set to useful values. For example, `R` is set to an identity matrix; if you want the diagonals of `R` to be 10. you may write (as we did earlier in this chapter) `my_filter.R += 10.`.\n",
"\n",
"The names used for each variable matches the math symbology used in this chapter. Thus, `self.P` is the covariance matrix, `self.x` is the state, and so on.\n",
"\n",
"The predict function implements the predict step of the Kalman equations, which are \n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\mathbf{x}^- &= \\mathbf{F x} + \\mathbf{B u} \\\\\n",
"\\mathbf{P}^- &= \\mathbf{FP{F}}^\\mathsf{T} + \\mathbf{Q} \n",
"\\end{aligned}\n",
"$$\n",
"\n",
"The corresponding code is\n",
"\n",
" def predict(self): \n",
" self.x = self.F.dot(self.x) + self.B.dot(self.u)\n",
" self.P = self.F.dot(self.P).dot(self.F.T) + self.Q\n",
"\n",
"I haven't discussed the use of NumPy much until now, but this method illustrates the power of that package. We use NumPy's `array` class to store our data and perform the linear algebra for our filters. `array` implements matrix multiplication using the `.dot()` method; if you use `*` you will get element-wise multiplication. As a heavy user of linear algebra this design is somewhat distressing as I use matrix multiplication far more often than element-wise multiplication. However, this design is due to historical developments in the library and we must live with it. The Python community has recognized this problem, and in Python 3.5 we will have the `@` operator to implement matrix multiplication. \n",
"\n",
"With that in mind, the Python code `self.F.dot(self.x)` implements the math expression $\\mathbf{F x}$.\n",
"\n",
"NumPy's `array` implements matrix transposition by using the `.T` property. Therefore, `F.T` is the python implementation of $\\mathbf{F}^\\mathsf{T}$.\n",
"\n",
"The `update()` method implements the update equations of the Kalman filter, which are\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\mathbf{y} &= \\mathbf{z} - \\mathbf{H}\\mathbf{x^-} \\\\\n",
"\\mathbf{K} &= \\mathbf{P} \\mathbf{H}^\\mathsf{T} (\\mathbf{H} \\mathbf{P^-} \\mathbf{H}^\\mathsf{T} +\\mathbf{R})^{-1} \\\\\n",
"\\mathbf{x} &= \\mathbf{x}^- + \\mathbf{K} \\mathbf{y} \\\\\n",
"\\mathbf{P} &= (\\mathbf{I} - \\mathbf{K} \\mathbf{H})\\mathbf{P^-}\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"The corresponding code is:\n",
"\n",
" def update(self, Z, R=None):\n",
" \"\"\"\n",
" Add a new measurement (Z) to the kalman filter. If Z is None, nothing\n",
" is changed.\n",
"\n",
" Optionally provide R to override the measurement noise for this\n",
" one call, otherwise self.R will be used.\n",
"\n",
" self.residual, self.S, and self.K are stored in case you want to\n",
" inspect these variables. Strictly speaking they are not part of the\n",
" output of the Kalman filter, however, it is often useful to know\n",
" what these values are in various scenarios.\n",
" \"\"\"\n",
"\n",
" if Z is None:\n",
" return\n",
"\n",
" if R is None:\n",
" R = self.R\n",
" elif np.isscalar(R):\n",
" R = np.eye(self.dim_z) * R\n",
"\n",
" # error (residual) between measurement and prediction\n",
" self.residual = Z - self.H.dot(self.x)\n",
"\n",
" # project system uncertainty into measurement space\n",
" self.S = self.H.dot(self.P).dot(self.H.T) + R\n",
"\n",
" # map system uncertainty into kalman gain\n",
" self.K = self.P.dot(self.H.T).dot(linalg.inv(self.S))\n",
"\n",
" # predict new x with residual scaled by the kalman gain\n",
" self.x += self.K.dot(self.residual)\n",
"\n",
" KH = self.K.dot(self.H)\n",
" I_KH = self._I - KH\n",
" self.P = (I_KH.dot(self.P.dot(I_KH.T)) +\n",
" self.K.dot(self.R.dot(self.K.T)))\n",
"\n",
"There are a few more complications in this piece of code compared to `predict()` but it should still be quite clear. \n",
"\n",
"The first complication are the lines:\n",
"\n",
" if Z is None:\n",
" return\n",
" \n",
"This just lets you deal with missing data in a natural way. It is typical to use `None` to indicate the absence of data. If there is no data for an update we skip the update equations. This bit of code means you can write something like:\n",
"\n",
" z = read_sensor() # may return None if no data\n",
" my_kf.update(z)\n",
" \n",
"instead of:\n",
" z = read_sensor()\n",
" if z is not None:\n",
" my_kf.update(z)\n",
" \n",
"Reasonable people will argue whether my choice is cleaner, or obscures the fact that we do not update if the measurement is `None`. Having written a lot of avionics code my proclivity is always to do the safe thing. If we pass 'None' into the function I do not want an exception to occur; instead, I want the reasonable thing to happen, which is to just return without doing anything. If you feel that my choice obscures that fact, go ahead and write the explicit `if` statement prior to calling `update()` and get the best of both worlds.\n",
"\n",
"The next bit of code lets you optionally pass in a value to override `R`. It is common for the sensor noise to vary over time; if it does you can pass in the value as the optional parameter `R`.\n",
"\n",
" if R is None:\n",
" R = self.R\n",
" elif np.isscalar(R):\n",
" R = np.eye(self.dim_z) * R\n",
" \n",
"This code will use self.R if you do not provide a value for `R`. If you did provide a value, it will check if you provided a scalar (number); if so it constructs a matrix of the correct dimension for you. Otherwise it assumes that you passed in a matrix of the correct dimension.\n",
"\n",
"The rest of the code implements the Kalman filter equations, with one exception. Instead of implementing \n",
"\n",
"$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}^-$$\n",
"\n",
"it implements the somewhat more complicated form \n",
"\n",
"$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}^-(\\mathbf{I} - \\mathbf{KH})^\\mathsf{T} + \\mathbf{KRK}^\\mathsf{T}$$.\n",
"\n",
"The reason for this altered equation is that it is more numerically stable than the former equation, at the cost of being a bit more expensive to compute. It is not always possible to find the optimal value for $\\text{K}$, in which case the former equation will not produce good results because it assumes optimality. The longer reformulation used in the code is derived from more general math that does not assume optimality, and hence provides good results for non-optimal filters (such as when we can not correctly model our measurement error).\n",
"\n",
"Various texts differ as to whether this form of the equation should always be used, or only used when you know you need it. I choose to expend a bit more processing power to ensure stability; if your hardware is very constrained and you are able to prove that the simpler equation is correct for your problem then you might choose to use it instead. Personally, I find that a risky approach and do not recommend it to non-experts. Brown's *Introduction to Random Signals and Applied Kalman Filtering* [3] discusses this issue in some detail, if you are interested."
]
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": []
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": []
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.4.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"[Table of Contents](http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Walking through the Kalman Filter code"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
"<style>\n",
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
"@import url('http://fonts.googleapis.com/css?family=Vollkorn');\n",
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"@import url('http://fonts.googleapis.com/css?family=Fira_sans');\n",
"\n",
" div.cell{\n",
" width: 900px;\n",
" margin-left: 0% !important;\n",
" margin-right: auto;\n",
" }\n",
" div.text_cell code {\n",
" background: transparent;\n",
" color: #000000;\n",
" font-weight: 600;\n",
" font-size: 11pt;\n",
" font-style: bold;\n",
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
" }\n",
" h1 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
"\t}\n",
"\t\n",
" div.input_area {\n",
" background: #F6F6F9;\n",
" border: 1px solid #586e75;\n",
" }\n",
"\n",
" .text_cell_render h1 {\n",
" font-weight: 200;\n",
" font-size: 30pt;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" text-align: left;\n",
" }\n",
" .text_cell_render h2 {\n",
" font-weight: 200;\n",
" font-size: 16pt;\n",
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h3 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h3 {\n",
" font-weight: 200;\n",
" font-size: 14pt;\n",
" line-height: 100%;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 2em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" }\n",
" h4 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h4 {\n",
" font-weight: 100;\n",
" font-size: 14pt;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" h5 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h5 {\n",
" font-weight: 200;\n",
" font-style: normal;\n",
" color: #1d3b84;\n",
" font-size: 16pt;\n",
" margin-bottom: 0em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Fira sans', verdana,arial,sans-serif;\n",
" line-height: 150%;\n",
" font-size: 110%;\n",
" font-weight: 400;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" div.output_subarea.output_text.output_pyout {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_subarea.output_stream.output_stdout.output_text {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_wrapper{\n",
" margin-top:0.2em;\n",
" margin-bottom:0.2em;\n",
"}\n",
"\n",
" code{\n",
" font-size: 70%;\n",
" }\n",
" .rendered_html code{\n",
" background-color: transparent;\n",
" }\n",
" ul{\n",
" margin: 2em;\n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li li{\n",
" padding-left: 0.2em; \n",
" margin-bottom: 0.2em; \n",
" margin-top: 0.2em; \n",
" }\n",
" ol{\n",
" margin: 2em;\n",
" }\n",
" ol li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.2em; \n",
" }\n",
" a:link{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:visited{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:hover{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:focus{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:active{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" .rendered_html :link {\n",
" text-decoration: underline; \n",
" }\n",
" .rendered_html :hover {\n",
" text-decoration: none; \n",
" }\n",
" .rendered_html :visited {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :focus {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :active {\n",
" text-decoration: none;\n",
" }\n",
" .warning{\n",
" color: rgb( 240, 20, 20 )\n",
" } \n",
" hr {\n",
" color: #f3f3f3;\n",
" background-color: #f3f3f3;\n",
" height: 1px;\n",
" }\n",
" blockquote{\n",
" display:block;\n",
" background: #fcfcfc;\n",
" border-left: 5px solid #c76c0c;\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" width:680px;\n",
" padding: 10px 10px 10px 10px;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" blockquote p {\n",
" margin-bottom: 0;\n",
" line-height: 125%;\n",
" font-size: 100%;\n",
" }\n",
"</style>\n",
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\", \"autobold.js\"]\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
" },\n",
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
" \"HTML-CSS\": {\n",
" scale:100,\n",
" availableFonts: [],\n",
" preferredFont:null,\n",
" webFont: \"TeX\",\n",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 1,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"#format the book\n",
"%matplotlib inline\n",
"from __future__ import division, print_function\n",
"from book_format import load_style\n",
"load_style()"
]
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": [
"** author's note: this code is somewhat old. This section needs to be edited; I would not pay a lot of attention to it right now. **\n",
"\n",
"The kalman filter code that we are using is implemented in my Python library `FilterPy`. If you are interested in the full implementation of the filter you should look in `filterpy\\kalman\\kalman_filter.py`. In the following I will present a simplified implementation of the same code. The code in the library handles issues that are beyond the scope of this chapter, such as numerical stability and support for the extended Kalman filter, subject of a later chapter. \n",
"\n",
"The code is implemented as the class `KalmanFilter`. Some Python programmers are not a fan of object oriented (OO) Python, and eschew classes. I do not intend to enter into that battle other than to say that I have often seen OO abused. Here I use the class to encapsulate the data that is pertinent to the filter so that you do not have to store and pass around a half dozen variables everywhere.\n",
"\n",
"The method `__init__()` is used by Python to create the object. Here is the method \n",
"\n",
" def __init__(self, dim_x, dim_z):\n",
" \"\"\" Create a Kalman filter. You are responsible for setting the \n",
" various state variables to reasonable values; the defaults below will\n",
" not give you a functional filter.\n",
" \n",
" Parameters\n",
" ----------\n",
" dim_x : int\n",
" Number of state variables for the Kalman filter. For example, if\n",
" you are tracking the position and velocity of an object in two\n",
" dimensions, dim_x would be 4.\n",
" \n",
" This is used to set the default size of P, Q, and u\n",
" \n",
" dim_z : int\n",
" Number of of measurement inputs. For example, if the sensor\n",
" provides you with position in (x,y), dim_z would be 2. \n",
" \"\"\"\n",
" \n",
" self.dim_x = dim_x\n",
" self.dim_z = dim_z\n",
"\n",
" self.x = np.zeros((dim_x, 1)) # state\n",
" self.P = np.eye(dim_x) # uncertainty covariance\n",
" self.Q = np.eye(dim_x) # process uncertainty\n",
" self.u = 0 # control input vector\n",
" self.B = np.zeros((dim_x, 1))\n",
" self.F = 0 # state transition matrix\n",
" self.H = 0 # Measurement function\n",
" self.R = np.eye(dim_z) # state uncertainty\n",
"\n",
" # identity matrix. Do not alter this.\n",
" self._I = np.eye(dim_x)\n",
"\n",
"More than anything this method exists to document for you what the variable names are in the filter. To do anything useful with this filter you will have to modify most of these values. Some are set to useful values. For example, `R` is set to an identity matrix; if you want the diagonals of `R` to be 10. you may write (as we did earlier in this chapter) `my_filter.R += 10.`.\n",
"\n",
"The names used for each variable matches the math symbology used in this chapter. Thus, `self.P` is the covariance matrix, `self.x` is the state, and so on.\n",
"\n",
"The predict function implements the predict step of the Kalman equations, which are \n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\mathbf{x}^- &= \\mathbf{F x} + \\mathbf{B u} \\\\\n",
"\\mathbf{P}^- &= \\mathbf{FP{F}}^\\mathsf{T} + \\mathbf{Q} \n",
"\\end{aligned}\n",
"$$\n",
"\n",
"The corresponding code is\n",
"\n",
" def predict(self): \n",
" self.x = self.F.dot(self.x) + self.B.dot(self.u)\n",
" self.P = self.F.dot(self.P).dot(self.F.T) + self.Q\n",
"\n",
"I haven't discussed the use of NumPy much until now, but this method illustrates the power of that package. We use NumPy's `array` class to store our data and perform the linear algebra for our filters. `array` implements matrix multiplication using the `.dot()` method; if you use `*` you will get element-wise multiplication. As a heavy user of linear algebra this design is somewhat distressing as I use matrix multiplication far more often than element-wise multiplication. However, this design is due to historical developments in the library and we must live with it. The Python community has recognized this problem, and in Python 3.5 we will have the `@` operator to implement matrix multiplication. \n",
"\n",
"With that in mind, the Python code `self.F.dot(self.x)` implements the math expression $\\mathbf{F x}$.\n",
"\n",
"NumPy's `array` implements matrix transposition by using the `.T` property. Therefore, `F.T` is the python implementation of $\\mathbf{F}^\\mathsf{T}$.\n",
"\n",
"The `update()` method implements the update equations of the Kalman filter, which are\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\mathbf{y} &= \\mathbf{z} - \\mathbf{H}\\mathbf{x^-} \\\\\n",
"\\mathbf{K} &= \\mathbf{P} \\mathbf{H}^\\mathsf{T} (\\mathbf{H} \\mathbf{P^-} \\mathbf{H}^\\mathsf{T} +\\mathbf{R})^{-1} \\\\\n",
"\\mathbf{x} &= \\mathbf{x}^- + \\mathbf{K} \\mathbf{y} \\\\\n",
"\\mathbf{P} &= (\\mathbf{I} - \\mathbf{K} \\mathbf{H})\\mathbf{P^-}\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"The corresponding code is:\n",
"\n",
" def update(self, Z, R=None):\n",
" \"\"\"\n",
" Add a new measurement (Z) to the kalman filter. If Z is None, nothing\n",
" is changed.\n",
"\n",
" Optionally provide R to override the measurement noise for this\n",
" one call, otherwise self.R will be used.\n",
"\n",
" self.residual, self.S, and self.K are stored in case you want to\n",
" inspect these variables. Strictly speaking they are not part of the\n",
" output of the Kalman filter, however, it is often useful to know\n",
" what these values are in various scenarios.\n",
" \"\"\"\n",
"\n",
" if Z is None:\n",
" return\n",
"\n",
" if R is None:\n",
" R = self.R\n",
" elif np.isscalar(R):\n",
" R = np.eye(self.dim_z) * R\n",
"\n",
" # error (residual) between measurement and prediction\n",
" self.residual = Z - self.H.dot(self.x)\n",
"\n",
" # project system uncertainty into measurement space\n",
" self.S = self.H.dot(self.P).dot(self.H.T) + R\n",
"\n",
" # map system uncertainty into kalman gain\n",
" self.K = self.P.dot(self.H.T).dot(linalg.inv(self.S))\n",
"\n",
" # predict new x with residual scaled by the kalman gain\n",
" self.x += self.K.dot(self.residual)\n",
"\n",
" KH = self.K.dot(self.H)\n",
" I_KH = self._I - KH\n",
" self.P = (I_KH.dot(self.P.dot(I_KH.T)) +\n",
" self.K.dot(self.R.dot(self.K.T)))\n",
"\n",
"There are a few more complications in this piece of code compared to `predict()` but it should still be quite clear. \n",
"\n",
"The first complication are the lines:\n",
"\n",
" if Z is None:\n",
" return\n",
" \n",
"This just lets you deal with missing data in a natural way. It is typical to use `None` to indicate the absence of data. If there is no data for an update we skip the update equations. This bit of code means you can write something like:\n",
"\n",
" z = read_sensor() # may return None if no data\n",
" my_kf.update(z)\n",
" \n",
"instead of:\n",
" z = read_sensor()\n",
" if z is not None:\n",
" my_kf.update(z)\n",
" \n",
"Reasonable people will argue whether my choice is cleaner, or obscures the fact that we do not update if the measurement is `None`. Having written a lot of avionics code my proclivity is always to do the safe thing. If we pass 'None' into the function I do not want an exception to occur; instead, I want the reasonable thing to happen, which is to just return without doing anything. If you feel that my choice obscures that fact, go ahead and write the explicit `if` statement prior to calling `update()` and get the best of both worlds.\n",
"\n",
"The next bit of code lets you optionally pass in a value to override `R`. It is common for the sensor noise to vary over time; if it does you can pass in the value as the optional parameter `R`.\n",
"\n",
" if R is None:\n",
" R = self.R\n",
" elif np.isscalar(R):\n",
" R = np.eye(self.dim_z) * R\n",
" \n",
"This code will use self.R if you do not provide a value for `R`. If you did provide a value, it will check if you provided a scalar (number); if so it constructs a matrix of the correct dimension for you. Otherwise it assumes that you passed in a matrix of the correct dimension.\n",
"\n",
"The rest of the code implements the Kalman filter equations, with one exception. Instead of implementing \n",
"\n",
"$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}^-$$\n",
"\n",
"it implements the somewhat more complicated form \n",
"\n",
"$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}^-(\\mathbf{I} - \\mathbf{KH})^\\mathsf{T} + \\mathbf{KRK}^\\mathsf{T}$$.\n",
"\n",
"The reason for this altered equation is that it is more numerically stable than the former equation, at the cost of being a bit more expensive to compute. It is not always possible to find the optimal value for $\\text{K}$, in which case the former equation will not produce good results because it assumes optimality. The longer reformulation used in the code is derived from more general math that does not assume optimality, and hence provides good results for non-optimal filters (such as when we can not correctly model our measurement error).\n",
"\n",
"Various texts differ as to whether this form of the equation should always be used, or only used when you know you need it. I choose to expend a bit more processing power to ensure stability; if your hardware is very constrained and you are able to prove that the simpler equation is correct for your problem then you might choose to use it instead. Personally, I find that a risky approach and do not recommend it to non-experts. Brown's *Introduction to Random Signals and Applied Kalman Filtering* [3] discusses this issue in some detail, if you are interested."
]
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": []
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": []
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.5.0"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

742
Appendix-D-HInfinity-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

22
Appendix-E-Ensemble-Kalman-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

2948
Appendix-F-FilterPy-Code.ipynb
View File

File diff suppressed because it is too large Load Diff

8
Appendix-G-Designing-Nonlinear-Kalman-Filters.ipynb
View File

@ -262,12 +262,8 @@
"source": [
"#format the book\n",
"%matplotlib inline\n",
"%load_ext autoreload\n",
"%autoreload 2 \n",
"from __future__ import division, print_function\n",
"import sys\n",
"sys.path.insert(0,'./code')\n",
"from book_format import load_style, set_figsize, figsize\n",
"from book_format import load_style\n",
"load_style()"
]
},
@ -1012,7 +1008,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.4.1"
"version": "3.5.0"
}
},
"nbformat": 4,

17
Appendix-H-Least-Squares-Filters.ipynb
View File

@ -16,7 +16,7 @@
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 1,
"metadata": {
"collapsed": false
},
@ -120,8 +120,9 @@
" }\n",
" div.text_cell_render{\n",
" font-family: 'Fira sans', verdana,arial,sans-serif;\n",
" line-height: 125%;\n",
" font-size: 115%;\n",
" line-height: 150%;\n",
" font-size: 110%;\n",
" font-weight: 400;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
@ -234,7 +235,7 @@
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\"]\n",
" extensions: [\"AMSmath.js\", \"autobold.js\"]\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
@ -255,7 +256,7 @@
"<IPython.core.display.HTML object>"
]
},
"execution_count": 2,
"execution_count": 1,
"metadata": {},
"output_type": "execute_result"
}
@ -263,11 +264,7 @@
"source": [
"#format the book\n",
"%matplotlib inline\n",
"%load_ext autoreload\n",
"%autoreload 2 \n",
"from __future__ import division, print_function\n",
"import sys\n",
"sys.path.insert(0,'./code')\n",
"from book_format import load_style\n",
"load_style()"
]
@ -320,7 +317,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.4.3"
"version": "3.5.0"
}
},
"nbformat": 4,

6
Appendix-I-Analytic-Evaluation-of-Performance.ipynb
View File

@ -263,11 +263,7 @@
"source": [
"#format the book\n",
"%matplotlib inline\n",
"%load_ext autoreload\n",
"%autoreload 2 \n",
"from __future__ import division, print_function\n",
"import sys\n",
"sys.path.insert(0,'./code')\n",
"from book_format import load_style\n",
"load_style()"
]
@ -316,7 +312,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.4.3"
"version": "3.5.0"
}
},
"nbformat": 4,

283
code/book_format.py → book_format.py
View File

@ -1,138 +1,145 @@
# -*- coding: utf-8 -*-
"""Copyright 2015 Roger R Labbe Jr.
Code supporting the book
Kalman and Bayesian Filters in Python
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
This is licensed under an MIT license. See the LICENSE.txt file
for more information.
"""
from __future__ import (absolute_import, division, print_function,
unicode_literals)
from contextlib import contextmanager
from distutils.version import LooseVersion
from IPython.core.display import HTML
import json
import matplotlib
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
import numpy as np
import os.path
import sys
sys.path.insert(0, './code') # allow us to import book_format
def test_filterpy_version():
import filterpy
min_version = [0,0,28]
v = filterpy.__version__
tokens = v.split('.')
for i,v in enumerate(tokens):
if int(v) > min_version[i]:
return
i = len(tokens) - 1
if min_version[i] > int(tokens[i]):
raise Exception("Minimum FilterPy version supported is {}.{}.{}.\n"
"Please install a more recent version.\n"
" ex: pip install filterpy --upgrade".format(
*min_version))
v = int(tokens[0]*1000)
# ensure that we have the correct filterpy loaded. This is
# called when this module is imported at the top of each book
# chapter so the reader can see that they need to update FilterPy.
test_filterpy_version()
def equal_axis():
pylab.rcParams['figure.figsize'] = 10,10
plt.axis('equal')
def reset_axis():
pylab.rcParams['figure.figsize'] = 11, 4
def set_figsize(x=11, y=4):
pylab.rcParams['figure.figsize'] = x, y
@contextmanager
def figsize(x=11, y=4):
"""Temporarily set the figure size using 'with figsize(a,b):'"""
size = pylab.rcParams['figure.figsize']
set_figsize(x, y)
yield
pylab.rcParams['figure.figsize'] = size
@contextmanager
def numpy_precision(precision):
old = np.get_printoptions()['precision']
np.set_printoptions(precision=precision)
yield
np.set_printoptions(old)
@contextmanager
def printoptions(*args, **kwargs):
original = np.get_printoptions()
np.set_printoptions(*args, **kwargs)
yield
np.set_printoptions(**original)
def _decode_list(data):
rv = []
for item in data:
if isinstance(item, unicode):
item = item.encode('utf-8')
elif isinstance(item, list):
item = _decode_list(item)
elif isinstance(item, dict):
item = _decode_dict(item)
rv.append(item)
return rv
def _decode_dict(data):
rv = {}
for key, value in data.iteritems():
if isinstance(key, unicode):
key = key.encode('utf-8')
if isinstance(value, unicode):
value = value.encode('utf-8')
elif isinstance(value, list):
value = _decode_list(value)
elif isinstance(value, dict):
value = _decode_dict(value)
rv[key] = value
return rv
def load_style(directory = '.', name='code/custom.css'):
if sys.version_info[0] >= 3:
style = json.load(open(os.path.join(directory, "code/538.json")))
else:
style = json.load(open(directory + "/code/538.json"), object_hook=_decode_dict)
# matplotlib has deprecated the use of axes.color_cycle as of version
version = [int(version_no) for version_no in matplotlib.__version__.split('.')]
if version[0] > 1 or (version[0] == 1 and version[1] >= 5):
style["axes.prop_cycle"] = "cycler('color', ['#6d904f','#013afe', '#202020','#fc4f30','#e5ae38','#A60628','#30a2da','#008080','#7A68A6','#CF4457','#188487','#E24A33'])"
style.pop("axes.color_cycle", None)
plt.rcParams.update(style)
reset_axis ()
np.set_printoptions(suppress=True)
styles = open(os.path.join(directory, name), 'r').read()
return HTML(styles)
# -*- coding: utf-8 -*-
"""Copyright 2015 Roger R Labbe Jr.
Code supporting the book
Kalman and Bayesian Filters in Python
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
This is licensed under an MIT license. See the LICENSE.txt file
for more information.
"""
from __future__ import (absolute_import, division, print_function,
unicode_literals)
from contextlib import contextmanager
from distutils.version import LooseVersion
from IPython.core.display import HTML
import json
import matplotlib
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
import numpy as np
import os.path
import sys
import warnings
# version 1.4.3 of matplotlib has a bug that makes
# it issue a spurious warning on every plot that
# clutters the notebook output
if matplotlib.__version__ == '1.4.3':
warnings.simplefilter(action="ignore", category=FutureWarning)
np.set_printoptions(precision=3)
sys.path.insert(0, './code') # allow us to import book_format
def test_filterpy_version():
import filterpy
min_version = [0,0,28]
v = filterpy.__version__
tokens = v.split('.')
for i,v in enumerate(tokens):
if int(v) > min_version[i]:
return
i = len(tokens) - 1
if min_version[i] > int(tokens[i]):
raise Exception("Minimum FilterPy version supported is {}.{}.{}.\n"
"Please install a more recent version.\n"
" ex: pip install filterpy --upgrade".format(
*min_version))
v = int(tokens[0]*1000)
# ensure that we have the correct filterpy loaded. This is
# called when this module is imported at the top of each book
# chapter so the reader can see that they need to update FilterPy.
test_filterpy_version()
def equal_axis():
pylab.rcParams['figure.figsize'] = 10,10
plt.axis('equal')
def reset_axis():
pylab.rcParams['figure.figsize'] = 11, 4
def set_figsize(x=11, y=4):
pylab.rcParams['figure.figsize'] = x, y
@contextmanager
def figsize(x=11, y=4):
"""Temporarily set the figure size using 'with figsize(a,b):'"""
size = pylab.rcParams['figure.figsize']
set_figsize(x, y)
yield
pylab.rcParams['figure.figsize'] = size
@contextmanager
def numpy_precision(precision):
old = np.get_printoptions()['precision']
np.set_printoptions(precision=precision)
yield
np.set_printoptions(old)
@contextmanager
def printoptions(*args, **kwargs):
original = np.get_printoptions()
np.set_printoptions(*args, **kwargs)
yield
np.set_printoptions(**original)
def _decode_list(data):
rv = []
for item in data:
if isinstance(item, unicode):
item = item.encode('utf-8')
elif isinstance(item, list):
item = _decode_list(item)
elif isinstance(item, dict):
item = _decode_dict(item)
rv.append(item)
return rv
def _decode_dict(data):
rv = {}
for key, value in data.iteritems():
if isinstance(key, unicode):
key = key.encode('utf-8')
if isinstance(value, unicode):
value = value.encode('utf-8')
elif isinstance(value, list):
value = _decode_list(value)
elif isinstance(value, dict):
value = _decode_dict(value)
rv[key] = value
return rv
def load_style(directory = '.', name='code/custom.css'):
if sys.version_info[0] >= 3:
style = json.load(open(os.path.join(directory, "code/538.json")))
else:
style = json.load(open(directory + "/code/538.json"), object_hook=_decode_dict)
# matplotlib has deprecated the use of axes.color_cycle as of version
version = [int(version_no) for version_no in matplotlib.__version__.split('.')]
if version[0] > 1 or (version[0] == 1 and version[1] >= 5):
style["axes.prop_cycle"] = "cycler('color', ['#6d904f','#013afe', '#202020','#fc4f30','#e5ae38','#A60628','#30a2da','#008080','#7A68A6','#CF4457','#188487','#E24A33'])"
style.pop("axes.color_cycle", None)
plt.rcParams.update(style)
reset_axis ()
np.set_printoptions(suppress=True)
styles = open(os.path.join(directory, name), 'r').read()
return HTML(styles)