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

Copy editing.

Browse Source
This commit is contained in:
Roger Labbe 2016-01-13 17:33:14 -08:00
parent 5921b20f87
commit 6933c33dbc
5 changed files with 56 additions and 3938 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
Appendix-A-Installation.ipynb
View File

@ -39,6 +39,9 @@
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"@import url('http://fonts.googleapis.com/css?family=Fira_sans');\n",
"\n",
".CodeMirror pre {\n",
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
"}\n",
" div.cell{\n",
" width: 900px;\n",
" margin-left: 0% !important;\n",
@ -58,7 +61,7 @@
"\t\n",
" div.input_area {\n",
" background: #F6F6F9;\n",
" border: 1px solid #586e75;\n",
" border: 1px solid #586e75; \n",
" }\n",
"\n",
" .text_cell_render h1 {\n",
@ -152,7 +155,8 @@
"}\n",
"\n",
" code{\n",
" font-size: 70%;\n",
" font-size: 6pt;\n",
"\n",
" }\n",
" .rendered_html code{\n",
" background-color: transparent;\n",
@ -245,7 +249,8 @@
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\", \"autobold.js\"]\n",
" extensions: [\"AMSmath.js\"],\n",
" equationNumbers: { autoNumber: \"AMS\", useLabelIds: true}\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
@ -253,7 +258,7 @@
" },\n",
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
" \"HTML-CSS\": {\n",
" scale:100,\n",
" scale:95,\n",
" availableFonts: [],\n",
" preferredFont:null,\n",
" webFont: \"TeX\",\n",
@ -283,9 +288,9 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"This book is written in Jupyter Notebook, a browser based interactive Python environment that mixes Python, text, and math. I choose it because of the interactive features - I found Kalman filtering nearly impossible to learn until I started working in an interactive environment. It is difficult to form an intuition of the effect of many of the parameters that you can tune until you can change them rapidly and immediately see the output. An interactive environment also allows you to play 'what if' scenarios out. \"What if I set $\\mathbf{Q}$ to zero?\" It is trivial to find out with Jupyter Notebook.\n",
"This book is written in Jupyter Notebook, a browser based interactive Python environment that mixes Python, text, and math. I choose it because of the interactive features - I found Kalman filtering nearly impossible to learn until I started working in an interactive environment. It is difficult to form an intuition about many of the parameters until you can change them and immediately see the output. An interactive environment also allows you to play 'what if' scenarios. \"What if I set $\\mathbf{Q}$ to zero?\" It is trivial to find out with Jupyter Notebook.\n",
"\n",
"Another reason I choose it is because I find that a typical textbook leaves many things opaque. For example, there might be a beautiful plot next to some pseudocode. That plot was produced by software, but software that is not available to me as a reader. I want everything that went into producing this book to be available to the reader. How do you plot a covariance ellipse? You won't know if you read most books. With Jupyter Notebook all you have to do is look at the source code.\n",
"Another reason I choose it is because most textbooks leaves many things opaque. For example, there might be a beautiful plot next to some pseudocode. That plot was produced by software, but software that is not available to the reader. I want everything that went into producing this book to be available to you. How do you plot a covariance ellipse? You won't know if you read most books. With Jupyter Notebook all you have to do is look at the source code.\n",
"\n",
"Even if you choose to read the book online you will want Python and the SciPy stack installed so that you can write your own Kalman filters. There are many different ways to install these libraries, and I cannot cover them all, but I will cover a few typical scenarios."
]
@ -301,15 +306,11 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"This book requires IPython, NumPy, SciPy, SymPy, and Matplotlib. The SciPy stack depends on third party Fortran and C code, and is *not* trivial to install from source code - even the SciPy website strongly urges using a pre-built installation, and I concur with this advice.\n",
"This book requires IPython, Jupiter, NumPy, SciPy, SymPy, and Matplotlib. The SciPy stack depends on third party Fortran and C code, and is not trivial to install from source code. The SciPy website strongly urges using a pre-built installation, and I concur with this advice.\n",
"\n",
"I use the Anaconda distribution from Continuum Analytics. This is an excellent package that combines all of the packages listed above, plus many others in one distribution. Installation is very straightforward, and it can be done alongside other Python installation you might already have on your machine. It is free to use, and Continuum Analytics will always ensure that the latest releases of all it's packages are available and built correctly for your OS. You may download it from here: http://continuum.io/downloads\n",
"I use the Anaconda distribution from Continuum Analytics. This is an excellent distribution that combines all of the packages listed above, plus many others. Installation is very straightforward, and it can be done alongside other Python installations you might already have on your machine. It is free to use. You may download it from here: http://continuum.io/downloads I strongly recommend using the latest Python 3 version that they provide.\n",
"\n",
"I strongly recommend using the latest Python 3 version that they provide; I am developing in Python 3.x, and only sporadically test that everything works in Python 2.7. However, it is my long term goal to support 2.7, and if you wish to either not be bothered with 3.x and are willing to be occasionally frustrated with breaking check ins you may stick with Python 2.7.\n",
"\n",
"I am still writing the book, so I do not know exactly what versions of each package is required. I do strongly urge you to use IPython 2.0 or later (this version number has nothing to do with Python 2 vs 3, by the way), as it provides many useful features which I will explain later. \n",
"\n",
"There are other choices for installing the SciPy stack. The SciPy stack provides instructions here: http://scipy.org/install.html"
"There are other choices for installing the SciPy stack. You can find instructions here: http://scipy.org/install.html"
]
},
{
@ -318,12 +319,12 @@
"source": [
"## Installing FilterPy\n",
"\n",
"FilterPy is a Python library that implements all of the filters used in this book, and quite a few not used by this book. Installation is easy using Pip. Issue the following command from the command prompt.\n",
"FilterPy is a Python library that implements all of the filters used in this book, and quite a few others. Installation is easy using `pip`. Issue the following from the command prompt:\n",
"\n",
" pip install filterpy\n",
" \n",
" \n",
"FilterPy is written by me, and the latest development version is always available at github.com/rlabbe/filterpy.\n",
"FilterPy is written by me, and the latest development version is always available at https://github.com/rlabbe/filterpy.\n",
" \n",
" "
]
@ -332,106 +333,36 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"### Manual Install of the SciPy stack"
"## Downloading and Running the Book"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This really isn't easy, and again I advice you to follow the advice of the SciPy website and to use a prepackaged solution. Still, I will give you one example of an install from a fresh operating system. You will have to adapt the instructions to fit your OS and OS version. I will use xubuntu, a linux distribution, because that is what I am most familiar with. I know there are pre-built binaries for Windows (link provided on the SciPy installation web page), and I have no experience with Mac and Python.\n",
"\n",
"I started by doing a fresh install of xubuntu 14.04 on a virtual machine. At that point there is a version of Python 2.7.6 and 3.4 pre-installed. As discussed above you may use either version; I will give the instructions for version 3.4 both because I prefer version 3 and because it can be slightly more tricky because 2.7 is the default Python that runs in xubuntu. Basically the difference is that you have to append a '3' at the end of commands. `python3` instead of `python`, `pip3` instead of `pip`, and so on.\n",
"\n",
"First we will install pip with the following command:\n",
"\n",
" sudo apt-get install python3-pip\n",
" \n",
" \n",
"Now you will need to install the various packages from the Ubuntu repositories. Unfortunately they usually do not contain the latest version, so we will also install the development tools necessary to perform an upgrade, which requires compiling various modules.\n",
"\n",
"\n",
" sudo apt-get install python3-numpy python3-scipy python3-matplotlib\n",
" sudo apt-get install libblas-dev liblapack-dev gfortran python3-dev \n",
"\n",
"\n",
"Now you can upgrade the packages. This will take a long time as everything needs to be compiled from source. If you get an error I do not know how to help you!\n",
"\n",
" sudo pip3 install numpy --upgrade\n",
" sudo pip3 install scipy --upgrade\n",
"\n",
"Now you get to install SymPy. You can download it from github (replace version number with the most recent version):\n",
"\n",
" wget https://github.com/sympy/sympy/releases/download/sympy-0.7.6/sympy-0.7.6.tar.gz\n",
" tar -zxvf sympy-0.7.6.tar.gz\n",
"\n",
"Now go into the directory you just extracted and run setup. \n",
"\n",
" sudo python3 setup.py install\n",
" \n",
" \n",
"If all of this went without a hitch you should have a good install. Try the following. From the command line type `ipython3` to launch ipython, then issue the following commands. Each should run with no exceptions.\n",
"\n",
" import numpy as np\n",
" import scipy as sp\n",
" import sympy as sym\n",
" np.__version__\n",
" sp.__version__\n",
" sum.__version__\n",
" \n",
"Now let's make sure plotting works correctly.\n",
"\n",
" import matplotlib.pyplot as plt\n",
" plt.plot([1, 4, 3])\n",
" plt.show()\n",
" \n",
"Now you get to fix IPython so Jupyter notebook will run. First, I had to uninstall IPython with\n",
"\n",
" sudo pip3 uninstall ipython\n",
" \n",
"Then, I reinstalled it with the `[all]` option so that all the required dependencies are installed.\n",
"\n",
" sudo pip3 install \"ipython3[all]\"\n",
" \n",
"Now test the installation by typing\n",
"\n",
" ipython notebook\n",
" \n",
"If successful, it should launch a browser showing you the Jupyter home page.\n",
"\n",
"That was not fun. It actually goes somewhat smoother in Windows, where you can download pre-built binaries for these packages; however, you are dependent on this being done for you in a timely manner. So, please follow the SciPy advice and use a pre-built distribution! I will not be supporting this manual install process."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Installing/downloading and running the book"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Okay, so now you have the SciPy stack installed, how do you download the book? This is easy as the book is in a github repository. From the command line type the following:\n",
"The book is stored in a github repository. From the command line type the following:\n",
"\n",
" git clone https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python.git\n",
" \n",
"Alternatively, browse to https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python and download it via your browser.\n",
"If you do not have git installed, browse to https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python and download it via your browser.\n",
"\n",
"Now, from the command prompt change to the directory that was just created, and then run Jupyter notebook:\n",
"\n",
" cd Kalman-and-Bayesian-Filters-in-Python\n",
" ipython notebook\n",
"\n",
"A browser window should launch showing you all of the chapters in the book. Browse to the first chapter by clicking on it, then open the notebook in that subdirectory by clicking on the link."
"A browser window should launch showing you all of the chapters in the book. Browse to the first chapter by clicking on it, then open the notebook in that subdirectory by clicking on the link.\n",
"\n",
"More information about running the notebook can be found here:\n",
"\n",
"http://jupyter-notebook-beginner-guide.readthedocs.org/en/latest/execute.html"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Using IPython Notebook"
"## Using Juptyer Notebook"
]
},
{
@ -478,14 +409,14 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"SymPy is a Python package for performing symbolic mathematics. The full scope of its abilities are beyond this book, but it can perform algebra, integrate and differentiate equations, find solutions to differential equations, and much more. For example, we use use to to compute the Jacobian of matrices and the expected value integral computations.\n",
"SymPy is a Python package for performing symbolic mathematics. The full scope of its abilities are beyond this book, but it can perform algebra, integrate and differentiate equations, find solutions to differential equations, and much more. For example, we use use it to compute the Jacobian of matrices and expected value integral computations.\n",
"\n",
"First, a simple example. We will import SymPy, initialize its pretty print functionality (which will print equations using LaTeX). We will then declare a symbol for Numpy to use."
"First, a simple example. We will import SymPy, initialize its pretty print functionality (which will print equations using LaTeX). We will then declare a symbol for SymPy to use."
]
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 3,
"metadata": {
"collapsed": false
},
@ -499,7 +430,7 @@
"\\phi"
]
},
"execution_count": 4,
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
@ -516,12 +447,12 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"Notice how we use a latex expression for the symbol `phi`. This is not necessary, but if you do it will render as LaTeX when output. Now let's do some math. What is the derivative of $\\sqrt{\\phi}$?"
"Notice how it prints the symbol `phi` using LaTeX. Now let's do some math. What is the derivative of $\\sqrt{\\phi}$?"
]
},
{
"cell_type": "code",
"execution_count": 5,
"execution_count": 4,
"metadata": {
"collapsed": false
},
@ -538,7 +469,7 @@
"2⋅╲╱ φ "
]
},
"execution_count": 5,
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
@ -556,7 +487,7 @@
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 5,
"metadata": {
"collapsed": false
},
@ -571,7 +502,7 @@
"(\\phi - 1)⋅⎝\\phi + 1⎠"
]
},
"execution_count": 6,
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
@ -589,7 +520,7 @@
},
{
"cell_type": "code",
"execution_count": 7,
"execution_count": 6,
"metadata": {
"collapsed": false
},
@ -604,7 +535,7 @@
"\\phi - 3⋅\\phi - 4"
]
},
"execution_count": 7,
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
@ -617,40 +548,36 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"You can find the value of an equation by substituting in a value for a variable"
"You can evauate an equation for specific values of its variables:"
]
},
{
"cell_type": "code",
"execution_count": 8,
"execution_count": 7,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/latex": [
"$$8$$"
],
"text/plain": [
"8"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
"name": "stdout",
"output_type": "stream",
"text": [
"8\n",
"112\n"
]
}
],
"source": [
"w =x**2 -3*x +4\n",
"w.subs(x,4)"
"print(w.subs(x, 4))\n",
"print(w.subs(x, 12))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can also use strings for equations that use symbols that you have not defined"
"You can also use strings for equations that use symbols that you have not defined:"
]
},
{
@ -683,11 +610,11 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"Now let's use SymPy to compute the Jacobian of a matrix. Let us have a function\n",
"Now let's use SymPy to compute the Jacobian of a matrix. Given the function\n",
"\n",
"$$h=\\sqrt{(x^2 + z^2)}$$\n",
"\n",
"for which we want to find the Jacobian with respect to x, y, and z."
"find the Jacobian with respect to x, y, and z."
]
},
{
@ -728,15 +655,15 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"Now let's compute the discrete process noise matrix $\\mathbf{Q}_k$ given the continuous process noise matrix \n",
"$$\\mathbf{Q} = \\Phi_s \\begin{bmatrix}0&0&0\\\\0&0&0\\\\0&0&1\\end{bmatrix}$$\n",
"Now let's compute the discrete process noise matrix $\\mathbf Q$ given the continuous process noise matrix \n",
"$$\\mathbf Q = \\Phi_s \\begin{bmatrix}0&0&0\\\\0&0&0\\\\0&0&1\\end{bmatrix}$$\n",
"\n",
"and the equation\n",
"The integral is \n",
"\n",
"$$\\mathbf{Q} = \\int_0^{\\Delta t} \\Phi(t)\\mathbf{Q}\\Phi^T(t) dt$$\n",
"$$\\mathbf Q = \\int_0^{\\Delta t} \\mathbf F(t)\\mathbf Q\\mathbf F^T(t)\\, dt$$\n",
"\n",
"where \n",
"$$\\Phi(t) = \\begin{bmatrix}1 & \\Delta t & {\\Delta t}^2/2 \\\\ 0 & 1 & \\Delta t\\\\ 0& 0& 1\\end{bmatrix}$$"
"$$\\mathbf F(\\Delta t) = \\begin{bmatrix}1 & \\Delta t & {\\Delta t}^2/2 \\\\ 0 & 1 & \\Delta t\\\\ 0& 0& 1\\end{bmatrix}$$"
]
},
{
@ -802,7 +729,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.5.0"
"version": "3.5.1"
}
},
"nbformat": 4,

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

@ -343,12 +343,7 @@
"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"
"\\end{aligned}$$"
]
}
],

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

@ -1,481 +0,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
}

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

File diff suppressed because it is too large Load Diff

275
old-content.ipynb
View File

@ -1,275 +0,0 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This contains text and code that I've removed from the book because it is redundant or replaced with what I think of as better material. Maybe you liked it, so I'll save it here for now."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Exercise: Track a target moving in a circle"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Change the simulated target movement to move in a circle. Avoid angular nonlinearities by putting the sensors well outside the movement range of the target, and avoid the angles $0^\\circ$ and $180^\\circ$."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"# your solution here"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Solution\n",
"We have a few choices here. First, if we know the movement of the target we can design our filter's state transition function so that it correctly predicts the circular movement. For example, suppose we were tracking a boat race optically - we would want to take track shape into account with our filter. However, in this chapter we have not been talking about such constrained behavior, so I will not build knowledge of the movement into the filter. So my implementation looks like this."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"def plot_circular_target(kf, std_noise, target_pos):\n",
" xs = []\n",
" txs = []\n",
" radius = 100\n",
" for i in range(300):\n",
" target_pos[0] = math.cos(i/10)*radius + randn()*0.0001\n",
" target_pos[1] = math.sin(i/10)*radius + randn()*0.0001\n",
" txs.append((target_pos[0], target_pos[1]))\n",
"\n",
" z = measurement(sa_pos, sb_pos, target_pos)\n",
" z[0] += randn() * std_noise\n",
" z[1] += randn() * std_noise\n",
"\n",
" kf.predict()\n",
" kf.update(z)\n",
" xs.append(kf.x)\n",
"\n",
" xs = np.asarray(xs)\n",
" txs = np.asarray(txs)\n",
"\n",
" plt.plot(xs[:, 0], xs[:, 2])\n",
" plt.plot(txs[: ,0], txs[:, 1], linestyle='-.')\n",
" plt.axis('equal')\n",
" plt.show()\n",
"\n",
"sa_pos = [-240, 200]\n",
"sb_pos = [240, 200]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"np.random.seed(12283)\n",
"std_noise = math.radians(0.5)\n",
"target_pos = [0, 0]\n",
"f = moving_target_filter(target_pos, std_noise, Q=1.1)\n",
"plot_circular_target(f, std_noise, target_pos)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Discussion\n",
"\n",
"The filter tracks the movement of the target, but never really converges on the track. This is because the filter is modeling a constant velocity target, but the target is anything but constant velocity. As mentioned above we could model the circular behavior by defining the `fx()` function, but then we would have problems when the target is not moving in a circle. Instead, lets tell the filter we are are less sure about our process model by making $\\mathbf{Q}$ larger. Here I have increased the variance from 0.1 to 1.0"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"np.random.seed(12283)\n",
"std_noise = math.radians(0.5)\n",
"cf = moving_target_filter(target_pos, std_noise, Q=10.)\n",
"target_pos = [0, 0]\n",
"plot_circular_target(cf, std_noise, target_pos)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The convergence is not perfect, but it is far better. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Exercise: Sensor Position Effects\n",
"\n",
"Is the behavior of the filter invariant for any sensor position? Find a sensor position that produces bad filter behavior."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"# your answer here"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Solution\n",
"\n",
"We have already discussed the problem of angles being modal, so causing that problem by putting the sensors at `y=0` would be a trivial solution. However, let's be more subtle than that. We want to create a situation where there are an infinite number of solutions for the sensor readings. We can achieve that whenever the target lies on the straight line between the two sensors. In that case there is no triangulation possible and there is no unique solution. My solution is as follows."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"std_noise = math.radians(0.5)\n",
"sa_pos = [-200, 200]\n",
"sb_pos = [200, -200]\n",
"plt.scatter(*sa_pos, s=200)\n",
"plt.scatter(*sb_pos, s=200)\n",
"target_pos = [0, 0]\n",
"cf = moving_target_filter(target_pos, std_noise, Q=10.)\n",
"plot_circular_target(cf, std_noise, target_pos)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"I put the sensors at the upper left hand side and lower right hand side of the target's movement. We can see how the filter diverges when the target enters the region directly between the two sensors. The state transition always predicts that the target is moving in a straight line. When the target is between the two sensors this straight line movement is well described the bearing measurements from the two sensors so the filter estimate starts to approximate a straight line. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Exercise: Compute Position Errors\n",
"\n",
"The position errors of the filter vary depending on how far away the target is from a sensor. Write a function that computes the distance error due to a bearing error. "
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"# your solution here"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Solution\n",
"\n",
"Basic trigonometry gives us this answer."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"def distance_error(target_distance, angle_error):\n",
" x = 1 - math.cos(angle_error)\n",
" y = math.sin(angle_error)\n",
" return target_distance*(x**2 + y**2)**.5\n",
"\n",
"d = distance_error(100, math.radians(1.))\n",
"print('\\n\\nError of 1 degree at 100km is {:.3}km'.format(d))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"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
}