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

Changes for latex rendering in Github.

Browse Source 
Github cannot handle certain formatting of latex, so fixing
it.
This commit is contained in:
Roger Labbe committed 2015-05-09 14:45:06 -07:00
1 parent f4909701f8
commit 65b4b59ba3
12 files changed
+12587 -12286
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

01_g-h_filter.ipynb
+1 -1
View File
@@ -11,7 +11,7 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"# The g-h Filter (alpha-beta filter)"
"# The g-h Filter"
]
},
{
04_Gaussians.ipynb
+1569 -1569
View File
File diff suppressed because it is too large. Load diff
07_Kalman_Filter_Math.ipynb
+2384 -2389
View File
File diff suppressed because it is too large. Load diff
08_Designing_Kalman_Filters.ipynb
+3875 -3872
View File
File diff suppressed because it is too large. Load diff
09_Nonlinear_Filtering.ipynb
+7 -2
View File
@@ -284,11 +284,11 @@
"which is a second order differential equation. It is not possible to write a linear equation for this equation, and therefore we cannot even formulate a design for the Kalman filter. \n",
"\n",
"The early adopters of Kalman filters needed to apply the filters to nonlinear problems and this fact was not lost on them. For example, radar is inherently nonlinear. Radars measure the slant range to an object, and we are typically interested in the aircraft's position over the ground. We invoke Pythagoras and get the nonlinear equation:\n",
"\n",
"$$x=\\sqrt{slant^2 - altitude^2}$$\n",
"\n",
"And then, of course, the behavior of the objects being tracked is also nonlinear. So shortly after the idea of the Kalman filter was published by Kalman people began working on how to extend the Kalman filter into nonlinear problems. \n",
"\n",
"\n",
"It is almost true to state that the only equation you know how to solve is $\\mathbf{Ax}=\\mathbf{b}$. I don't mean you the reader, but the entire mathematical community. We only really know how to do linear algebra. I can give you a linear equation and you can solve it. If I told you your job depended on knowing how to solve the equation $2x+3=4$ you would trivially solve it. If I then told you I needed to know the answer to $2x-3=8$ you would not break out into a sweat because we know all the rules for solving linear equations. I could write a program to randomly generate linear equations and you would be able to solve every one.\n",
"\n",
"Any of us with a formal mathematical education spent years learning various analytic ways to solve integrals, differential equations and so on. It was all a lie, nearly. Even trivial physical systems produce equations that no one in the world know how to solve analytically. I can give you an equation that you are able to integrate, multiply or add in an $ln$ term, and render it insolvable. \n",
@@ -314,10 +314,12 @@
"metadata": {},
"source": [
"As asserted in the introduction the only math you really know how to do is linear math. Equations of the form \n",
"\n",
"$$ A\\mathbf{x}=\\mathbf{b}$$.\n",
"\n",
"That may strike you as hyperbole. After all, in this book we have integrated a polynomial to get distance from velocity and time:\n",
" We know how to integrate a polynomial, for example, and so we are able to find the closed form equation for distance given velocity and time:\n",
"\n",
"$$\\int{(vt+v_0)}\\,dt = \\frac{a}{2}t^2+v_0t+d_0$$\n",
"\n",
"That's nonlinear. But it is also a very special form. You spent a lot of time, probably at least a year, learning how to integrate various terms, and you still can not integrate some arbitrary equation - no one can. We don't know how. If you took freshman Physics you perhaps remember homework involving sliding frictionless blocks on a plane and other toy problems. At the end of the course you were almost entirely unequipped to solve real world problems because the real world is nonlinear, and you were taught linear, closed forms of equations. It made the math tractable, but mostly useless. \n",
@@ -335,8 +337,9 @@
"$$\n",
"\n",
">which is clearly nonsense. Hence, an apparently linear equation such as\n",
">\n",
"$$L(f(t)) = f(t) + 1$$\n",
"\n",
">\n",
">is not linear because $L(0) = 1$! Be careful!"
]
},
@@ -526,6 +529,7 @@
"metadata": {},
"source": [
"This result may be somewhat surprising to you. The transfer function looks \"fairly\" linear - it is pretty close to a straight line, but the probability distribution of the output is completely different from a Gaussian. Recall the equations for multiplying two univariate Gaussians:\n",
"\n",
"$$\\begin{aligned}\n",
"\\mu =\\frac{\\sigma_1^2 \\mu_2 + \\sigma_2^2 \\mu_1} {\\sigma_1^2 + \\sigma_2^2}\\mbox{, } \n",
"\\sigma = \\frac{1}{\\frac{1}{\\sigma_1^2} + \\frac{1}{\\sigma_2^2}}\n",
@@ -594,6 +598,7 @@
"metadata": {},
"source": [
"Let's compare that to the linear function that passes through (-2,3) and (2,-3), which is very close to the nonlinear function we have plotted. Using the equation of a line we have\n",
"\n",
"$$m=\\frac{-3-3}{2-(-2)}=-1.5$$"
]
},
10_Unscented_Kalman_Filter.ipynb
+9 -6
View File
@@ -379,7 +379,9 @@
"$$\\begin{aligned}x&=x+y\\\\\n",
"y &= 0.1x^2 + y^2\\end{aligned}$$ \n",
"\n",
"for the mean and covariance $$\\mu = \\begin{bmatrix}0\\\\0\\end{bmatrix}, \n",
"for the mean and covariance \n",
"\n",
"$$\\mu = \\begin{bmatrix}0\\\\0\\end{bmatrix}, \n",
"\\Sigma=\\begin{bmatrix}32&15\\\\15&40\\end{bmatrix}$$"
]
},
@@ -658,14 +660,15 @@
"$$ \\mathcal{X}_0 = \\mu$$\n",
"\n",
"The corresponding weight for this sigma point is\n",
"\n",
"$$\n",
"W_0 = \\frac{\\kappa}{n+\\kappa}\n",
"$$\n",
"\n",
"where $n$ is the dimension of the problem, and $\\kappa$ is a scaling factor that will be discussed in a moment.\n",
"\n",
"The rest of the sigma points are defined to be\n",
"\n",
"\n",
"$$ \n",
"\\begin{aligned}\n",
"\\mathcal{X}_i &= \\mu + (\\sqrt{(n+\\kappa)\\Sigma})_i\\,\\,\\,\\, &\\text{for}\\text{ i=1 .. n} \\\\\n",
@@ -753,6 +756,7 @@
"$$\n",
"\n",
"This computes the mean and covariance represented by the green ellipse above, and corresponds with the linear Kalman filter equations of\n",
"\n",
"$$ \\begin{aligned}\n",
"\\mathbf{x}^- &= \\mathbf{Fx}\\\\\n",
"\\mathbf{P}^- &= \\mathbf{FPF}^T+\\mathbf{Q}\n",
@@ -820,6 +824,7 @@
"$$\\mathbf{P}_{xz} =\\sum W(\\mathcal{X}-x)(\\mathcal{X_z}-\\mathbf{x}_z)^\\mathsf{T}$$\n",
"\n",
"And then the Kalman gain is defined as\n",
"\n",
"$$K = \\mathbf{P}_{xz} \\mathbf{P}_z^{-1}$$\n",
"\n",
"Finally, we compute the new state estimate using the residual and Kalman gain:\n",
@@ -1453,10 +1458,10 @@
"\n",
"This requires the following change to the state transition function, which is still linear.\n",
"\n",
"\n",
"$$\\mathbf{x}^- = \\begin{bmatrix} 1 & \\Delta t & 0 &0 \\\\ 0& 1& 0 &0\\\\ 0&0&1&dt \\\\ 0&0&0&1\\end{bmatrix}\n",
"\\begin{bmatrix}x \\\\\\dot{x}\\\\ z\\\\ \\dot{z}\\end{bmatrix} \n",
"$$\n",
"\n",
"The measurement function stays the same, but we will have to alter Q to account for the state dimensionality change."
]
},
@@ -2699,7 +2704,6 @@
"\\end{aligned}\n",
"$$\n",
"\n",
"\n",
"The Python for this is not much more difficult once we wrap our heads around the $[\\sqrt{(n+\\kappa)\\Sigma}]_i$ term.\n",
"\n",
"The term $[\\sqrt{(n+\\kappa)\\Sigma}]_i$ has to be a matrix because $\\Sigma$ is a matrix. The subscript $i$ is choosing the column vector of the matrix. What is the 'square root of a matrix'? The usual definition is that the square root of a matrix $\\Sigma$ is just the matrix $S$ that, when multiplied by itself, yields $\\Sigma$.\n",
@@ -2738,6 +2742,7 @@
"metadata": {},
"source": [
"Now let's implement the unscented transform. Recall the equations\n",
"\n",
"$$\\begin{aligned}\n",
"\\mu &= \\sum_i w_i\\mathcal{X}_i\\;\\;\\;&(2) \\\\\n",
"\\Sigma &= \\sum_i w_i{(\\mathcal{X}_i-\\mu)(\\mathcal{X}_i-\\mu)^\\mathsf{T}}\\;\\;\\;&(3)\n",
@@ -2929,10 +2934,8 @@
"\n",
"The mean an covariance of those points is computed with the unscented transform. The residual and Kalman gain is then computed. The cross variance is computed as:\n",
"\n",
"\n",
"$$\\mathbf{P}_{xz} =\\sum W(\\mathcal{X}-x)(\\mathcal{X_z}-\\mathbf{x}_z)^\\mathsf{T}$$\n",
"\n",
"\n",
"Finally, we compute the new state estimate using the residual and Kalman gain:\n",
"\n",
"$$\\hat{\\mathbf{x}} = \\mathbf{x}^- + \\mathbf{Ky}$$\n",
11_Extended_Kalman_Filters.ipynb
+1171 -1174
View File
File diff suppressed because it is too large. Load diff
12_Designing_Nonlinear_Kalman_Filters.ipynb
+1
View File
@@ -461,6 +461,7 @@
"$$\n",
"\n",
"Which we compute as\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\frac{\\partial h_1}{\\partial x} &= ((x-x_A)^2 + (y-y_A)^2))^\\frac{1}{2} \\\\\n",
13_Particle_Filters.ipynb
+592 -305
View File
@@ -1,305 +1,592 @@
{
"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": [
"# Particle Filters"
]
},
{
"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: 850px;\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",
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\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: inline;\n",
" white-space: wrap;\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: nowrap;\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",
" width:740px;\n",
" margin-left:auto;\n",
" margin-right:auto;\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:85,\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 matplotlib.pyplot as plt\n",
"import book_format\n",
"book_format.load_style()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"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.3"
}
},
"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": [
"# Particle Filters"
]
},
{
"cell_type": "code",
"execution_count": 2,
"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: 850px;\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",
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\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: inline;\n",
" white-space: wrap;\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: nowrap;\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",
" width:740px;\n",
" margin-left:auto;\n",
" margin-right:auto;\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:85,\n",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 2,
"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 matplotlib.pyplot as plt\n",
"import book_format\n",
"book_format.load_style()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here is our problem. We have object moving in a space, and we want to track them. Maybe the objects are fighter jets and missiles in the sky, or maybe we are tracking people playing cricket in a field. It doesn't really matter. Which of the filters that we have learned can handle this problem? Well, none of them are ideal. Let's think about the characteristics of this problem. \n",
"\n",
"1. **multi-modal**: We want to track zero, one, or more than one object simultaneously.\n",
"\n",
"2. **occlusions**: One object can hide another, causing there to be only one measurement for multiple objects.\n",
"\n",
"3. **nonlinear behavior**: Aircraft are buffeted by winds, balls move in parabolas, and people collide into each other.\n",
"\n",
"4. **nonlinear measurements**: Radar gives us the distance to an object. Converting that to an (x,y,z) coordinate requires a square root, which is nonlinear.\n",
"\n",
"5. **non-Gaussian noise:** as objects move across a background the computer vision can mistake part of the background for the object. \n",
"\n",
"6. **continuous:** the object's position and velocity (i.e. the state space) can smoothly vary over time.\n",
"\n",
"7. **multivariate**: we want to track several attributes, such as position, velocity, turn rates, etc.\n",
"\n",
"None of the filters we have learned work well with all of these constraints. \n",
"\n",
"* **Discrete Bayes filter**: This has most of the attributes. It is multimodal, can handle nonlinear measurements, and can be made to work with nonlinear behavior (in this book we only handled the linear case). However, it is discrete, not continuous, and it is *univariate*, not multivariate.\n",
"\n",
"* **Kalman filter**: The Kalman filter produces optimal estimates for unimodal linear systems with Gaussian noise. None of these are true for our problem.\n",
"\n",
"* **Unscented Kalman filter**: The UKF handles nonlinear, continuous, multivariate problems. However, it is neither multimodal nor does it handle occlusions. It can handle noise that is modestly non-Gaussian, but does not do well with distributions that are very non-Gaussian.\n",
"\n",
"* **Extended Kalman filter**: The EKF has the same strengths and limitations as the UKF, except that is it even more sensitive to strong nonlinearities and non-Gaussian noise."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## some title\n",
"\n",
"The particle filter is easy to understand, and I could explain the entire algorithm to you now. But I want you to understand it within the context of the other Bayesian filters in this book. When you design a filter you first have to choose which filter to use, and understanding the relative strengths and weaknesses of each is important.\n",
"\n",
"So let's start with a chart we first saw in the *Nonlinear Filtering* chapter. We started with a Gaussian with a mean of 0 and a variance of 1 and created 500,000 random points fitting that distribution. We passed each of those points through a nonlinear function, and plotted the result."
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuIAAAFfCAYAAAALX6kJAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x77d9908>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import numpy as np\n",
"from numpy.random import normal\n",
"from nonlinear_plots import plot_transfer_func\n",
"\n",
"gaussian = (1., 1.)\n",
"data = normal(loc=gaussian[0], scale=gaussian[1], size=500000)\n",
"\n",
"def g(x):\n",
" return .2*x**3 - .8*x**2\n",
"\n",
"with book_format.figsize(y=5):\n",
" plot_transfer_func(data, g, gaussian)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The EKF deals with this problem by linearizing the function at $x$ by evaluating the derivative at x: $f'(x)$. This computes the slope of the function at $x$, which is a linear function. If the function is smooth and 'fairly' linear locally the EKF produces a reasonable but suboptimal result. We can see from the chart above how much error is incurred with this linearization. \n",
"\n",
"The UKF handles larger nonlinearities by taking $2n+1$ samples near $x$. These samples are passed through the function, and the mean and standard deviation of the transformed points are used as the new estimate for the system state. In general this provides better estimates than the EKF at the cost of more computation. \n",
"\n",
"The EKF represents the nonlinearity of the function with one point, and the UKF represents it with $2n+1$ points. This allows them to handle the nonlinearity of the function, but in the end both assume a Gaussian distribution by taking the mean and standard deviation of the sampled points. \n",
"\n",
"It has probably occurred to you that the code that I used to graph the solution above constitutes an accurate solution, and it is. In other words, if we take a *large enough* sample from any probability distribution and pass them through our function the result will be an accurate representation of the new mean and probability distribution. \n",
"\n",
"The drawback to this approach is the amount of computation required. The charts above create half a million points to achieve their accuracy, and this is only a 1D problem. A typical tracking problem might have 9 or more dimensions (in 3D: position, velocity, attitude). Even with today's computers the problem quickly becomes computational intractable for real time tracking."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## some title\n",
"\n",
"All of this brings us to the particle filter. Going back to the problem that opened the chapter, let's discuss tracking objects using computer vision. For the time being we will stay in 2 spatial dimensions just because it is easier to plot for the book. Assume we have computer vision software that identifies moving objects in a video and creates 'blobs' where each blob is a potential object. Each blob can have error - shadows or the background may make the blob larger or smaller than the actual object, and spurious data such as insects flying near the lens can cause outlier data. Assume we know the camera pose and a terrain map and thus have a function that converts the screen coordinates of a blob to a 2D world position. We want to track the position and velocity of all objects in the video.\n",
"\n",
"Taking insight from the discussion above we start by creating a few thousand *particles*. Each particle represents a possible belief of where an object is in the video. We want to track position and motion, so each particle will be assigned a random position and velocity. Here is a function that creates a particle in a square with a random velocity. We ignore units, trusting that the caller will use self consistant units such as MKS. Each particle is represented as a list with the structure `[x_position, y_position, speed, direction]`"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"from random import uniform\n",
"from math import pi\n",
"def make_particle(xrange=(0, 100), yrange=(0, 100), max_velocity=2.):\n",
" return [uniform(*xrange), uniform(*yrange), \n",
" uniform(0, max_velocity), uniform(0, 2*pi)]\n",
"\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here we create 1000 random plots and plot them."
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"particles = np.asarray([make_particle((0, 100), (0, 100), 2) for i in range(1000)])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now we can write code that moves the particle over a period of time."
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"def move(particles, t=1.):\n",
" vx = particles[:, 2]*np.cos(particles[:, 3])\n",
" vy = particles[:, 2]*np.sin(particles[:, 3])\n",
" particles[:, 0] = (particles[:, 0] + vx*t) % 100\n",
" particles[:, 1] = (particles[:, 1] + vy*t) % 100"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"So let's plot the particles, move them, and plot them again"
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAr8AAAD9CAYAAAC1Fh+gAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x77d9240>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"def plot_particles(particles, x_range=(0, 100), y_range=(0, 100)):\n",
" with book_format.figsize(4,4):\n",
" plt.scatter(particles[:, 0], particles[:, 1])\n",
" plt.axis('equal')\n",
" plt.xlim(*x_range)\n",
" plt.ylim(*y_range)\n",
" plt.gca().get_xaxis().set_ticks([])\n",
" plt.gca().get_yaxis().set_ticks([])\n",
"\n",
"plt.subplot(121)\n",
"plot_particles(particles)\n",
"move(particles, t=10)\n",
"plt.subplot(122)\n",
"plot_particles(particles)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"It is difficult to visualize the movement from two static images, so here is an animation.\n",
"<img src='animations/13_particle_move.gif'>"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"So we have a random distribution of particles, how do we use them to track objects? Think of these particles as *beliefs*. We want each particle to represent a possible belief for the movement of a tracked object. Each particle only contains a position and velocity, so we need a separate data structure to contain the probability that the particle is tracking an object. Much like we did in the **Discrete Bayes** chapter we can use a NumPy array to store the probabilities. \n",
"\n",
"We have not yet incorported any measurements, so if you recall from the Bayesian math sections we would call this our *prior*, as it contains the probability *prior* to adding a measurement. With no measurements we have no reason to believe any particular particle is more likely to be correct than any other particle, so it is reasonable to assign the same probability to each. We want a *probability distribution*, which means that the sum of the probabilities of all the particles must be one. Here is some code to do that."
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"N=len(particles)\n",
"p = np.array([1/N]*N)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We have 1,000 particles so the probability for each particle is 0.001 (1/1000). \n",
"\n",
"Now how do we incorporate measurements? Say we run our computer vision software on the first video frame and it reports that there is a blob at (43.2, 21.8). We can evaluate how likely each particle is taking that measurement into account. I don't want to get bogged down in the math at the moment, but recall that we would write that as $P(particle|blob)$. I bring that up to emphasize that this is a *Bayesian* filter- we incorporate new information to adjust our current beliefs. \n",
"\n",
"We don't yet know how to compute the new probabilities for the particles, but we can reason that a particle at (0,0), far from the measurement, would have a very low probability of representing the object, whereas a particle at (42.9, 21.9) should have a relatively high probability of representing the object."
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/plain": [
"-1.88266670220060"
]
},
"execution_count": 22,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"from sympy import *\n",
"from sympy.abc import x\n",
"\n",
"\n",
"a = cos(4*(x/2 + 0.7)) * sin(0.3*x) - 1.6*x\n",
"da = diff(a,x)\n",
"da.subs(x, 0)"
]
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": [
"objects in a two or three dimension space. via computer vision, radar, or other sensing technique. There might be zero, one, or multiple objects in view at once. We may or may not be able to mathematically describe the behavior of the tracked objects. If we are tracking passenger jets with radar we can model the behavior because large aircraft do not change direction or speed rapidly. If we are tracking people in a crowd, a ball on a football field, or fighter jets our prediction model will range from difficult to impossible. Finally, our measurements are likely to have very strong outliers. A bird flying in front of a camera tracking a sporting event is quite likely to fool the computer vision by creating spurious detections - the bird will be misidentified as a player or the ball. In radar design the designer has to account for noise - either domestic noise or hostile jamming in the case of war environments.\n",
"\n",
"So our problem is multi-modal, has a non-Gaussian to nonexistent process model, and has non-Gaussian measurements. Finally, both the measurements and behavior of the tracked objects are likely to be nonlinear.\n",
"\n",
"None of the filters that we have learned so far are particularly good at solving this problem. \n",
"The Kalman filter provides optimal output if and only if the data and object behavior is Gaussian and linear. The extended Kalman filter (EKF) and unscented Kalman filter (UKF) \n",
"\n",
"The discrete Bayes filter is multimodal and can handle nonlinear \n",
"\n",
"The filters that we have encountered so far is this book are very powerful, but they have significant limitations and difficulties. The discrete Bayes filter can track multiple objects at once because it is multi-modal, but it does not model the behavior of the tracked objects and it requires vast amounts of compute power and memory to represent large problems. The linear Kalman filter assumes that both the model and the measurements are Gaussian, and that both are linear. Of course, the world is not linear and data is rarely truly Gaussian. If these things are true then the Kalman filter produces an optimal solution, but in practice this is rare. Nonlinear Kalman filters, such as the unscented Kalman filter and extended Kalman filter deal with the nonlinearities in the process model and measurements by linearizing the problem. This allows the Kalman filter to work in nonlinear domains, but the linearization introduces inaccuracies that, in the worst case, causes the filter to diverge. There are filters which we haven't described in this book that are designed to handle data which is not fully Gaussian, but they again involve compromises.\n",
"\n",
"Particle filtering is a technique that largely transcends these problems. As we will see it is still non-optimal, but "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## How the Particle Filter Works"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Assume that we are trying to track objects in a two dimensional space. The particle filter starts by creating a random set of *particles*. Each particle is assigned a random location and velocity. Each particle represents one possible "
]
}
],
"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.3"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
14_Smoothing.ipynb
+634 -632
View File
File diff suppressed because it is too large. Load diff
15_Adaptive_Filtering.ipynb
+1611 -1606
View File
File diff suppressed because it is too large. Load diff
17_Ensemble_Kalman_Filters.ipynb
+733 -730
View File
File diff suppressed because it is too large. Load diff