EKF and Q content additions.

A lot of new text on designing Extended Kalman filters and
also on designing the Q matrix.
This commit is contained in:
Roger Labbe 2014-11-16 00:42:47 -08:00
parent e131cbb1c0
commit d974f78d37
4 changed files with 1558 additions and 146 deletions

View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:b7f65a1b7640cd43f95156970a5c73ee923fb430d06acbc4debe6508e5327549"
"signature": "sha256:dc1d96fdfafdd06f5b65eb11a1bda31f0fd112f7cceaa954c22732296e2889b1"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -43,6 +43,8 @@
"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",
@ -129,9 +131,9 @@
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" font-family: 'Arimo',verdana,arial,sans-serif;\n",
" line-height: 135%;\n",
" font-size: 110%;\n",
" font-size: 125%;\n",
" width:750px;\n",
" margin-left:auto;\n",
" margin-right:auto;\n",
@ -141,12 +143,12 @@
" div.output_subarea.output_text.output_pyout {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 300px;\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: 300px;\n",
" max-height: 50000px;\n",
" }\n",
" code{\n",
" font-size: 70%;\n",
@ -259,7 +261,7 @@
"output_type": "pyout",
"prompt_number": 1,
"text": [
"<IPython.core.display.HTML at 0x7ff2cda13f98>"
"<IPython.core.display.HTML at 0x7f306bfdb5f8>"
]
}
],
@ -631,24 +633,415 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"*equations of continuous Wiener process acceleration model - just a cut paste being saved for later use when I write this*\n",
"\n",
" $$\\begin{bmatrix}\n",
" \\frac{1}{3}{\\Delta t}^3 & \\frac{1}{2}{\\Delta t}^2 \\\\\n",
" \\frac{1}{2}{\\Delta t}^2 & \\Delta t\n",
" \\end{bmatrix}\\tilde{q}\n",
" $$\n",
"In general the design of the $\\mathbf{Q}$ matrix is among the most difficult aspects of Kalman filter design. This is due to several factors. First, the math itself is somewhat difficult and requires a good foundation in signal theory. Second, we are trying to model the noise in something for which we have little information. For example, consider trying to model the process noise for a baseball. We can model it as a sphere moving through the air, but that leave many unknown factors - the wind, ball rotation and spin decay, the coefficient of friction of a scuffed ball with stitches, the effects of wind and air density, and so on. I will develop the equations for an exact mathematical solution for a given process model, but since the process model is incomplete the result for $\\mathbf{Q}$ will also be incomplete. This has a lot of ramifications for the behavior of the Kalman filter. If $\\mathbf{Q}$ is too small than the filter will be overconfident in it's prediction model and will diverge from the actual solution. If $\\mathbf{Q}$ is too large than the filter will be unduly influenced by the noise in the measurements and perform suboptimally. In practice we spend a lot of time running simulations and evaluating collected data to try to select an appropriate value for $\\mathbf{Q}$. But let's start by looking at the math.\n",
"\n",
"\n",
" $$\\begin{bmatrix}\n",
" \\frac{1}{20}{\\Delta t}^5 & \\frac{1}{8}{\\Delta t}^4 & \\frac{1}{6}{\\Delta t}^3 \\\\\n",
" \\frac{1}{8}{\\Delta t}^8 & \\frac{1}{3}{\\Delta t}^3 & \\frac{1}{2}{\\Delta t}^2 \\\\\n",
" \\frac{1}{6}{\\Delta t}^3 & \\frac{1}{2}{\\Delta t}^2 & \\Delta t\n",
" \\end{bmatrix} \\tilde{q}\n",
" $$\n",
" "
"Let's assume a kinematic system - some system that can be modelled using Newton's equations of motion. We can make a few different assumptions about this process. \n",
"\n",
"We have been using a process model of\n",
"\n",
"$$ f(\\mathbf{x}) = \\mathbf{Fx} + \\mathbf{w}$$\n",
"\n",
"where $\\mathbf{w}$ is the process noise. Kinematic systems are *continuous* - their inputs and outputs can vary at any arbitrary point in time. However, our Kalman filters are *discrete*. We sample the system at regular intervals. Therefore we must find the discrete representation for the noise term in the equation above. However, this depends on what assumptions we make about the behavior of the noise. We will consider two different models for the noise."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Continuous White Noise Model\n",
"\n",
"We model kinematic systems using Newton's equations. So far in this book we have either used position and velocity, or position,velocity, and acceleration as the models for our systems. There is nothing stopping us from going further - we can model jerk, jounce, snap, and so on. We don't do that normally because adding terms beyond the dynamics of the real system actually degrades the solution. \n",
"\n",
"Let's say that we need to model the position, velocity, and acceleration. We can then assume that acceleration is constant. Of course, there is process noise in the system and so the acceleration is not actually constant. In this section we will assume that the acceleration changes by a continouus time zero-mean white noise $w(t)$. In other words, we are assuming that velocity is acceleration changing by small amounts that over time average to 0 (zero-mean). \n",
"\n",
"\n",
"Since the noise is changing continuously we will need to integrate to get the discrete noise for the discretization interval that we have chosen. We will not prove it here, but the equation for discretizing the noise is\n",
"\n",
"$$\\mathbf{Q} = \\int_0^{\\Delta t} \\Phi(t)\\mathbf{Q_c}\\Phi^T(t) dt$$\n",
"\n",
"where $\\mathbf{Q_c}$ is the continuous noise.\n",
"\n",
"This gives us\n",
"\n",
"$$\\Phi = \\begin{bmatrix}1 & \\Delta t & {\\Delta t}^2/2 \\\\ 0 & 1 & \\Delta t\\\\ 0& 0& 1\\end{bmatrix}$$\n",
"\n",
"for the fundamental matrix, and\n",
"\n",
"$$\\mathbf{Q_c} = \\begin{bmatrix}0&0&0\\\\0&0&0\\\\0&0&1\\end{bmatrix} \\Phi_s$$\n",
"\n",
"for the continuous process noise matrix, where $\\Phi_s$ is the spectral density of the white noise.\n",
"\n",
"We could carry out these computations ourselves, but I prefer using SymPy to solve the equation."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import sympy\n",
"from sympy import init_printing, Matrix,MatMul, integrate, symbols\n",
"\n",
"init_printing(use_latex='mathjax')\n",
"dt, phi = symbols('\\Delta{t} \\Phi_s')\n",
"F_k = Matrix([[1, dt, dt**2/2],\n",
" [0, 1, dt],\n",
" [0, 0, 1]])\n",
"Q_c = Matrix([[0,0,0],\n",
" [0,0,0],\n",
" [0,0,1]])*phi\n",
"\n",
"Q=sympy.integrate(F_k*Q_c*F_k.T,(dt, 0, dt))\n",
"\n",
"# factor phi out of the matrix to make it more readable\n",
"Q = Q/phi\n",
"sympy.MatMul(Q, phi)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{\\Delta{t}^{5}}{20} & \\frac{\\Delta{t}^{4}}{8} & \\frac{\\Delta{t}^{3}}{6}\\\\\\frac{\\Delta{t}^{4}}{8} & \\frac{\\Delta{t}^{3}}{3} & \\frac{\\Delta{t}^{2}}{2}\\\\\\frac{\\Delta{t}^{3}}{6} & \\frac{\\Delta{t}^{2}}{2} & \\Delta{t}\\end{matrix}\\right] \\Phi_{s}$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 40,
"text": [
"\u23a1 5 4 3\u23a4 \n",
"\u23a2\\Delta{t} \\Delta{t} \\Delta{t} \u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5\u22c5\\Phi_s\n",
"\u23a2 20 8 6 \u23a5 \n",
"\u23a2 \u23a5 \n",
"\u23a2 4 3 2\u23a5 \n",
"\u23a2\\Delta{t} \\Delta{t} \\Delta{t} \u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5 \n",
"\u23a2 8 3 2 \u23a5 \n",
"\u23a2 \u23a5 \n",
"\u23a2 3 2 \u23a5 \n",
"\u23a2\\Delta{t} \\Delta{t} \u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \\Delta{t} \u23a5 \n",
"\u23a3 6 2 \u23a6 "
]
}
],
"prompt_number": 40
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For completeness, let us compute the equations for the 0th order and 1st order equations.\n"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"F_k = sympy.Matrix([[1]])\n",
"Q_c = sympy.Matrix([[phi]])\n",
"\n",
"print('0th order discrete process noise')\n",
"sympy.integrate(F_k*Q_c*F_k.T,(dt, 0, dt))"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"0th order discrete process noise\n"
]
},
{
"latex": [
"$$\\left[\\begin{matrix}\\Delta{t} \\Phi_{s}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 38,
"text": [
"[\\Delta{t}\u22c5\\Phi_s]"
]
}
],
"prompt_number": 38
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"F_k = sympy.Matrix([[1, dt],\n",
" [0, 1]])\n",
"Q_c = sympy.Matrix([[0,0],\n",
" [0,1]])*phi\n",
"\n",
"Q = sympy.integrate(F_k*Q_c*F_k.T,(dt, 0, dt))\n",
"\n",
"print('1st order discrete process noise')\n",
"# factor phi out of the matrix to make it more readable\n",
"Q = Q/phi\n",
"sympy.MatMul(Q, phi)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Piecewise White Noise Model\n",
"\n",
"Another model for the noise assumes that the that highest order term (say, acceleration) is constant for each time period, but differs for each time period, and each of these is uncorrelated between time periods. This is subtly different than the model above, where we assumed that the last term had a continuously varying noisy signal applied to it. \n",
"\n",
"We will model this as\n",
"\n",
"$$f(x)=Fx+\\Gamma w$$\n",
"\n",
"where $\\Gamma$ is the *noise gain* of the system, and $w$ is the constant piecewise accleration (or velocity, or jerk, etc). \n",
"\n",
"\n",
"Lets start with by looking a first order system. In this case we have the state transition function\n",
"\n",
"$$\\mathbf{F} = \\begin{bmatrix}1&\\Delta t \\\\ 0& 1\\end{bmatrix}$$\n",
"\n",
"In one time period, the change in velocity will be $w(t)\\Delta t$, and the change in position will be $w(t)\\Delta t^2/2$, giving us\n",
"\n",
"$$\\Gamma = \\begin{bmatrix}\\frac{1}{2}\\Delta t^2 \\\\ \\Delta t\\end{bmatrix}$$\n",
"\n",
"The covariance of the process noise is then\n",
"\n",
"$$Q = E[\\Gamma w(t) w(t) \\Gamma^T] = \\Gamma\\sigma^2_v\\Gamma^T$$.\n",
"\n",
"We can compute that with SymPy as follows"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"var=symbols('sigma^2_v')\n",
"v = Matrix([[dt**2/2],[dt]])\n",
"\n",
"\n",
"Q = v * var * v.T\n",
"\n",
"# factor variance out of the matrix to make it more readable\n",
"Q = Q/var\n",
"sympy.MatMul(Q, var)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{\\Delta{t}^{4}}{4} & \\frac{\\Delta{t}^{3}}{2}\\\\\\frac{\\Delta{t}^{3}}{2} & \\Delta{t}^{2}\\end{matrix}\\right] \\sigma^{2}_{v}$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 53,
"text": [
"\u23a1 4 3\u23a4 \n",
"\u23a2\\Delta{t} \\Delta{t} \u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5\u22c5\u03c3\u00b2\u1d65\n",
"\u23a2 4 2 \u23a5 \n",
"\u23a2 \u23a5 \n",
"\u23a2 3 \u23a5 \n",
"\u23a2\\Delta{t} 2\u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \\Delta{t} \u23a5 \n",
"\u23a3 2 \u23a6 "
]
}
],
"prompt_number": 53
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The second order system proceeds with the same math.\n",
"\n",
"\n",
"$$\\mathbf{F} = \\begin{bmatrix}1 & \\Delta t & {\\Delta t}^2/2 \\\\ 0 & 1 & \\Delta t\\\\ 0& 0& 1\\end{bmatrix}$$\n",
"\n",
"Here we will assume that the white noise is a discrete time Wiener process. This gives us\n",
"\n",
"$$\\Gamma = \\begin{bmatrix}\\frac{1}{2}\\Delta t^2 \\\\ \\Delta t\\\\ 1\\end{bmatrix}$$\n",
"\n",
"There is no 'truth' to this model, it is just convienent and provides good results. For example, we could assume that the noise is applied to the jerk at the cost of a more complicated equation. \n",
"\n",
"The covariance of the process noise is then\n",
"\n",
"$$Q = E[\\Gamma w(t) w(t) \\Gamma^T] = \\Gamma\\sigma^2_v\\Gamma^T$$.\n",
"\n",
"We can compute that with SymPy as follows"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"var=symbols('sigma^2_v')\n",
"v = Matrix([[dt**2/2],[dt], [1]])\n",
"\n",
"\n",
"Q = v * var * v.T\n",
"\n",
"# factor variance out of the matrix to make it more readable\n",
"Q = Q/var\n",
"sympy.MatMul(Q, var)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{\\Delta{t}^{4}}{4} & \\frac{\\Delta{t}^{3}}{2} & \\frac{\\Delta{t}^{2}}{2}\\\\\\frac{\\Delta{t}^{3}}{2} & \\Delta{t}^{2} & \\Delta{t}\\\\\\frac{\\Delta{t}^{2}}{2} & \\Delta{t} & 1\\end{matrix}\\right] \\sigma^{2}_{v}$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 55,
"text": [
"\u23a1 4 3 2\u23a4 \n",
"\u23a2\\Delta{t} \\Delta{t} \\Delta{t} \u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5\u22c5\u03c3\u00b2\u1d65\n",
"\u23a2 4 2 2 \u23a5 \n",
"\u23a2 \u23a5 \n",
"\u23a2 3 \u23a5 \n",
"\u23a2\\Delta{t} 2 \u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \\Delta{t} \\Delta{t} \u23a5 \n",
"\u23a2 2 \u23a5 \n",
"\u23a2 \u23a5 \n",
"\u23a2 2 \u23a5 \n",
"\u23a2\\Delta{t} \u23a5 \n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \\Delta{t} 1 \u23a5 \n",
"\u23a3 2 \u23a6 "
]
}
],
"prompt_number": 55
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We cannot say that this model is more or less correct than the continuous model - both are approximations to what is happening to the actual object. Only experience and experiments can guide you to the appropriate model. In practice you will usually find that either model provides reasonable results, but typically one will perform better than the other.\n",
"\n",
"The advantage of the second model is that we can model the noise in terms of $\\sigma^2$ which we can describe in terms of the motion and the amount of error we expect. The first model requires us to specify the spectral density, which is not very intuitive, but it handles varying time samples much more easily since the noise is integrated across the time period. However, these are not fixed rules - use whichever model (or a model of your own devising) based on testing how the filter performs and/or your knowledge of the behavior of the physical model."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Using FilterPy to Compute Q\n",
"\n",
"FilterPy offers several routines to compute the $\\mathbf{Q}$ matrix. The function `Q_continuous_white_noise()` computes $\\mathbf{Q}$ for a given value for $\\Delta t$ and the spectral density."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import filterpy.common as common\n",
"\n",
"common.Q_continuous_white_noise(dim=2, dt=1, spectral_density=1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 9,
"text": [
"array([[ 0.33333333, 0.5 ],\n",
" [ 0.5 , 1. ]])"
]
}
],
"prompt_number": 9
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"common.Q_continuous_white_noise(dim=3, dt=1, spectral_density=1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 10,
"text": [
"array([[ 0.05 , 0.125 , 0.16666667],\n",
" [ 0.125 , 0.33333333, 0.5 ],\n",
" [ 0.16666667, 0.5 , 1. ]])"
]
}
],
"prompt_number": 10
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The function `Q_discrete_white_noise()` computes $\\mathbf{Q}$ assuming a piecewise model for the noise."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"common.Q_discrete_white_noise(2, var=1.)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 8,
"text": [
"array([[ 0.25, 0.5 ],\n",
" [ 0.5 , 1. ]])"
]
}
],
"prompt_number": 8
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"common.Q_discrete_white_noise(3,var=1.)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 7,
"text": [
"array([[ 0.25, 0.5 , 0.5 ],\n",
" [ 0.5 , 1. , 1. ],\n",
" [ 0.5 , 1. , 1. ]])"
]
}
],
"prompt_number": 7
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 10
},
{
"cell_type": "heading",
"level": 2,

File diff suppressed because one or more lines are too long

File diff suppressed because one or more lines are too long

View File

@ -0,0 +1,584 @@
{
"metadata": {
"name": "",
"signature": "sha256:08d8da9b9be60564b5f4478ec019d413427884ab1107ce154b37e5411794ccd3"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Appendix A: Using NumPy, SciPy, and SymPy"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"#format the book\n",
"%matplotlib inline\n",
"from __future__ import division, print_function\n",
"import matplotlib.pyplot as plt\n",
"import sys\n",
"sys.path.insert(0,'../code') # allow us to format the book\n",
"import book_format\n",
"book_format.load_style()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"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: 20pt;\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: nowrap;\n",
" } \n",
" h3 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h3 {\n",
" font-weight: 300;\n",
" font-size: 18pt;\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: 300;\n",
" font-size: 16pt;\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: 300;\n",
" font-style: normal;\n",
" color: #1d3b84;\n",
" font-size: 16pt;\n",
" margin-bottom: 0em;\n",
" margin-top: 1.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: 135%;\n",
" font-size: 125%;\n",
" width:750px;\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",
" 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",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 1,
"text": [
"<IPython.core.display.HTML at 0x7fe692038b00>"
]
}
],
"prompt_number": 1
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Installation"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## NumPy"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## SymPy"
]
},
{
"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",
"\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."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import sympy\n",
"from sympy import init_printing\n",
"#from sympy.interactive import printing\n",
"init_printing(use_latex='mathjax')\n",
"\n",
"phi, x = sympy.symbols('\\phi, x')\n",
"phi"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\phi$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 2,
"text": [
"\\phi"
]
}
],
"prompt_number": 2
},
{
"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}$?"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sympy.diff('sqrt(phi)')"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\frac{1}{2 \\sqrt{\\phi}}$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 3,
"text": [
" 1 \n",
"\u2500\u2500\u2500\u2500\u2500\u2500\u2500\n",
" ___\n",
"2\u22c5\u2572\u2571 \u03c6 "
]
}
],
"prompt_number": 3
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We can factor equations"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sympy.factor(phi**3 -phi**2 + phi - 1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left(\\phi - 1\\right) \\left(\\phi^{2} + 1\\right)$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 4,
"text": [
" \u239b 2 \u239e\n",
"(\\phi - 1)\u22c5\u239d\\phi + 1\u23a0"
]
}
],
"prompt_number": 4
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"and we can expand them."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"((phi+1)*(phi-4)).expand()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\phi^{2} - 3 \\phi - 4$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 8,
"text": [
" 2 \n",
"\\phi - 3\u22c5\\phi - 4"
]
}
],
"prompt_number": 8
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can find the value of an equation by substituting in a value for a variable"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"w =x**2 -3*x +4\n",
"w.subs(x,4)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$8$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 9,
"text": [
"8"
]
}
],
"prompt_number": 9
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can also use strings for equations that use symbols that you have not defined"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"x = sympy.expand('(t+1)*2')\n",
"x"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$2 t + 2$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 14,
"text": [
"2\u22c5t + 2"
]
}
],
"prompt_number": 14
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now let's use SymPy to compute the Jacobian of a matrix. Let us have a 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."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"x, y, z = sympy.symbols('x y z')\n",
"\n",
"H = sympy.Matrix([sympy.sqrt(x**2 + z**2)])\n",
"\n",
"state = sympy.Matrix([x, y, z])\n",
"H.jacobian(state)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{x}{\\sqrt{x^{2} + z^{2}}} & 0 & \\frac{z}{\\sqrt{x^{2} + z^{2}}}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 11,
"text": [
"\u23a1 x z \u23a4\n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 0 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5\n",
"\u23a2 _________ _________\u23a5\n",
"\u23a2 \u2571 2 2 \u2571 2 2 \u23a5\n",
"\u23a3\u2572\u2571 x + z \u2572\u2571 x + z \u23a6"
]
}
],
"prompt_number": 11
},
{
"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",
"\n",
"and the equation\n",
"\n",
"$$\\mathbf{Q} = \\int_0^{\\Delta t} \\Phi(t)\\mathbf{Q}\\Phi^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}$$"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"dt = sympy.symbols('\\Delta{t}')\n",
"F_k = sympy.Matrix([[1, dt, dt**2/2],\n",
" [0, 1, dt],\n",
" [0, 0, 1]])\n",
"Q = sympy.Matrix([[0,0,0],\n",
" [0,0,0],\n",
" [0,0,1]])\n",
"\n",
"sympy.integrate(F_k*Q*F_k.T,(dt, 0, dt))"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{\\Delta{t}^{5}}{20} & \\frac{\\Delta{t}^{4}}{8} & \\frac{\\Delta{t}^{3}}{6}\\\\\\frac{\\Delta{t}^{4}}{8} & \\frac{\\Delta{t}^{3}}{3} & \\frac{\\Delta{t}^{2}}{2}\\\\\\frac{\\Delta{t}^{3}}{6} & \\frac{\\Delta{t}^{2}}{2} & \\Delta{t}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 12,
"text": [
"\u23a1 5 4 3\u23a4\n",
"\u23a2\\Delta{t} \\Delta{t} \\Delta{t} \u23a5\n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5\n",
"\u23a2 20 8 6 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 4 3 2\u23a5\n",
"\u23a2\\Delta{t} \\Delta{t} \\Delta{t} \u23a5\n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5\n",
"\u23a2 8 3 2 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 3 2 \u23a5\n",
"\u23a2\\Delta{t} \\Delta{t} \u23a5\n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \\Delta{t} \u23a5\n",
"\u23a3 6 2 \u23a6"
]
}
],
"prompt_number": 12
}
],
"metadata": {}
}
]
}