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

EKF and Q content additions.

Browse Source 
A lot of new text on designing Extended Kalman filters and
also on designing the Q matrix.
This commit is contained in:
Roger Labbe committed 2014-11-16 00:42:47 -08:00
1 parent e131cbb1c0
commit d974f78d37
4 files changed
+1558 -146
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

07_Kalman_Filter_Math/Kalman_Filter_Math.ipynb
+413 -20
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,
09_Extended_Kalman_Filters/Extended_Kalman_Filters.ipynb
+554 -118
View File
@@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:f9b7acba67bef2276a493d93976a5cf0997cb86a8644ecb6817597f4a3759b01"
"signature": "sha256:3fb14452fc76dc0ecb66a465a2f6e18412940cc33adec8dac76e80111450e30e"
},
"nbformat": 3,
"nbformat_minor": 0,
@@ -261,7 +261,7 @@
"output_type": "pyout",
"prompt_number": 1,
"text": [
"<IPython.core.display.HTML at 0x7fa5b3193048>"
"<IPython.core.display.HTML at 0x7fe0265715c0>"
]
}
],
@@ -271,7 +271,7 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"**Author's note: this is still being heavily edited - there is a lot of duplicate material, incomplete material, and so on**\n",
"**Author's note: this is still being heavily edited - there is a lot of duplicate material, incomplete material, and so on. In particular, there is a lot of blathering on about linearizing a function at a point. There is not a lot of content there - please feel free to skip ahead if you get the idea - use the slope at the point as the linearization of a function.**\n",
"\n",
"The Kalman filter that we have developed to this point is extremely good, but it is also limited. Its derivation is in the linear space, and hence it only works for linear problems. Let's be a bit more rigorous here. You can, and we have in this book, apply the Kalman filter to nonlinear problems. For example, in the g-h filter chapter we explored using a g-h filter in a problem with constant acceleration. It 'worked', in that it remained numerically stable and the filtered output did track the input, but there was always a lag. It is easy to prove that there will always be a lag when $\\mathbf{\\ddot{x}}>0$. The filter no longer produces an optimal result. If we make our time step arbitrarily small we can still handle many problems, but typically we are using Kalman filters with physical sensors and solving real-time problems. Either fast enough sensors do not exist, are prohibitively expensive, or the computation time required is excessive. It is not a workable solution.\n",
"\n",
@@ -304,9 +304,9 @@
"\n",
"The mathematics of the Kalman filter is beautiful in part due to the Gaussian equation being so special. It is nonlinear, but when we add and multipy it using linear algebra we get another Gaussian equation as a result. That is very rare. $\\sin{x}*\\sin{y}$ does not yield a $\\sin(\\cdot)$ as an output.\n",
"\n",
"If you are not well versed in signals and systems there is a perhaps startling fact that you should be aware of. A linear system is defined as a system whose output is linearly proportional to the sum of all its inputs. A consequence of this is that to be linear if the input is zero than the output must also be zero. Consider an audio amp - if a sing into a microphone, and you start talking, the output should be the sum of our voices (input) scaled by the amplifier gain. But if amplifier outputs a nonzero signal for a zero input the additive relationship no longer holds. This is because you can say $amp(roger) = amp(roger + 0)$ This clearly should give the same output, but if amp(0) is nonzero, then\n",
"> If you are not well versed in signals and systems there is a perhaps startling fact that you should be aware of. A linear system is defined as a system whose output is linearly proportional to the sum of all its inputs. A consequence of this is that to be linear if the input is zero than the output must also be zero. Consider an audio amp - if a sing into a microphone, and you start talking, the output should be the sum of our voices (input) scaled by the amplifier gain. But if amplifier outputs a nonzero signal for a zero input the additive relationship no longer holds. This is because you can say $amp(roger) = amp(roger + 0)$ This clearly should give the same output, but if amp(0) is nonzero, then\n",
"\n",
"$$\n",
"> $$\n",
"\\begin{aligned}\n",
"amp(roger) &= amp(roger + 0) \\\\\n",
"&= amp(roger) + amp(0) \\\\\n",
@@ -314,10 +314,10 @@
"\\end{aligned}\n",
"$$\n",
"\n",
"which is clearly nonsense. Hence, an apparently linear equation such as\n",
">which is clearly nonsense. Hence, an apparently linear equation such as\n",
"$$L(f(t)) = f(t) + 1$$\n",
"\n",
"is not linear because $L(0) = 1$! Be careful!"
">is not linear because $L(0) = 1$! Be careful!"
]
},
{
@@ -724,7 +724,7 @@
},
{
"cell_type": "heading",
"level": 3,
"level": 2,
"metadata": {},
"source": [
"Example: Tracking a Flying Airplane"
@@ -804,182 +804,618 @@
"The only question left is how do we implement use $f()$ and $h()$ in the Kalman filter if they are nonlinear? We reach for the single tool that we have available for solving nonlinear equations - we linearize them at the point we want to evaluate the system. For example, consider the function $f(x) = x^2 -2x$\n",
"\n",
"\n",
"The rest of the equations are unchanged, so $f()$ and $h()$ must produce a matrix that approximates the values of the matrices $\\mathbf{F}$ and $\\mathbf{H}$ at the current value for $\\mathbf{x}$. We do this by computing the partial derivatives of the state and measurements functions:\n",
"\n",
"\n",
"$$\n",
"F \\equiv {\\frac{\\partial{f}}{\\partial{x}}}\\biggr|_x, \\\\\n",
"H \\equiv \\frac{\\partial{h}}{\\partial{x}}\\biggr|_x \n",
"$$\n",
"\n",
"All this means is that at each update step we compute F as the partial derivative of our function $f()$ evaluated at the point of f. "
"The rest of the equations are unchanged, so $f()$ and $h()$ must produce a matrix that approximates the values of the matrices $\\mathbf{F}$ and $\\mathbf{H}$ at the current value for $\\mathbf{x}$. We do this by computing the partial derivatives of the state and measurements functions:"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"xs = np.arange(0,2,0.01)\n",
"ys = [x**2 - 2*x for x in xs]\n",
"plt.plot (xs, ys)\n",
"plt.show()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAtUAAAF2CAYAAACh02S2AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7fb196b14a20>"
]
}
],
"prompt_number": 13
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Suppose we want to linearlize this equation so we can evaluate it's value at 1.5. In other words, we want to create a linear function of the form $y_l(x) = ax+b$ such that $y_l(1.5)$ gives the same value as $y(1.5)$. Obviously there is not single linear equation that will do this. But if we linearize $y(x)$ at 1.5, then we will have a perfect answer for $y_l(1.5)$, and a progressively worse answer as our evaluation point gets further away from 1.5.\n",
"### Design the State Variables\n",
"\n",
"The simplest way to linearize a function is to take a partial derivative of it. In geometic terms, the derivative of a function at a point is just the slope of the function. Let's just look at that, and then reason about why this is a good choice.\n",
"So we want to track the position of an aircraft assuming a constant velocity and altitude, and measurements of the slant distance to the aircraft. That means we need 3 state variables - horizontal distance, velocity, and altitude.\n",
"\n",
"The derivative of $f(x) = x^2 -2x$ is $\\frac{\\partial{f}}{\\partial{x}} = 2x - 2$, so the slope at 1.5 is $2*1.5-2=1$. Let's plot that."
"$$ $\\mathbf{x} = \\begin{bmatrix}distance \\\\velocity\\\\ altitude\\end{bmatrix}= \\begin{bmatrix}x_{pos} \\\\x_{vel}\\\\ x_{alt}\\end{bmatrix}$$"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"def y(x): \n",
" return x - 2.25\n",
"\n",
"plt.plot (xs, ys)\n",
"plt.plot ([1,2], [y(1),y(2)], c='r')\n",
"plt.ylim([-1.5, 1])\n",
"\n",
"plt.show()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAtUAAAF2CAYAAACh02S2AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7fb197f6b0b8>"
]
}
],
"prompt_number": 14
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This is a nice visual demonstration of how the slope of the function gives the ideal linear approximation of the function near a point. We could use any linear function such that $f(1.5)=-0.75$, here, but as x varies the value computed by the function would potentially be very far from the functions value. For example, consider using $f(x) = 8x - 12.75$ as the linearization, as in the plot below."
"### Design the System Model\n",
"We will model this as a set of differential equations. So we need an equation in the form \n",
"$$\\dot{\\mathbf{x}} = \\mathbf{Ax} + \\mathbf{w}$$\n",
"\n",
"where $\\mathbf{w}$ is the system noise. \n",
"\n",
"Let's work out the equation for each of the rows in $\\mathbf{x}.$\n",
"\n",
"The first row is $\\dot{x}_{pos}$, which is the velocity of the airplane. So we can say \n",
"\n",
"$$\\dot{x}_{pos} = x_{vel}$$\n",
"\n",
"The second row is $\\dot{x}_{vel}$, which is the acceleration of the airplane. We assume constant velocity, so the acceleration equals zero. However, we also assume system noise due to things like buffeting winds, errors in control inputs, and so on, so we need to add an error $w_{acc}$ to the term, like so\n",
"\n",
"$$\\dot{x}_{vel} = 0 + w_{acc}$$\n",
"\n",
"\n",
"The final row contains $\\dot{x}_{alt}$, which is the rate of change in the altitude. We assume a constant altitude, so this term is 0, but as with acceleration we need to add in a noise term to account for things like wind, air density, and so on. This gives us\n",
"\n",
"$$\\dot{x}_{alt} = 0 + w_{alt}$$\n",
"\n",
"We turn this into matrix form with the following:\n",
"\n",
"$$\\dot{\\mathbf{x}} = \\begin{bmatrix} 0 & 1 & 0 \\\\ 0& 0& 0 \\\\ 0&0&0\\end{bmatrix}\n",
"\\begin{bmatrix}x_{pos} \\\\x_{vel}\\\\ x_{alt}\\end{bmatrix} + \\begin{bmatrix}0 \\\\w_{vel}\\\\ w_{alt}\\end{bmatrix}\n",
"$$\n",
"\n",
"Now we have our differential equations for the system we can somehow solve for them to get our familiar Kalman filter state equation\n",
"\n",
"$$ \\mathbf{x}=\\mathbf{Fx}$$\n",
"\n",
"Solving an arbitrary set of differential equations is beyond the scope of this book, however most Kalman filters are amenable to Taylor-series expansion which I will briefly explain here without proof. \n",
"\n",
"Given the partial differential equation \n",
"\n",
"$$\\mathbf{F} = \\frac{\\partial f(\\mathbf{x})}{\\partial x}$$\n",
"\n",
"the solution is $e^{\\mathbf{F}t}$. This is a standard answer learned in a first year partial differential equations course, and is not intuitively obvious from the material presented so far. However, we can compute the exponential matrix $e^{\\mathbf{F}t}$ using a Taylor-series expansion in the form:\n",
"\n",
"$$\\Phi = \\mathbf{I} + \\mathbf{F}\\Delta t + \\frac{(\\mathbf{F}\\Delta t)^2}{2!} + \\frac{(\\mathbf{F}\\Delta t)^3}{3!} + \\ldots$$\n",
"\n",
"You may expand that equation to as many terms as required for accuracy, however many problems only use the first term\n",
"\n",
"$$\\Phi \\approx \\mathbf{I} + \\mathbf{F}\\Delta t$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$\\Phi$ is our system matrix. We cannot use greek symbols in Python, so the code uses the symbol `F` for $\\Phi$. This is admittedly confusing. In the math above $\\mathbf{F}$ represents the system of partial differential equations, and $\\Phi$ is the system matrix. In the Python the partial differential equations are not represented in the code, and the system matrix is `F`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Design the Measurement Model\n",
"\n",
"The measurement function for our filter needs to take the filter state $\\mathbf{x}$ and turn it into a slant range distance. This is nothing more than the pythagorean theorem.\n",
"\n",
"$$h(\\mathbf{x}) = \\sqrt{x_{pos}^2 + x_{alt}^2}$$\n",
"\n",
"\n",
"The relationship between the slant distance and the position on the ground is nonlinear due to the square root term.\n",
"So what we need to do is linearize the measurement function at some point. As we discussed above, the best way to linearize an equation at a point is to find its slope, which we do by taking its derivative.\n",
"\n",
"$$\n",
"\\mathbf{H} \\equiv \\frac{\\partial{h}}{\\partial{x}}\\biggr|_x \n",
"$$\n",
"\n",
"The derivative of a matrix is called a Jacobian, which in general takes the form \n",
"\n",
"$$\\frac{\\partial \\mathbf{h}}{\\partial \\mathbf{x}} = \n",
"\\begin{bmatrix}\n",
"\\frac{\\partial h_1}{\\partial x_1} & \\frac{\\partial h_1}{\\partial x_2} &\\dots \\\\\n",
"\\frac{\\partial h_2}{\\partial x_1} & \\frac{\\partial h_2}{\\partial x_2} &\\dots \\\\\n",
"\\vdots & \\vdots\n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"In other words, each element in the matrix is the partial derivative of the function $h$ with respect to the variables $x$. For our problem we have\n",
"\n",
"$$\\mathbf{H} = \\begin{bmatrix}\\frac{\\partial h}{\\partial x_{pos}} & \\frac{\\partial h}{\\partial x_{vel}} & \\frac{\\partial h}{\\partial x_{alt}}\\end{bmatrix}$$\n",
"\n",
"where $h(x) = \\sqrt{x_{pos}^2 + x_{alt}^2}$ as given above.\n",
"\n",
"Solving each in turn:\n",
"\n",
"$$\\begin{aligned}\n",
"\\frac{\\partial h}{\\partial x_{pos}} &= \\\\ &=\\frac{\\partial}{\\partial x_{pos}} \\sqrt{x_{pos}^2 + x_{alt}^2} \\\\ &= \\frac{x_{pos}}{\\sqrt{x^2 + x_{alt}^2}}\n",
"\\end{aligned}$$\n",
"\n",
"and\n",
"\n",
"$$\\begin{aligned}\n",
"\\frac{\\partial h}{\\partial x_{vel}} &=\\\\\n",
"&= \\frac{\\partial}{\\partial x_{vel}} \\sqrt{x_{pos}^2 + x_{alt}^2} \\\\ \n",
"&= 0\n",
"\\end{aligned}$$\n",
"\n",
"and\n",
"$$\\begin{aligned}\n",
"\\frac{\\partial h}{\\partial x_{alt}} &=\\\\ &= \\frac{\\partial}{\\partial x_{alt}} \\sqrt{x_{pos}^2 + x_{alt}^2} \\\\ &= \\frac{x_{alt}}{\\sqrt{x_{pos}^2 + x_{alt}^2}}\n",
"\\end{aligned}$$\n",
"\n",
"giving us \n",
"\n",
"$$\\mathbf{H} = \n",
"\\begin{bmatrix}\n",
"\\frac{x_{pos}}{\\sqrt{x_{pos}^2 + x_{alt}^2}} & \n",
"0 &\n",
"&\n",
"\\frac{x_{alt}}{\\sqrt{x_{pos}^2 + x_{alt}^2}}\n",
"\\end{bmatrix}$$\n",
"\n",
"This may seem daunting, so step back and recognize that all of this math is just doing something very simple. We have an equation for the slant range to the airplane which is nonlinear. The Kalman filter only works with linear equations, so we need to find a linear equation that approximates $\\mathbf{H}$ As we discussed above, finding the slope of a nonlinear equation at a given point is a good approximation. For the Kalman filter, the 'given point' is the state variable $\\mathbf{x}$ so we need to take the derivative of the slant range with respect to $\\mathbf{x}$. \n",
"\n",
"To make this more concrete, let's now write a Python function that computes the Jacobian of $\\mathbf{H}$. The `ExtendedKalmanFilter` class will be using this to generate `ExtendedKalmanFilter.H` at each step of the process."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"def y(x): \n",
" return 8*x - 12.75\n",
"plt.plot (xs, ys)\n",
"plt.plot ([1.25, 1.75], [y(1.25), y(1.75)], c='r')\n",
"plt.ylim([-1.5, 1])\n",
"plt.show()"
"from math import sqrt\n",
"def HJacobian_at(x):\n",
" \"\"\" compute Jacobian of H matrix for state x \"\"\"\n",
"\n",
" horiz_dist = x[0,0]\n",
" altitude = x[2,0]\n",
" denom = sqrt(horiz_dist**2 + altitude**2)\n",
" return array ([[horiz_dist/denom, 0., altitude/denom]])"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAtUAAAF2CAYAAACh02S2AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7fb196bb4ef0>"
]
}
],
"outputs": [],
"prompt_number": 15
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We can see that the linearization is exactly correct for $x=1.5$, but very quickly diverges as x varies from 1.5. This does not constitute a proof that taking the derivative is a good linearization, but it should be fairly convincing. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We will begin by writing a simulation for the radar."
"Finally, let's provide the code for $h(\\mathbf{x})$"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import random\n",
"def hx(x):\n",
" \"\"\" compute measurement for slant range that would correspond to state x\"\"\"\n",
" return (x[0,0]**2 + x[2,0]**2) ** 0.5"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 5
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now lets write a simulation for our radar."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from numpy.random import randn\n",
"import math\n",
"\n",
"class Radar(object):\n",
" def __init__(self, pos, vel, alt, dt):\n",
"class RadarSim(object):\n",
" \"\"\" Simulates the radar signal returns from an object flying \n",
" at a constant altityude and velocity in 1D. \n",
" \"\"\"\n",
" \n",
" def __init__(self, dt, pos, vel, alt):\n",
" self.pos = pos\n",
" self.vel = vel\n",
" self.alt = alt\n",
" self.dt = dt\n",
" \n",
" def get(self):\n",
" \"\"\" Simulate radar range to object at 1K altidue and moving at 100m/s.\n",
" Adds about 5% measurement noise. Returns slant range to the object.\n",
" Call once for each new measurement at dt time from last call.\n",
" def get_range(self):\n",
" \"\"\" Returns slant range to the object. Call once for each\n",
" new measurement at dt time from last call.\n",
" \"\"\"\n",
" \n",
" # add some process noise to the system\n",
" vel = self.vel + 5*random.gauss(0,1)\n",
" alt = self.alt + 10*random.gauss(0,1)\n",
" vel = self.vel + 5*randn()\n",
" alt = self.alt + 10*randn()\n",
" self.pos = self.pos + vel*self.dt\n",
" \n",
" # add measurment noise\n",
" err = self.pos * 0.05*random.gauss(0,1)\n",
" return math.sqrt(self.pos**2 + alt**2) + err"
" err = self.pos * 0.05*randn()\n",
" slant_dist = math.sqrt(self.pos**2 + alt**2)\n",
" \n",
" return slant_dist + err"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 16
"prompt_number": 11
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"$$F = I + \\begin{bmatrix}0 & 1 & 0\\\\ 0 & 0 & 0\\\\0&0&0\\end{bmatrix}dt$$"
"Now we can implement our filter. I have not yet designed $\\mathbf{R}$ and $\\mathbf{Q}$ which is required to get optimal performance. However, we have already covered a lot of confusing material and I want you to see concrete examples as soon as possible. Therefore I will use 'reasonable' values for $\\mathbf{R}$ and $\\mathbf{Q}$.\n",
"\n",
"The `filterpy` library provides the class `ExtendedKalmanFilter`. It works very similar to the `KalmanFilter` class we have been using, except that it allows you to provide functions that compute the Jacobian of $\\mathbf{H}$ and the function $h(\\mathbf{x})$. We have already written the code for these two functions, so let's just get going.\n",
"\n",
"We start by importing the filter and creating it. There are 3 variables in `x` and only 1 measurement. At the same time we will create our radar simulator.\n",
"\n",
" from filterpy.kalman import ExtendedKalmanFilter\n",
"\n",
" rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)\n",
" radar = RadarSim(dt, pos=0., vel=100., alt=1000.)\n",
" \n",
"We will initialize the filter near the airplane's actual position\n",
"\n",
" rk.x = array([[radar.pos, radar.vel-10, radar.alt+100]]).T\n",
" \n",
"We assign the system matrix using the first term of the Taylor series expansion we computed above.\n",
"\n",
" dt = 0.05\n",
" rk.F = eye(3) + array ([[0, 1, 0],\n",
" [0, 0, 0],\n",
" [0, 0, 0]])*dt\n",
" \n",
"After assigning reasonble values to $\\mathbf{R}$, $\\mathbf{Q}$, and $\\mathbf{P}$ we can run the filter with a simple loop\n",
"\n",
" for i in range(int(20/dt)):\n",
" z = radar.get_range()\n",
" rk.update(array([[z]]), HJacobian_at, hx)\n",
" rk.predict()\n",
" \n",
"Putting that all together along with some boilerplate code to save the results and plot them, we get"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from filterpy.kalman import ExtendedKalmanFilter\n",
"from numpy import eye, array, asarray\n",
"\n",
"rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)\n",
"radar = RadarSim(dt, pos=0., vel=100., alt=1000.)\n",
"\n",
"rk.x = array([[radar.pos, radar.vel+100, radar.alt+1000]]).T\n",
"\n",
"dt = 0.05\n",
"rk.F = eye(3) + array ([[0, 1, 0],\n",
" [0, 0, 0],\n",
" [0, 0, 0]])*dt\n",
"\n",
"rk.R = radar.alt * 0.05 # 5% of distance\n",
"rk.Q = array([[0, 0, 0],\n",
" [0, 1, 0],\n",
" [0, 0, 1]]) * 0.001\n",
"\n",
"rk.P *= 50\n",
"\n",
"\n",
"xs = []\n",
"for i in range(int(20/dt)):\n",
" z = radar.get_range()\n",
" \n",
" rk.update(array([[z]]), HJacobian_at, hx)\n",
" xs.append(rk.x.T[0])\n",
" rk.predict()\n",
"\n",
"\n",
"xs = asarray(xs)\n",
"\n",
"plt.figure()\n",
"plt.plot(xs[:,0])\n",
"plt.ylabel('position')\n",
"\n",
"plt.figure()\n",
"plt.plot(xs[:,1])\n",
"plt.ylabel('velocity')\n",
"\n",
"plt.figure()\n",
"plt.plot(xs[:,2])\n",
"plt.ylabel('altitude')\n",
"plt.show()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAugAAAF2CAYAAAA83m0+AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f11361aa1d0>"
]
},
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAuEAAAF2CAYAAADAy0b0AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f113630e780>"
]
},
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAucAAAF2CAYAAADN1TazAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f11362ed048>"
]
}
],
"prompt_number": 40
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Using SymPy to compute Jacobians\n",
"\n",
"Depending on your experience with derivatives you may have found the computation of the Jacobian above either fairly straightforward, or quite difficult. Even if you found it easy, a slightly more difficult problem easily leads to very difficult computations.\n",
"\n",
"As explained in Appendix C, we can use the SymPy package to compute the Jacobian for us. "
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"x_pos, x_vel, x_alt = sympy.symbols('x_pos, x_vel x_alt')\n",
"\n",
"H = sympy.Matrix([sympy.sqrt(x_pos**2 + x_alt**2)])\n",
"\n",
"state = sympy.Matrix([x_pos, x_vel, x_alt])\n",
"H.jacobian(state)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{x_{pos}}{\\sqrt{x_{alt}^{2} + x_{pos}^{2}}} & 0 & \\frac{x_{alt}}{\\sqrt{x_{alt}^{2} + x_{pos}^{2}}}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 48,
"text": [
"\u23a1 x_pos x_alt \u23a4\n",
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\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\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_alt + x_pos \u2572\u2571 x_alt + x_pos \u23a6"
]
}
],
"prompt_number": 48
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This result is the same as the result we computed above, and at much less effort on our part!"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Designing Q\n",
"\n",
"Now we need to design the process noise matrix $\\mathbf{Q}$. From the previous section we have the system equation\n",
"\n",
"$$\\dot{\\mathbf{x}} = \\begin{bmatrix} 0 & 1 & 0 \\\\ 0& 0& 0 \\\\ 0&0&0\\end{bmatrix}\n",
"\\begin{bmatrix}x_{pos} \\\\x_{vel}\\\\ x_{alt}\\end{bmatrix} + \\begin{bmatrix}0 \\\\w_{vel}\\\\ w_{alt}\\end{bmatrix}\n",
"$$\n",
"\\begin{aligned}\n",
"H&=\\begin{bmatrix}\n",
"\\frac{\\partial h}{\\partial x_{pos}} & \n",
"\\frac{\\partial h}{\\partial x_{vel}} &\n",
"\\frac{\\partial h}{\\partial x_{alt}}\\end{bmatrix} \\\\\n",
"&= \\begin{bmatrix}\n",
"\\frac{x_{pos}}{\\sqrt{x_{pos}^2 + x_{alt}^2}} & 0 & \\frac{x_{alt}}{\\sqrt{x_{pos}^2 + x_{alt}^2}}\n",
"\\end{bmatrix}\n",
"\\end{aligned}\n",
"$$"
"\n",
"where our process noise is\n",
"\n",
"$$w = \\begin{bmatrix}0 \\\\w_{vel}\\\\ w_{alt}\\end{bmatrix}$$\n",
"\n",
"We know from the Kalman filter math chapter that \n",
"\n",
"$$\\mathbf{Q} = E(ww^T)$$\n",
"\n",
"where $E(\\bullet)$ is the expected value. We compute the expected value as\n",
"\n",
"$$\\mathbf{Q} = \\int_0^{dt} \\Phi(t)\\mathbf{Q}\\Phi^T(t) dt$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Rather than do this by hand, let's use sympy."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import sympy\n",
"w_vel, w_alt, dt = sympy.symbols('w_vel w_alt \\Delta{t}')\n",
"w = sympy.Matrix([[0, w_vel, w_alt]]).T\n",
"phi = sympy.Matrix([[1, dt, 0], [0, 1, 0], [0,0,1]])\n",
"\n",
"q = w*w.T\n",
"\n",
"sympy.integrate(phi*q*phi.T, (dt, 0, dt))"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{\\Delta{t}^{3} w_{vel}^{2}}{3} & \\frac{\\Delta{t}^{2} w_{vel}^{2}}{2} & \\frac{w_{alt} w_{vel}}{2} \\Delta{t}^{2}\\\\\\frac{\\Delta{t}^{2} w_{vel}^{2}}{2} & \\Delta{t} w_{vel}^{2} & \\Delta{t} w_{alt} w_{vel}\\\\\\frac{w_{alt} w_{vel}}{2} \\Delta{t}^{2} & \\Delta{t} w_{alt} w_{vel} & \\Delta{t} w_{alt}^{2}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 15,
"text": [
"\u23a1 3 2 2 2 2 \u23a4\n",
"\u23a2 \\Delta{t} \u22c5w_vel \\Delta{t} \u22c5w_vel \\Delta{t} \u22c5w_alt\u22c5w_vel\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\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 3 2 2 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 2 2 \u23a5\n",
"\u23a2 \\Delta{t} \u22c5w_vel 2 \u23a5\n",
"\u23a2 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \\Delta{t}\u22c5w_vel \\Delta{t}\u22c5w_alt\u22c5w_vel \u23a5\n",
"\u23a2 2 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 2 \u23a5\n",
"\u23a2\\Delta{t} \u22c5w_alt\u22c5w_vel 2 \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 \\Delta{t}\u22c5w_alt\u22c5w_vel \\Delta{t}\u22c5w_alt \u23a5\n",
"\u23a3 2 \u23a6"
]
}
],
"prompt_number": 15
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"f21, f22, t, phi = sympy.symbols('f_21 f_22 t \\Phi_S')\n",
"Q_t = sympy.Matrix([[0,0],[0,phi]])\n",
"f = sympy.Matrix([[1,dt],[f21, 1+f22*dt]])\n",
"term = f*Q_t*f.T\n",
"term\n",
"sympy.integrate(term,(dt, 0, dt))\n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{\\Delta{t}^{3} \\Phi_{S}}{3} & \\frac{\\Phi_{S} f_{22}}{3} \\Delta{t}^{3} + \\frac{\\Delta{t}^{2} \\Phi_{S}}{2}\\\\\\frac{\\Phi_{S} f_{22}}{3} \\Delta{t}^{3} + \\frac{\\Delta{t}^{2} \\Phi_{S}}{2} & \\frac{\\Delta{t}^{3} \\Phi_{S}}{3} f_{22}^{2} + \\Delta{t}^{2} \\Phi_{S} f_{22} + \\Delta{t} \\Phi_{S}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 40,
"text": [
"\u23a1 3 3 \n",
"\u23a2 \\Delta{t} \u22c5\\Phi_S \\Delta{t} \u22c5\\Phi_S\u22c5f\u2082\u2082 \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\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \n",
"\u23a2 3 3 \n",
"\u23a2 \n",
"\u23a2 3 2 3 2 \n",
"\u23a2\\Delta{t} \u22c5\\Phi_S\u22c5f\u2082\u2082 \\Delta{t} \u22c5\\Phi_S \\Delta{t} \u22c5\\Phi_S\u22c5f\u2082\u2082 \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\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 + \\Delta{t}\n",
"\u23a3 3 2 3 \n",
"\n",
" 2 \u23a4\n",
" \\Delta{t} \u22c5\\Phi_S \u23a5\n",
"+ \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u23a5\n",
" 2 \u23a5\n",
" \u23a5\n",
" \u23a5\n",
"2 \u23a5\n",
" \u22c5\\Phi_S\u22c5f\u2082\u2082 + \\Delta{t}\u22c5\\Phi_S\u23a5\n",
" \u23a6"
]
}
],
"prompt_number": 40
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"#Bar-Shalom P270\n",
"T,t,dt = sympy.symbols('T t \\Delta{t}')\n",
"v_k = sympy.Matrix([[T-t],[1]])\n",
"term = v_k*v_k.T\n",
"term\n",
"sympy.integrate(v_k*v_k.T,(t, 0, T))\n",
"\n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{T^{3}}{3} & \\frac{T^{2}}{2}\\\\\\frac{T^{2}}{2} & T\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 51,
"text": [
"\u23a1 3 2\u23a4\n",
"\u23a2T T \u23a5\n",
"\u23a2\u2500\u2500 \u2500\u2500\u23a5\n",
"\u23a23 2 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 2 \u23a5\n",
"\u23a2T \u23a5\n",
"\u23a2\u2500\u2500 T \u23a5\n",
"\u23a32 \u23a6"
]
}
],
"prompt_number": 51
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"#Bar-Shalom P272\n",
"T,t,dt = sympy.symbols('T t \\Delta{t}')\n",
"v_k = sympy.Matrix([[(T-t)*(T-t)/2], [T-t], [1]])\n",
"term = v_k*v_k.T\n",
"term\n",
"print(sympy.pretty(sympy.integrate(v_k*v_k.T,(t, 0, T))))\n",
"((T-t)*(T-t)/2).expand()\n",
"\n",
"wk=sympy.Matrix([[T**2/2,T,1]]).T\n",
"wk*wk.T\n",
"sympy.integrate(wk*wk.T, T)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"\u23a1 5 4 3\u23a4\n",
"\u23a2T T T \u23a5\n",
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
"\u23a220 8 6 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 4 3 2\u23a5\n",
"\u23a2T T T \u23a5\n",
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
"\u23a28 3 2 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 3 2 \u23a5\n",
"\u23a2T T \u23a5\n",
"\u23a2\u2500\u2500 \u2500\u2500 T \u23a5\n",
"\u23a36 2 \u23a6\n"
]
},
{
"latex": [
"$$\\left[\\begin{matrix}\\frac{T^{5}}{20} & \\frac{T^{4}}{8} & \\frac{T^{3}}{6}\\\\\\frac{T^{4}}{8} & \\frac{T^{3}}{3} & \\frac{T^{2}}{2}\\\\\\frac{T^{3}}{6} & \\frac{T^{2}}{2} & T\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 72,
"text": [
"\u23a1 5 4 3\u23a4\n",
"\u23a2T T T \u23a5\n",
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
"\u23a220 8 6 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 4 3 2\u23a5\n",
"\u23a2T T T \u23a5\n",
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
"\u23a28 3 2 \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 3 2 \u23a5\n",
"\u23a2T T \u23a5\n",
"\u23a2\u2500\u2500 \u2500\u2500 T \u23a5\n",
"\u23a36 2 \u23a6"
]
}
],
"prompt_number": 72
},
{
"cell_type": "heading",
"level": 3,
11_Designing_Nonlinear_Kalman_Filters/Designing_Nonlinear_Kalman_Filters.ipynb
+7 -8
View File
@@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:1c019f940c5d07d4049402e4d9271803fc1fd66617cda061f1a54a65015769fc"
"signature": "sha256:928cf8eeb269cc5e75d780bd7c29a146c7558c45f138798038288577c813816c"
},
"nbformat": 3,
"nbformat_minor": 0,
@@ -261,7 +261,7 @@
"output_type": "pyout",
"prompt_number": 1,
"text": [
"<IPython.core.display.HTML at 0x7f571d5ff7b8>"
"<IPython.core.display.HTML at 0x7f4ed5fb65f8>"
]
}
],
@@ -377,8 +377,7 @@
"cell_type": "code",
"collapsed": false,
"input": [
"\n",
"circle1=plt.Circle((-4,0),5,color='#004080',fill=False,linewidth=10, alpha=.7)\n",
"circle1=plt.Circle((-4,0),5,color='#004080',fill=False,linewidth=20, alpha=.7)\n",
"circle2=plt.Circle((4,0),5,color='#E24A33', fill=False, linewidth=5, alpha=.7)\n",
"\n",
"fig = plt.gcf()\n",
@@ -404,19 +403,19 @@
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAsMAAAFwCAYAAABdOnYEAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"png": "iVBORw0KGgoAAAANSUhEUgAAAsMAAAFwCAYAAABdOnYEAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f57051417b8>"
"<matplotlib.figure.Figure at 0x7f4eb98327f0>"
]
}
],
"prompt_number": 2
"prompt_number": 3
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here I have attempted to show transmitter A, drawn in red, at (-4,0) and a second one B, drawn in blue, at (4,0). The red and blue circles show the range from the transmitters to the robot, with the width illustrating the effect of the $1\\sigma$ angular error for each transmitter. Here I have given the red transmitter more error than the blue one. The most probable position for the robot is where the two circles intersect, which I have depicted with the red and blue lines. You will object that we have two intersections, not one, but we will see how we deal with that when we design the measurement function.\n",
"Here I have attempted to show transmitter A, drawn in red, at (-4,0) and a second one B, drawn in blue, at (4,0). The red and blue circles show the range from the transmitters to the robot, with the width illustrating the effect of the $1\\sigma$ angular error for each transmitter. Here I have given the blue transmitter more error than the red one. The most probable position for the robot is where the two circles intersect, which I have depicted with the red and blue lines. You will object that we have two intersections, not one, but we will see how we deal with that when we design the measurement function.\n",
"\n",
"This is a very common sensor set up. Aircraft still use this system to navigate, where it is called DME (Distance Measuring Equipment). Today GPS is a much more common navigation sytem, but I have worked on an aircraft where we integrated sensors like this into our filter along with the GPS, INS, altimeters, etc. We will tackle what is called *multi-sensor fusion* later; for now we will just address this simple configuration.\n",
"\n",
Appendix_C_Using_NumPy_SciPy_and_SymPy/Using_NumPy_SciPy_and_SymPy.ipynb
+584
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": {}
}
]
}