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

fixed spelling of directory

Browse Source
This commit is contained in:
Roger Labbe committed 2015-02-16 07:08:15 -08:00
1 parent 5843306598
commit aed695d102
33 files changed
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

experiments/1d_kf_compare.ipynb
+383
View File
@@ -0,0 +1,383 @@
{
"metadata": {
"name": "",
"signature": "sha256:df9fb9c477b6a2610674b7ee0fbb91de2fede5f150497593d932da77a6f002ba"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Compare 1d kalman filter with full kalman filter with only 1 observed state variable."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from __future__ import division\n",
"import numpy as np\n",
"\n",
"def predict(pos, variance, movement, movement_variance):\n",
" return (pos + movement, variance + movement_variance)\n",
" \n",
"def update(mean, variance, measurement, measurement_variance):\n",
" return multiply(mean, variance, measurement, measurement_variance)\n",
" \n",
"def multiply(mu1, var1, mu2, var2):\n",
" mean = (var1*mu2 + var2*mu1) / (var1+var2)\n",
" variance = 1 / (1/var1 + 1/var2)\n",
" return (mean, variance)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 1
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"%matplotlib inline\n",
"from numpy.random import randn\n",
"import numpy as np\n",
"from filterpy.kalman import KalmanFilter\n",
"from filterpy.leastsq import LeastSquaresFilter\n",
"import matplotlib.pyplot as plt\n",
"import matplotlib.pylab as pylab\n",
"\n",
"pylab.rcParams['figure.figsize'] = (16.0, 16.0)\n",
"\n",
"lsq = LeastSquaresFilter(dt=1, order=2, noise_variance = 5)\n",
"\n",
"f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)\n",
"f.P = 500\n",
"f.H = np.array([[1.]])\n",
"f.F = np.array([[1.]])\n",
"f.B = np.array([[1.]])\n",
"f.Q = 3\n",
"f.R = 5\n",
"\n",
"pos = (0., 500.)\n",
"print (pos)\n",
"plt.figure()\n",
"for i in range(50):\n",
" z = i + randn()\n",
" pos = update(pos[0], pos[1], z, 5)\n",
" x = lsq(z)\n",
" f.update(z)\n",
" \n",
" plt.scatter(i, pos[0], c='b')\n",
" plt.scatter(i, x, c='g')\n",
" plt.scatter(i, f.x[0,0], c='r')\n",
" \n",
" assert abs(pos[0]- f.x[0,0]) < 1.e-12\n",
" \n",
" pos = predict(pos[0], pos[1], 1., 3.)\n",
" f.predict(u=1)\n",
" \n",
"plt.show()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"(0.0, 500.0)\n"
]
},
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAA6gAAAOeCAYAAAAOYj2YAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f6e93fff358>"
]
}
],
"prompt_number": 2
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"%matplotlib inline\n",
"from numpy.random import randn\n",
"import numpy as np\n",
"from filterpy.kalman import KalmanFilter\n",
"from filterpy.leastsq import LeastSquaresFilter\n",
"import matplotlib.pyplot as plt\n",
"import matplotlib.pylab as pylab\n",
"\n",
"pylab.rcParams['figure.figsize'] = (16.0, 16.0)\n",
"\n",
"lsq = LeastSquaresFilter(dt=1, order=2, noise_variance = 5)\n",
"\n",
"f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)\n",
"f.P = 500\n",
"f.H = np.array([[1.]])\n",
"f.F = np.array([[1.]])\n",
"f.B = np.array([[1.]])\n",
"f.Q = .02\n",
"f.R = 5\n",
"\n",
"kxs = []\n",
"lxs = []\n",
"kKs = []\n",
"lKs = []\n",
"zs = []\n",
"\n",
"for i in range(500):\n",
" z = i + randn()*5\n",
" zs.append(z)\n",
"\n",
" lsq(z)\n",
" f.update(z)\n",
" \n",
" kxs.append(f.x[0,0])\n",
" lxs.append(lsq.x)\n",
" \n",
" kKs.append(f.K[0,0])\n",
" lKs.append(lsq.K2)\n",
"\n",
" f.predict(u=1)\n",
" \n",
" \n",
"plt.figure()\n",
"plt.plot (kxs, c='b')\n",
"plt.plot (lxs, c='g')\n",
"plt.plot (zs, c='r')\n",
"\n",
"plt.figure()\n",
"plt.plot (kKs, c='b')\n",
"plt.plot (lKs, c='r')\n",
" \n",
"plt.show()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAA7EAAAOeCAYAAADV2Ze6AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f6e8f256ac8>"
]
},
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAA6YAAAOeCAYAAAAQqw0rAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f6e8f2af978>"
]
}
],
"prompt_number": 19
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"print(lKs[-1])\n",
"print(kKs[-1])\n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"0.00014299687477634373\n",
"0.0612771680782\n"
]
}
],
"prompt_number": 18
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"# pos,vel filter\n",
"\n",
"%matplotlib inline\n",
"from numpy.random import randn\n",
"import numpy as np\n",
"from filterpy.kalman import KalmanFilter\n",
"from filterpy.leastsq import LeastSquaresFilter\n",
"import matplotlib.pyplot as plt\n",
"import matplotlib.pylab as pylab\n",
"\n",
"pylab.rcParams['figure.figsize'] = (16.0, 16.0)\n",
"\n",
"lsq = LeastSquaresFilter(dt=1, order=2, noise_variance = 5)\n",
"\n",
"f = KalmanFilter(dim_x=2, dim_z=1, dim_u=0)\n",
"f.P *= 500\n",
"f.F = np.array([[1., 1.],\n",
" [0., 1.]])\n",
"f.H = np.array([[1., 0.]])\n",
"f.Q *= .001\n",
"f.R *= 5\n",
"\n",
"kxs = []\n",
"lxs = []\n",
"kKs = []\n",
"lKs = []\n",
"zs = []\n",
"\n",
"for i in range(100):\n",
" z = i + randn()*5\n",
" zs.append(z)\n",
"\n",
" lsq(z)\n",
" f.update(z)\n",
" \n",
" kxs.append(f.x[0,0])\n",
" lxs.append(lsq.x)\n",
" \n",
" kKs.append(f.K[0,0])\n",
" lKs.append(lsq.K2)\n",
"\n",
" f.predict(u=1)\n",
" \n",
" \n",
"plt.figure()\n",
"plt.plot (kxs, c='b')\n",
"plt.plot (lxs, c='g')\n",
"plt.plot (zs, c='r')\n",
"\n",
"plt.figure()\n",
"plt.plot (kKs, c='b')\n",
"plt.plot (lKs, c='r')\n",
" \n",
"plt.show()\n",
"\n",
"#print(f.K)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAA6sAAAOeCAYAAADlVYabAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f6e8f947518>"
]
},
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAA6YAAAOeCAYAAAAQqw0rAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f6e8f739710>"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"[[ 0.15538196]\n",
" [ 0.01299707]]\n"
]
}
],
"prompt_number": 38
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"#testing a constant K gain filter implementation\n",
"\n",
"from numpy import dot\n",
"class kf(object):\n",
" \n",
" def __init__ (self):\n",
" self.H = 1\n",
" self.x = 0\n",
" self.y = 0\n",
" self.F = 1\n",
" self.B = 0\n",
" self.K = 1\n",
" \n",
" \n",
" def update(self, Z):\n",
" self.y = Z - dot(self.H, self.x)\n",
" \n",
" # x = x + Ky\n",
" # predict new x with residual scaled by the kalman gain\n",
" self.x = self.x + dot(self.K, self.y)\n",
"\n",
"\n",
" def predict(self, u=0):\n",
" # x = Fx + Bu\n",
" self._x = dot(self.F, self.x) + dot(self.B, u) \n",
" \n",
" \n",
"k = kf()\n",
"k.x = np.array([[1., 0.]]).T\n",
"k.F = np.array([[1., 1.],\n",
" [0., 1.]])\n",
"k.H = np.array([[1., 0.]])\n",
"k.K = np.array([[0.15538196, 0.01299707]]).T\n",
"lsq = LeastSquaresFilter(dt=1, order=2, noise_variance = 5)\n",
"\n",
"\n",
"kxs = []\n",
"lxs = []\n",
"zs = []\n",
"for i in range(1,100):\n",
" z = i + randn()*5\n",
" zs.append(z)\n",
"\n",
" lsq(z)\n",
" k.predict()\n",
" k.update(z)\n",
" \n",
" kxs.append(k.x[0,0])\n",
" lxs.append(lsq.x)\n",
" \n",
"plt.figure()\n",
"plt.plot (kxs, c='b')\n",
"plt.plot (lxs, c='g')\n",
"plt.plot (zs, c='r')\n",
"plt.show()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAA6kAAAOeCAYAAADhoFamAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f6e8f734f28>"
]
}
],
"prompt_number": 49
}
],
"metadata": {}
}
]
}
experiments/1dposvel.ipynb
+415
View File
@@ -0,0 +1,415 @@
{
"metadata": {
"name": "",
"signature": "sha256:11e1418ff66f54f922969f038a790dfe89ea35dd53a49764ef0cb06a9c32f67b"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"%matplotlib inline\n",
"from __future__ import division, print_function\n",
"import matplotlib.pyplot as plt\n",
"import numpy as np\n",
"from numpy.random import randn\n",
"from numpy import array, asarray\n",
"\n",
"import sys\n",
"sys.path.insert(0, '../code') # allow us to import book_format\n",
"import book_format\n",
"book_format.load_style()\n"
],
"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 0x7f7aee4a4978>"
]
}
],
"prompt_number": 1
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"pos = 1\n",
"vel = 1\n",
"p_pos = 1\n",
"p_move = .5\n",
"\n",
"data = [(0,pos+randn()*p_pos) for i in range(1000)]\n",
"data = np.asarray(data)\n",
"\n",
"pos += vel\n",
"\n",
"\n",
"plt.scatter(data[:,0], data[:,1], marker=',')\n",
"plt.show()\n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAtIAAAF2CAYAAABDD3/PAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f7aee499b70>"
]
}
],
"prompt_number": 3
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from filterpy.kalman import KalmanFilter\n",
"from numpy.random import multivariate_normal\n",
"import matplotlib.cm as cm\n",
"\n",
"vel = 20.\n",
"kf = KalmanFilter(dim_x=2, dim_z=1)\n",
"kf.F = array([[1, 1], [0, 1]], dtype=float)\n",
"kf.H = array([[1, 0]])\n",
"kf.x = array([[0., vel]]).T\n",
"kf.P *= 100\n",
"kf.R *= 10\n",
"kf.Q *= .0001\n",
"\n",
"steps = 6\n",
"colors = cm.winter(np.linspace(0, .5, steps))\n",
"pcolors = cm.Reds(np.linspace(.5, 1, steps))\n",
"for i in range(steps):\n",
" kf.predict()\n",
" x,y = multivariate_normal(mean=kf.x.T[0], cov=kf.P, size=1000).T\n",
" plt.scatter(x,y, marker=',', c=pcolors[i])\n",
" \n",
" z = (i+1)*vel\n",
" kf.update(z)\n",
" x,y = multivariate_normal(mean=kf.x.T[0], cov=kf.P, size=1000).T\n",
" plt.scatter(x,y, marker='.', c=colors[i], alpha=0.7)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAs4AAAF2CAYAAAB+nR6pAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f7ad12f6da0>"
]
}
],
"prompt_number": 99
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"def update(mu1, var1, mu2, var2):\n",
" if var1 == 0.0:\n",
" var1=1.e-80\n",
" \n",
" if var2 == 0:\n",
" var2 = 1e-80\n",
" \n",
" mean = (var1*mu2 + var2*mu1) / (var1+var2)\n",
" variance = 1. / (1/var1 + 1/var2)\n",
" return (mean, variance)\n",
"\n",
"\n",
"\n",
"def predict(mu1, var1, mu2, var2):\n",
" return (mu1+mu2, var1+var2)\n",
"\n",
"pos = 0\n",
"pos_p = 100.\n",
"move = 1\n",
"move_p = .1\n",
"sense_p = 8\n",
"\n",
"N = 10\n",
"colors = cm.winter(np.linspace(0, .5, N))\n",
"pcolors = cm.Reds(np.linspace(.5, 1, N))\n",
"for i in range(N):\n",
" pos, pos_p = predict(pos, pos_p, move, move_p)\n",
" x,y = multivariate_normal(mean=(pos,i), cov=[[0.001, 0],[0, p_pos]], size=1000).T\n",
" plt.scatter(x,y, marker=',', c=colors[i]) \n",
" print(pos, pos_p)\n",
" \n",
" pos, pos_p = update(pos, pos_p, i, sense_p)\n",
" x,y = multivariate_normal(mean=(i, pos), cov=asarray([[0.001, 0],[0, pos_p]]).T, size=1000).T\n",
" plt.scatter(x,y, marker='.', c=pcolors[i], alpha=0.5) \n",
" \n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"1 100.1\n",
"1.0740055504162813 7.50795559666975\n",
"2.0381768183201006 3.9730859395971154\n",
"3.02550842348427 2.7546779733418045\n",
"4.018974755764885 2.1491012228687625\n",
"5.014956796940505"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
" 1.7940228898505703\n",
"6.012217081465885 1.5654022438192952\n",
"7.010217725218009 1.409220211690132\n",
"8.008687415099768 1.2981611057964606\n",
"9.007474523188765 1.2169185743509554\n"
]
},
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAAsoAAAF2CAYAAAB3dr7TAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text": [
"<matplotlib.figure.Figure at 0x7f7ac824d320>"
]
}
],
"prompt_number": 100
}
],
"metadata": {}
}
]
}
experiments/2014-03-26-000-Data.csv
+10801
View File
File diff suppressed because it is too large. Load diff
experiments/DiscreteBayes1D.py
+149
View File
@@ -0,0 +1,149 @@
# -*- coding: utf-8 -*-
"""
Created on Fri May 2 11:48:55 2014
@author: rlabbe
"""
from __future__ import division
from __future__ import print_function
import copy
import numpy as np
import bar_plot
import numpy.random as random
import matplotlib.pyplot as plt
''' should this be a class? seems like both sense and update are very
problem specific
'''
class DiscreteBayes1D(object):
def __init__(self, world_map, belief=None):
self.world_map = copy.deepcopy(world_map)
self.N = len(world_map)
if belief is None:
# create belief, make all equally likely
self.belief = np.empty(self.N)
self.belief.fill (1./self.N)
else:
self.belief = copy.deepcopy(belief)
# This will be used to temporarily store calculations of the new
# belief. 'k' just means 'this iteration'.
self.belief_k = np.empty(self.N)
assert self.belief.shape == self.world_map.shape
def _normalize (self):
s = sum(self.belief)
self.belief = self.belief/s
def sense(self, Z, pHit, pMiss):
for i in range (self.N):
hit = (self.world_map[i] ==Z)
self.belief_k[i] = self.belief[i] * (pHit*hit + pMiss*(1-hit))
# copy results to the belief vector using swap-copy idiom
self.belief, self.belief_k = self.belief_k, self.belief
self._normalize()
def update(self, U, kernel):
N = self.N
kN = len(kernel)
width = int((kN - 1) / 2)
self.belief_k.fill(0)
for i in range(N):
for k in range (kN):
index = (i + (width-k)-U) % N
#print(i,k,index)
self.belief_k[i] += self.belief[index] * kernel[k]
# copy results to the belief vector using swap-copy idiom
self.belief, self.belief_k = self.belief_k, self.belief
def add_noise (Z, count):
n= len(Z)
for i in range(count):
j = random.randint(0,n)
Z[j] = random.randint(0,2)
def animate_three_doors (loops=5):
world = np.array([1,0,1,0,0,0,0,1,0,0,0,1,0,0,0,0,0])
#world = np.array([1,1,1,1,1])
#world = np.array([1,0,1,0,1,0])
f = DiscreteBayes1D(world)
measurements = np.tile(world,loops)
add_noise(measurements, 4)
for m in measurements:
f.sense (m, .8, .2)
f.update(1, (.05, .9, .05))
bar_plot.plot(f.belief)
plt.pause(0.01)
def _test_filter():
def is_near_equal(a,b):
try:
assert sum(abs(a-b)) < 0.001
except:
print(a, b)
assert False
def test_update_1():
f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0]))
f.update (1, [1])
is_near_equal (f.belief, np.array([0,0,0,.8,0]))
f.update (1, [1])
is_near_equal (f.belief, np.array([0,0,0,0,.8]))
f.update (1, [1])
is_near_equal (f.belief, np.array([.8,0,0,0,0]))
f.update (-1, [1])
is_near_equal (f.belief, np.array([0,0,0,0,.8]))
f.update (2, [1])
is_near_equal (f.belief, np.array([0,.8,0,0,0]))
f.update (5, [1])
is_near_equal (f.belief, np.array([0,.8,0,0,0]))
def test_undershoot():
f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0]))
f.update (2, [.2, .8,0.])
is_near_equal (f.belief, np.array([0,0,0,.16,.64]))
def test_overshoot():
f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0]))
f.update (2, [0, .8, .2])
is_near_equal (f.belief, np.array([.16,0,0,0,.64]))
test_update_1()
test_undershoot()
if __name__ == "__main__":
_test_filter()
#animate_three_doors(loops=1)
experiments/RungeKutta.py
+189
View File
@@ -0,0 +1,189 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jul 05 09:54:39 2014
@author: rlabbe
"""
from __future__ import division, print_function
import matplotlib.pyplot as plt
from scipy.integrate import ode
import math
import numpy as np
from numpy import random, radians, cos, sin
class BallTrajectory2D(object):
def __init__(self, x0, y0, velocity, theta_deg=0., g=9.8, noise=[0.0,0.0]):
theta = radians(theta_deg)
self.vx0 = velocity * cos(theta)
self.vy0 = velocity * sin(theta)
self.x0 = x0
self.y0 = y0
self.x = x
self.g = g
self.noise = noise
def position(self, t):
""" returns (x,y) tuple of ball position at time t"""
self.x = self.vx0*t + self.x0
self.y = -0.5*self.g*t**2 + self.vy0*t + self.y0
return (self.x +random.randn()*self.noise[0], self.y +random.randn()*self.noise[1])
class BallEuler(object):
def __init__(self, y=100., vel=10., omega=0):
self.x = 0.
self.y = y
omega = radians(omega)
self.vel = vel*np.cos(omega)
self.y_vel = vel*np.sin(omega)
def step (self, dt):
g = -9.8
self.x += self.vel*dt
self.y += self.y_vel*dt
self.y_vel += g*dt
#print self.x, self.y
def rk4(y, x, dx, f):
"""computes 4th order Runge-Kutta for dy/dx.
y is the initial value for y
x is the initial value for x
dx is the difference in x (e.g. the time step)
f is a callable function (y, x) that you supply to compute dy/dx for
the specified values.
"""
k1 = dx * f(y, x)
k2 = dx * f(y + 0.5*k1, x + 0.5*dx)
k3 = dx * f(y + 0.5*k2, x + 0.5*dx)
k4 = dx * f(y + k3, x + dx)
return y + (k1 + 2*k2 + 2*k3 + k4) / 6.
def fx(x,t):
return fx.vel
def fy(y,t):
return fy.vel - 9.8*t
class BallRungeKutta(object):
def __init__(self, x=0, y=100., vel=10., omega = 0.0):
self.x = x
self.y = y
self.t = 0
omega = math.radians(omega)
fx.vel = math.cos(omega) * vel
fy.vel = math.sin(omega) * vel
def step (self, dt):
self.x = rk4 (self.x, self.t, dt, fx)
self.y = rk4 (self.y, self.t, dt, fy)
self.t += dt
print(fx.vel)
return (self.x, self.y)
def ball_scipy(y0, vel, omega, dt):
vel_y = math.sin(math.radians(omega)) * vel
def f(t,y):
return vel_y-9.8*t
solver = ode(f).set_integrator('dopri5')
solver.set_initial_value(y0)
ys = [y0]
while brk.y >= 0:
t += dt
brk.step (dt)
ys.append(solver.integrate(t))
def RK4(f):
return lambda t, y, dt: (
lambda dy1: (
lambda dy2: (
lambda dy3: (
lambda dy4: (dy1 + 2*dy2 + 2*dy3 + dy4)/6
)( dt * f( t + dt , y + dy3 ) )
)( dt * f( t + dt/2, y + dy2/2 ) )
)( dt * f( t + dt/2, y + dy1/2 ) )
)( dt * f( t , y ) )
def theory(t): return (t**2 + 4)**2 /16
from math import sqrt
dy = RK4(lambda t, y: t*sqrt(y))
t, y, dt = 0., 1., .1
while t <= 10:
if abs(round(t) - t) < 1e-5:
print("y(%2.1f)\t= %4.6f \t error: %4.6g" % (t, y, abs(y - theory(t))))
t, y = t + dt, y + dy(t, y, dt)
t = 0.
y=1.
def test(y, t):
return t*sqrt(y)
while t <= 10:
if abs(round(t) - t) < 1e-5:
print("y(%2.1f)\t= %4.6f \t error: %4.6g" % (t, y, abs(y - theory(t))))
y = rk4(y, t, dt, test)
t += dt
if __name__ == "__main__":
1/0
dt = 1./30
y0 = 15.
vel = 100.
omega = 30.
vel_y = math.sin(math.radians(omega)) * vel
def f(t,y):
return vel_y-9.8*t
be = BallEuler (y=y0, vel=vel,omega=omega)
#be = BallTrajectory2D (x0=0, y0=y0, velocity=vel, theta_deg = omega)
ball_rk = BallRungeKutta (y=y0, vel=vel, omega=omega)
while be.y >= 0:
be.step (dt)
ball_rk.step(dt)
print (ball_rk.y - be.y)
'''
p1 = plt.scatter (be.x, be.y, color='red')
p2 = plt.scatter (ball_rk.x, ball_rk.y, color='blue', marker='v')
plt.legend([p1,p2], ['euler', 'runge kutta'])
'''
experiments/Untitled0.ipynb
+69
View File
@@ -0,0 +1,69 @@
{
"metadata": {
"name": "",
"signature": "sha256:d69b1215417e5c675abdbd0de4e82039e393b8a3be44f039c407180b720444c8"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"from IPython.display import Image\n",
"Image(url='http://python.org/images/python-logo.gif', format='gif')"
],
"language": "python",
"metadata": {},
"outputs": [
{
"html": [
"<img src=\"http://python.org/images/python-logo.gif\"/>"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 5,
"text": [
"<IPython.core.display.Image at 0x7fc664185e10>"
]
}
],
"prompt_number": 5
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"Image(url='https://raw.githubusercontent.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/master/basic_animation.gif', format='gif')\n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"html": [
"<img src=\"https://raw.githubusercontent.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/master/basic_animation.gif\"/>"
],
"metadata": {},
"output_type": "pyout",
"prompt_number": 15,
"text": [
"<IPython.core.display.Image at 0x7fc6640f4390>"
]
}
],
"prompt_number": 15
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<img src=\"/home/rlabbe/Downloads/basic_animation\">"
]
}
],
"metadata": {}
}
]
}
experiments/Untitled1.ipynb
+179
View File
@@ -0,0 +1,179 @@
{
"metadata": {
"name": "",
"signature": "sha256:fc6923e502800422d1ca66394e71d567efb82c4dcd09c273561149f8d3b00fe5"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "heading",
"level": 1,
"metadata": {},
"source": [
"Complementary Filter"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Convert continuous transfer function into discrete\n",
"\n"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import scipy.signal as sig\n",
"from math import sqrt\n",
"\n",
"dt = 0.01\n",
"ki = 0.1**2\n",
"kp = sqrt(2)*0.1\n",
"\n",
"num = [kp, ki]\n",
"den = [1, 0.]\n",
"sig.cont2discrete(sys=(num,den), dt=0.1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 8,
"text": [
"(array([[ 0.14142136, -0.14042136]]), array([ 1., -1.]), 0.1)"
]
}
],
"prompt_number": 8
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"a = 5\n",
"num = [a, 0]\n",
"den = [a, 1]\n",
"sig.cont2discrete(sys=(num,den), dt=0.1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 11,
"text": [
"(array([[ 1., -1.]]), array([ 1. , -0.98019867]), 0.1)"
]
}
],
"prompt_number": 11
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from sympy import *\n",
"from sympy.abc import *\n",
"a = Symbol('a', positive=True)\n",
"foo = (s/(s+a)).rewrite()\n",
"print(foo)\n",
"inverse_laplace_transform(1/((s+a)), s, t)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"s/(a + s)\n"
]
},
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 39,
"text": [
"exp(-a*t)*Heaviside(t)"
]
}
],
"prompt_number": 39
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
}
],
"metadata": {}
}
]
}
experiments/__init__.py
View File
Whitespace-only changes.
experiments/ball.py
+322
View File
@@ -0,0 +1,322 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jul 5 16:07:29 2014
@author: rlabbe
"""
import numpy as np
from KalmanFilter import KalmanFilter
from math import radians, sin, cos, sqrt, exp
import numpy.random as random
import matplotlib.markers as markers
import matplotlib.pyplot as plt
from RungeKutta import *
class BallPath(object):
def __init__(self, x0, y0, omega_deg, velocity, g=9.8, noise=[1.0,1.0]):
omega = radians(omega_deg)
self.vx0 = velocity * cos(omega)
self.vy0 = velocity * sin(omega)
self.x0 = x0
self.y0 = y0
self.g = g
self.noise = noise
def pos_at_t(self, t):
""" returns (x,y) tuple of ball position at time t"""
x = self.vx0*t + self.x0
y = -0.5*self.g*t**2 + self.vy0*t + self.y0
return (x +random.randn()*self.noise[0], y +random.randn()*self.noise[1])
def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
g = 9.8 # gravitational constant
f1 = KalmanFilter(dim_x=5, dim_z=2)
ay = .5*dt**2
f1.F = np.mat ([[1, dt, 0, 0, 0], # x = x0+dx*dt
[0, 1, 0, 0, 0], # dx = dx
[0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 1]]) # ddy = -g.
f1.H = np.mat([
[1, 0, 0, 0, 0],
[0, 0, 1, 0, 0]])
f1.R *= r
f1.Q *= q
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x,vx,y,vy,-9.8]).T
return f1
def ball_kf_noay(x, y, omega, v0, dt, r=0.5, q=0.02):
g = 9.8 # gravitational constant
f1 = KalmanFilter(dim_x=5, dim_z=2)
ay = .5*dt**2
f1.F = np.mat ([[1, dt, 0, 0, 0], # x = x0+dx*dt
[0, 1, 0, 0, 0], # dx = dx
[0, 0, 1, dt, 0], # y = y0 +dy*dt
[0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 1]]) # ddy = -g.
f1.H = np.mat([
[1, 0, 0, 0, 0],
[0, 0, 1, 0, 0]])
f1.R *= r
f1.Q *= q
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x,vx,y,vy,-9.8]).T
return f1
def test_kf():
dt = 0.1
t = 0
f1 = ball_kf (0,1, 35, 50, 0.1)
f2 = ball_kf_noay (0,1, 35, 50, 0.1)
path = BallPath( 0, 1, 35, 50, noise=(0,0))
path_rk = BallRungeKutta(0, 1, 50, 35)
xs = []
ys = []
while f1.x[2,0]>= 0:
t += dt
f1.predict()
f2.predict()
#x,y = path.pos_at_t(t)
x,y = path_rk.step(dt)
xs.append(x)
ys.append(y)
plt.scatter (f1.x[0,0], f1.x[2,0], color='blue',alpha=0.6)
plt.scatter (f2.x[0,0], f2.x[2,0], color='red', alpha=0.6)
plt.plot(xs, ys, c='g')
class BaseballPath(object):
def __init__(self, x0, y0, launch_angle_deg, velocity_ms, noise=(1.0,1.0)):
""" Create baseball path object in 2D (y=height above ground)
x0,y0 initial position
launch_angle_deg angle ball is travelling respective to ground plane
velocity_ms speeed of ball in meters/second
noise amount of noise to add to each reported position in (x,y)
"""
omega = radians(launch_angle_deg)
self.v_x = velocity_ms * cos(omega)
self.v_y = velocity_ms * sin(omega)
self.x = x0
self.y = y0
self.noise = noise
def drag_force (self, velocity):
""" Returns the force on a baseball due to air drag at
the specified velocity. Units are SI
"""
B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.))
return B_m * velocity
def update(self, dt, vel_wind=0.):
""" compute the ball position based on the specified time step and
wind velocity. Returns (x,y) position tuple.
"""
# Euler equations for x and y
self.x += self.v_x*dt
self.y += self.v_y*dt
# force due to air drag
v_x_wind = self.v_x - vel_wind
v = sqrt (v_x_wind**2 + self.v_y**2)
F = self.drag_force(v)
# Euler's equations for velocity
self.v_x = self.v_x - F*v_x_wind*dt
self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt
return (self.x + random.randn()*self.noise[0],
self.y + random.randn()*self.noise[1])
def test_baseball_path():
ball = BaseballPath (0, 1, 35, 50)
while ball.y > 0:
ball.update (0.1, 0.)
plt.scatter (ball.x, ball.y)
def test_ball_path():
y = 15
x = 0
omega = 0.
noise = [1,1]
v0 = 100.
ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
dt = 1
f1 = KalmanFilter(dim_x=6, dim_z=2)
dt = 1/30. # time step
ay = -.5*dt**2
f1.F = np.mat ([[1, dt, 0, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 0, 0, 0, 0], # ddx = 0
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 0, 1]]) # ddy = -g
f1.H = np.mat([
[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]])
f1.R = np.eye(2) * 5
f1.Q = np.eye(6) * 0.
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x,vx,0,y,vy,-9.8]).T
f1.P = np.eye(6) * 500.
z = np.mat([[0,0]]).T
count = 0
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
while f1.x[3,0] > 0:
count += 1
#f1.update (z)
f1.predict()
plt.scatter(f1.x[0,0],f1.x[3,0], color='green')
def drag_force (velocity):
""" Returns the force on a baseball due to air drag at
the specified velocity. Units are SI
"""
B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.))
return B_m * velocity
def update_drag(f, dt):
vx = f.x[1,0]
vy = f.x[3,0]
v = sqrt(vx**2 + vy**2)
F = -drag_force(v)
print F
f.u[0,0] = -drag_force(vx)
f.u[1,0] = -drag_force(vy)
#f.x[2,0]=F*vx
#f.x[5,0]=F*vy
def test_kf_drag():
y = 1
x = 0
omega = 35.
noise = [0,0]
v0 = 50.
ball = BaseballPath (x0=x, y0=y,
launch_angle_deg=omega,
velocity_ms=v0, noise=noise)
#ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
dt = 1
f1 = KalmanFilter(dim_x=6, dim_z=2)
dt = 1/30. # time step
ay = -.5*dt**2
ax = .5*dt**2
f1.F = np.mat ([[1, dt, ax, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 1, 0, 0, 0], # ddx = 0
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 0, 1]]) # ddy = -g
# We will inject air drag using Bu
f1.B = np.mat([[0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 1.]]).T
f1.u = np.mat([[0., 0.]]).T
f1.H = np.mat([
[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]])
f1.R = np.eye(2) * 5
f1.Q = np.eye(6) * 0.
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x,vx,0,y,vy,-9.8]).T
f1.P = np.eye(6) * 500.
z = np.mat([[0,0]]).T
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
t=0
while f1.x[3,0] > 0:
t+=dt
#f1.update (z)
x,y = ball.update(dt)
#x,y = ball.pos_at_t(t)
update_drag(f1, dt)
f1.predict()
print f1.x.T
plt.scatter(f1.x[0,0],f1.x[3,0], color='red', alpha=0.5)
plt.scatter (x,y)
return f1
if __name__ == '__main__':
#test_baseball_path()
#test_ball_path()
#test_kf()
f1 = test_kf_drag()
experiments/balzer.py
+38
View File
@@ -0,0 +1,38 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jan 24 15:20:47 2015
@author: rlabbe
"""
import numpy as np
import matplotlib.pyplot as plt
try:
data
except:
cols = ('time','millis','ax','ay','az','rollrate','pitchrate','yawrate',
'roll','pitch','yaw','speed','course','latitude','longitude',
'altitude','pdop','hdop','vdop','epe')
data = np.genfromtxt('2014-03-26-000-Data.csv', delimiter=',',
names=True, usecols=cols).view(np.recarray)
plt.subplot(311)
plt.plot(data.ax)
plt.subplot(312)
plt.plot(data.speed)
plt.subplot(313)
plt.plot(data.course)
plt.tight_layout()
plt.figure()
plt.subplot(311)
plt.plot(data.pitch)
plt.subplot(312)
plt.plot(data.roll)
plt.subplot(313)
plt.plot(data.yaw)
plt.figure()
plt.plot(data.longitude, data.latitude)
experiments/baseball.py
+192
View File
@@ -0,0 +1,192 @@
# -*- coding: utf-8 -*-
"""
Computes the trajectory of a stitched baseball with air drag.
Takes altitude into account (higher altitude means further travel) and the
stitching on the baseball influencing drag.
This is based on the book Computational Physics by N. Giordano.
The takeaway point is that the drag coefficient on a stitched baseball is
LOWER the higher its velocity, which is somewhat counterintuitive.
"""
from __future__ import division
import math
import matplotlib.pyplot as plt
from numpy.random import randn
import numpy as np
class BaseballPath(object):
def __init__(self, x0, y0, launch_angle_deg, velocity_ms, noise=(1.0,1.0)):
""" Create baseball path object in 2D (y=height above ground)
x0,y0 initial position
launch_angle_deg angle ball is travelling respective to ground plane
velocity_ms speeed of ball in meters/second
noise amount of noise to add to each reported position in (x,y)
"""
omega = radians(launch_angle_deg)
self.v_x = velocity_ms * cos(omega)
self.v_y = velocity_ms * sin(omega)
self.x = x0
self.y = y0
self.noise = noise
def drag_force (self, velocity):
""" Returns the force on a baseball due to air drag at
the specified velocity. Units are SI
"""
B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.))
return B_m * velocity
def update(self, dt, vel_wind=0.):
""" compute the ball position based on the specified time step and
wind velocity. Returns (x,y) position tuple.
"""
# Euler equations for x and y
self.x += self.v_x*dt
self.y += self.v_y*dt
# force due to air drag
v_x_wind = self.v_x - vel_wind
v = sqrt (v_x_wind**2 + self.v_y**2)
F = self.drag_force(v)
# Euler's equations for velocity
self.v_x = self.v_x - F*v_x_wind*dt
self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt
return (self.x + random.randn()*self.noise[0],
self.y + random.randn()*self.noise[1])
def a_drag (vel, altitude):
""" returns the drag coefficient of a baseball at a given velocity (m/s)
and altitude (m).
"""
v_d = 35
delta = 5
y_0 = 1.0e4
val = 0.0039 + 0.0058 / (1 + math.exp((vel - v_d)/delta))
val *= math.exp(-altitude / y_0)
return val
def compute_trajectory_vacuum (v_0_mph,
theta,
dt=0.01,
noise_scale=0.0,
x0=0., y0 = 0.):
theta = math.radians(theta)
x = x0
y = y0
v0_y = v_0_mph * math.sin(theta)
v0_x = v_0_mph * math.cos(theta)
xs = []
ys = []
t = dt
while y >= 0:
x = v0_x*t + x0
y = -0.5*9.8*t**2 + v0_y*t + y0
xs.append (x + randn() * noise_scale)
ys.append (y + randn() * noise_scale)
t += dt
return (xs, ys)
def compute_trajectory(v_0_mph, theta, v_wind_mph=0, alt_ft = 0.0, dt = 0.01):
g = 9.8
### comput
theta = math.radians(theta)
# initial velocity in direction of travel
v_0 = v_0_mph * 0.447 # mph to m/s
# velocity components in x and y
v_x = v_0 * math.cos(theta)
v_y = v_0 * math.sin(theta)
v_wind = v_wind_mph * 0.447 # mph to m/s
altitude = alt_ft / 3.28 # to m/s
ground_level = altitude
x = [0.0]
y = [altitude]
i = 0
while y[i] >= ground_level:
v = math.sqrt((v_x - v_wind) **2+ v_y**2)
x.append(x[i] + v_x * dt)
y.append(y[i] + v_y * dt)
v_x = v_x - a_drag(v, altitude) * v * (v_x - v_wind) * dt
v_y = v_y - a_drag(v, altitude) * v * v_y * dt - g * dt
i += 1
# meters to yards
np.multiply (x, 1.09361)
np.multiply (y, 1.09361)
return (x,y)
def predict (x0, y0, x1, y1, dt, time):
g = 10.724*dt*dt
v_x = x1-x0
v_y = y1-y0
v = math.sqrt(v_x**2 + v_y**2)
x = x1
y = y1
while time > 0:
v_x = v_x - a_drag(v, y) * v * v_x
v_y = v_y - a_drag(v, y) * v * v_y - g
x = x + v_x
y = y + v_y
time -= dt
return (x,y)
if __name__ == "__main__":
x,y = compute_trajectory(v_0_mph = 110., theta=35., v_wind_mph = 0., alt_ft=5000.)
plt.plot (x, y)
plt.show()
experiments/bb_test.py
+219
View File
@@ -0,0 +1,219 @@
# -*- coding: utf-8 -*-
"""
Created on Wed Jun 4 12:33:38 2014
@author: rlabbe
"""
from __future__ import print_function,division
from filterpy.kalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import baseball
from numpy.random import randn
def ball_filter6(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=6)
g = 10
f1.F = np.mat ([[1., dt, dt**2, 0, 0, 0],
[0, 1., dt, 0, 0, 0],
[0, 0, 1., 0, 0, 0],
[0, 0, 0., 1., dt, -0.5*dt*dt*g],
[0, 0, 0, 0, 1., -g*dt],
[0, 0, 0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,1,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0]])
f1.R = np.mat(np.eye(6)) * R
f1.Q = np.zeros((6,6))
f1.Q[2,2] = Q
f1.Q[5,5] = Q
f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T
f1.P = np.eye(6) * 50.
f1.B = 0.
f1.u = 0
return f1
def ball_filter4(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=4)
g = 10
f1.F = np.mat ([[1., dt, 0, 0,],
[0, 1., 0, 0],
[0, 0, 1., dt],
[0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0],
[0,0,0,0],
[0,0,1,0],
[0,0,0,0]])
f1.B = np.mat([[0,0,0,0],
[0,0,0,0],
[0,0,1.,0],
[0,0,0,1.]])
f1.u = np.mat([[0],
[0],
[-0.5*g*dt**2],
[-g*dt]])
f1.R = np.mat(np.eye(4)) * R
f1.Q = np.zeros((4,4))
f1.Q[1,1] = Q
f1.Q[3,3] = Q
f1.x = np.mat([0, 0 , 0, 0]).T
f1.P = np.eye(4) * 50.
return f1
def plot_ball_filter6 (f1, zs, skip_start=-1, skip_end=-1):
xs, ys = [],[]
pxs, pys = [],[]
for i,z in enumerate(zs):
m = np.mat([z[0], 0, 0, z[1], 0, 0]).T
f1.predict ()
if i == skip_start:
x2 = xs[-2]
x1 = xs[-1]
y2 = ys[-2]
y1 = ys[-1]
if i >= skip_start and i <= skip_end:
#x,y = baseball.predict (x2, y2, x1, y1, 1/30, 1/30* (i-skip_start+1))
x,y = baseball.predict (xs[-2], ys[-2], xs[-1], ys[-1], 1/30, 1/30)
m[0] = x
m[3] = y
#print ('skip', i, f1.x[2],f1.x[5])
f1.update(m)
'''
if i >= skip_start and i <= skip_end:
#f1.x[2] = -0.1
#f1.x[5] = -17.
pass
else:
f1.update (m)
'''
xs.append (f1.x[0,0])
ys.append (f1.x[3,0])
pxs.append (z[0])
pys.append(z[1])
if i > 0 and z[1] < 0:
break;
p1, = plt.plot (xs, ys, 'r--')
p2, = plt.plot (pxs, pys)
plt.axis('equal')
plt.legend([p1,p2], ['filter', 'measurement'], 2)
plt.xlim([0,xs[-1]])
plt.show()
def plot_ball_filter4 (f1, zs, skip_start=-1, skip_end=-1):
xs, ys = [],[]
pxs, pys = [],[]
for i,z in enumerate(zs):
m = np.mat([z[0], 0, z[1], 0]).T
f1.predict ()
if i == skip_start:
x2 = xs[-2]
x1 = xs[-1]
y2 = ys[-2]
y1 = ys[-1]
if i >= skip_start and i <= skip_end:
#x,y = baseball.predict (x2, y2, x1, y1, 1/30, 1/30* (i-skip_start+1))
x,y = baseball.predict (xs[-2], ys[-2], xs[-1], ys[-1], 1/30, 1/30)
m[0] = x
m[2] = y
f1.update (m)
'''
if i >= skip_start and i <= skip_end:
#f1.x[2] = -0.1
#f1.x[5] = -17.
pass
else:
f1.update (m)
'''
xs.append (f1.x[0,0])
ys.append (f1.x[2,0])
pxs.append (z[0])
pys.append(z[1])
if i > 0 and z[1] < 0:
break;
p1, = plt.plot (xs, ys, 'r--')
p2, = plt.plot (pxs, pys)
plt.axis('equal')
plt.legend([p1,p2], ['filter', 'measurement'], 2)
plt.xlim([0,xs[-1]])
plt.show()
start_skip = 20
end_skip = 60
def run_6():
dt = 1/30
noise = 0.0
f1 = ball_filter6(dt, R=.16, Q=0.1)
plt.cla()
x,y = baseball.compute_trajectory(v_0_mph = 100., theta=50., dt=dt)
znoise = [(i+randn()*noise,j+randn()*noise) for (i,j) in zip(x,y)]
plot_ball_filter6 (f1, znoise, start_skip, end_skip)
def run_4():
dt = 1/30
noise = 0.0
f1 = ball_filter4(dt, R=.16, Q=0.1)
plt.cla()
x,y = baseball.compute_trajectory(v_0_mph = 100., theta=50., dt=dt)
znoise = [(i+randn()*noise,j+randn()*noise) for (i,j) in zip(x,y)]
plot_ball_filter4 (f1, znoise, start_skip, end_skip)
if __name__ == "__main__":
run_4()
experiments/compute_q.ipynb
+54
View File
@@ -0,0 +1,54 @@
{
"metadata": {
"name": "",
"signature": "sha256:72ca7769e63a40b2f7f89ae8f816dcd0df3658bb19519d8c95b2f6a5e99413ec"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"from sympy import Symbol, Matrix\n",
"from sympy.interactive import printing\n",
"printing.init_printing()\n",
"\n",
"dt = Symbol('\\Delta t')\n",
"\n",
"Q = Matrix([[.5*dt**2], [.5*dt**2], [dt], [dt]])\n",
"Q*Q.T\n",
"\n",
"\n",
"Q = Matrix([[.5*dt**2], [dt]])\n",
"Q*Q.T"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}0.25 \\Delta t^{4} & 0.5 \\Delta t^{3}\\\\0.5 \\Delta t^{3} & \\Delta t^{2}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"png": "iVBORw0KGgoAAAANSUhEUgAAAKQAAAAzBAMAAADr+L6JAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMA74lUMhCZds3dIma7\nq0Ru0ZIZAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAEHUlEQVRYCe2XTWgTQRTH/0m6m68mFqwgHszi\nJSKURhQRFJJqLd6ailg8JRQtlVYbC34giEHBXks9KEXpnnoQoelFDx7MQREP4oKfqGBuHoq0fkSl\nfqxvZndnd22329KexDnsvJn572/2vZ3sewH8W6O/hCvSLXnep/RPfndkDaWfTK7IVbTqH7G2fZ+P\nVmpdIlKJz+Jg+1Y0ewEr1kK4sCgybukAuUx2G0ceSh/h8xMD94HhfEeNRg3f+BRd9rqRUs/pHFuK\nnGo4wfrOErvy1pWjzkBewmGVBpKG1hpSOocldP5stKC4kZGMNEZiBHSdw97V2YjNdQ8ziyPjTQiW\naZDII9CEiy9VsvG+VWFdFnHVjZwCNrOVSMs51iWH+OsNzZItV+nCkY0aQmyrwAgSX6CRRU0r8N37\n0A038jHtwxxIcBkSks5GPIwNn8niyDUakmwQqdvIZCXIdx/Fjt7BfuNu4/oLmMmQaSKnwd2JKujK\nxX7QPEcWikiaLyNax5s77F0lIKXySL79+RKYZI9hNekrIUs0Stzlr0lDF7nz4cnjUlSNr6N5jpxU\n0GCe9xkFDzBZA6aBCwrAQhR/8pSUVouRslOhUWM+9ptCWUGY3ZulM5JuUcn6G3md5tA4QqGk0NLu\nPERsTjSBpJlHhvvkDvqEwHBcsRyXq2xFnkOoAoTJRQrRX02iZ+KO0/ygil3UXSgCo0LGkWuKCLHX\nA5ynbcsI/jZiT7sXanzeeaGNshmaeAV2iskb5o5EMTAbR0ariPMTEyriZqAMuY5tbJ12n4FkKkVH\nsblHnmI9PWU+xODkTnIWbI41jgw3mTHrAM7J5G3ZOJyBOTrU2w2hfT1pBq4IXDNPUkoNlkM5U8KR\nuEJnSvqO2Oim4yOxIqbU0OVease+4rhUsmGGFc1IzSiUMYH4Buxkut7xoqx1WzoDeeDMM+AGIrqu\nj2Ci5SiiZLGWv3XGUopeSp/PIaoh1vMij3Eu0+uxh2JrAynkq2H8R65GFA3GPxLL4NBz/5gEB47a\nIl/H+zFgq72sLewbYjVfZA/uWVrvfgydqlj1Qjryvf1xFXeZhhBt5InAnLWQ3vmevhFezVEUZK1P\nm/lxo1s88710VnwP5pHtogBX7UXrKRfJ956vx1EUBIvzkYvk+86MLXdZjqLgtmPBekqvfE+RnKw4\n9E7TLgqSSjInVkykd74fw5QdeXEbN+yiYP+23bbIRIrkPC/f7xl67QaJkaMoSOm6mDbSGSCQtLJg\nvrfvEBaridxFgbFkOe6X7wXINuYXBS4k/PK9TRKWRpa7KHAj/fK9AAljgaLAjfTL94IkDKO8dBUF\nbqRfvhckYSxQFLiRfvlekISxQFHgRgrlyg3rB7lykiD8R4pQrNhoW8Kf52Vtwv48L+Ev/nKY9Bf/\nD+iXYj8NWuVEAAAAAElFTkSuQmCC\n",
"prompt_number": 8,
"text": [
"\u23a1 4 3\u23a4\n",
"\u23a20.25\u22c5\\Delta t 0.5\u22c5\\Delta t \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 3 2 \u23a5\n",
"\u23a30.5\u22c5\\Delta t \\Delta t \u23a6"
]
}
],
"prompt_number": 8
}
],
"metadata": {}
}
]
}
experiments/distributions.py
+42
View File
@@ -0,0 +1,42 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jan 17 20:17:14 2015
@author: rlabbe
"""
from scipy.stats import t, norm
import matplotlib.pyplot as plt
import numpy as np
import math
import random
df =4
mu = 10
std = 2
x = np.linspace(-5, 20, 100)
plt.plot(x, t.pdf(x, df=df, loc=mu, scale=std), 'r-', lw=5, label='t pdf')
x2 = np.linspace(mu-10, mu+10, 100)
plt.plot(x, norm.pdf(x, mu, std), 'b-', lw=5, label='gaussian pdf')
plt.legend()
plt.figure()
def student_t(df, mu, std): # nu equals number of degrees of freedom
x = random.gauss(0, std)
y = 2.0*random.gammavariate(0.5*df, 2.0)
return x / (math.sqrt(y/df)) + mu
N = 100000
ys = [student_t(2.7, 100, 2) for i in range(N)]
plt.hist(ys, 10000, histtype='step')
ys = [random.gauss(100,2) for i in range(N)]
plt.hist(ys, 10000, histtype='step',color='r')
plt.show()
experiments/dme.py
+108
View File
@@ -0,0 +1,108 @@
# -*- coding: utf-8 -*-
"""
Spyder Editor
This is a temporary script file.
"""
from KalmanFilter import *
from math import cos, sin, sqrt, atan2
def H_of (pos, pos_A, pos_B):
""" Given the position of our object at 'pos' in 2D, and two transmitters
A and B at positions 'pos_A' and 'pos_B', return the partial derivative
of H
"""
theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0])
theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0])
if False:
return np.mat([[0, -cos(theta_a), 0, -sin(theta_a)],
[0, -cos(theta_b), 0, -sin(theta_b)]])
else:
return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0],
[-cos(theta_b), 0, -sin(theta_b), 0]])
class DMESensor(object):
def __init__(self, pos_a, pos_b, noise_factor=1.0):
self.A = pos_a
self.B = pos_b
self.noise_factor = noise_factor
def range_of (self, pos):
""" returns tuple containing noisy range data to A and B
given a position 'pos'
"""
ra = sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
rb = sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)
return (ra + random.randn()*self.noise_factor,
rb + random.randn()*self.noise_factor)
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [],[]
pxs, pys = [],[]
# create the simulated sensor
d = DMESensor (pos_a, pos_b, noise_factor=1.)
# pos will contain our nominal position since the filter does not
# maintain position.
pos = [0,0]
for i in range(count):
# move (1,1) each step, so just use i
pos = [i,i]
# compute the difference in range between the nominal track and measured
# ranges
ra,rb = d.range_of(pos)
rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0]))
print ra, rb
print rx,ry
z = np.mat([[ra-rx],[rb-ry]])
print z.T
# compute linearized H for this time step
f1.H = H_of (pos, pos_a, pos_b)
# store stuff so we can plot it later
xs.append (f1.x[0,0]+i)
ys.append (f1.x[2,0]+i)
pxs.append (pos[0])
pys.append(pos[1])
# perform the Kalman filter steps
f1.predict ()
f1.update(z)
p1, = plt.plot (xs, ys, 'r--')
p2, = plt.plot (pxs, pys)
plt.legend([p1,p2], ['filter', 'ideal'], 2)
plt.show()
experiments/dog_track_1d.py
+36
View File
@@ -0,0 +1,36 @@
# -*- coding: utf-8 -*-
"""
Created on Wed Apr 30 10:35:19 2014
@author: rlabbe
"""
import numpy.random as random
import math
class dog_sensor(object):
def __init__(self, x0 = 0, motion=1, noise=0.0):
self.x = x0
self.motion = motion
self.noise = math.sqrt(noise)
def sense(self):
self.x = self.x + self.motion
self.x += random.randn() * self.noise
return self.x
def measure_dog ():
if not hasattr(measure_dog, "x"):
measure_dog.x = 0
measure_dog.motion = 1
if __name__ == '__main__':
dog = dog_sensor(noise = 1)
for i in range(10):
print (dog.sense())
experiments/doppler.py
+140
View File
@@ -0,0 +1,140 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jan 17 15:04:08 2015
@author: rlabbe
"""
from numpy.random import randn
import numpy as np
from numpy import array
import matplotlib.pyplot as plt
from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import KalmanFilter
from math import sqrt
import math
import random
pos_var = 1.
dop_var = 2.
dt = 1/20
def rand_student_t(df, mu=0, std=1):
"""return random number distributed by student's t distribution with
`df` degrees of freedom with the specified mean and standard deviation.
"""
x = random.gauss(0, std)
y = 2.0*random.gammavariate(0.5*df, 2.0)
return x / (math.sqrt(y/df)) + mu
np.random.seed(124)
class ConstantVelocityObject(object):
def __init__(self, x0, vel, noise_scale):
self.x = np.array([x0, vel])
self.noise_scale = noise_scale
self.vel = vel
def update(self, dt):
pnoise = abs(randn()*self.noise_scale)
if self.x[1] > self.vel:
pnoise = -pnoise
self.x[1] += pnoise
self.x[0] += self.x[1]*dt
return self.x.copy()
def sense_pos(self):
return self.x[0] + [randn()*self.noise_scale[0]]
def vel(self):
return self.x[1] + [randn()*self.noise_scale[1]]
def sense(x, noise_scale=(1., 0.01)):
return x + [randn()*noise_scale[0], randn()*noise_scale[1]]
def sense_t(x, noise_scale=(1., 0.01)):
return x + [rand_student_t(2)*noise_scale[0],
rand_student_t(2)*noise_scale[1]]
R2 = np.zeros((2, 2))
R1 = np.zeros((1, 1))
R2[0, 0] = pos_var
R2[1, 1] = dop_var
R1[0,0] = dop_var
H2 = np.array([[1., 0.], [0., 1.]])
H1 = np.array([[0., 1.]])
kf = KalmanFilter(2, 1)
kf.F = array([[1, dt],
[0, 1]])
kf.H = array([[1, 0],
[0, 1]])
kf.Q = Q_discrete_white_noise(2, dt, .02)
kf.x = array([0., 0.])
kf.R = R1
kf.H = H1
kf.P *= 10
random.seed(1234)
track = []
zs = []
ests = []
sensor_noise = (sqrt(pos_var), sqrt(dop_var))
obj = ConstantVelocityObject(0., 1., noise_scale=.18)
for i in range(30000):
x = obj.update(dt/10)
z = sense_t(x, sensor_noise)
if i % 10 != 0:
continue
if i % 60 == 87687658760:
kf.predict()
kf.update(z, H=H2, R=R2)
else:
kf.predict()
kf.update(z[1], H=H1, R=R1)
ests.append(kf.x)
track.append(x)
zs.append(z)
track = np.asarray(track)
zs = np.asarray(zs)
ests = np.asarray(ests)
plt.figure()
plt.subplot(211)
plt.plot(track[:,0], label='track')
plt.plot(zs[:,0], label='measurements')
plt.plot(ests[:,0], label='filter')
plt.legend(loc='best')
plt.subplot(212)
plt.plot(track[:,1], label='track')
plt.plot(zs[:,1], label='measurements')
plt.plot(ests[:,1], label='filter')
plt.show()
experiments/fusion.py
+66
View File
@@ -0,0 +1,66 @@
# -*- coding: utf-8 -*-
"""
Created on Fri Feb 13 17:47:56 2015
@author: rlabbe
"""
import numpy as np
from filterpy.kalman import UnscentedKalmanFilter as UKF
from math import atan2, radians,degrees
from filterpy.common import stats
import matplotlib.pyplot as plt
p = (-10, -10)
def hx(x):
dx = x[0] - hx.p[0]
dy = x[1] - hx.p[1]
return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5])
def fx(x,dt):
return x
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.)
kf.x = np.array([100, 100.])
kf.P *= 40
hx.p = kf.x - np.array([50,50])
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
stats.plot_covariance_ellipse(
kf.x, cov=kf.P, axis_equal=True,
facecolor='y', edgecolor=None, alpha=0.6)
plt.scatter([100], [100], c='y', label='Initial')
kf.R[0,0] = radians (1)**2
kf.R[1,1] = 2.**2
kf.predict()
kf.update(np.array([radians(45), d]))
print(kf.x)
print(kf.P)
stats.plot_covariance_ellipse(
kf.x, cov=kf.P, axis_equal=True,
facecolor='g', edgecolor=None, alpha=0.6)
plt.scatter([100], [100], c='g', label='45 degrees')
p = (13, -11)
hx.p = kf.x - np.array([-50,50])
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
kf.predict()
kf.update(np.array([radians(135), d]))
stats.plot_covariance_ellipse(
kf.x, cov=kf.P, axis_equal=True,
facecolor='b', edgecolor=None, alpha=0.6)
plt.scatter([100], [100], c='b', label='135 degrees')
plt.legend(scatterpoints=1, markerscale=3)
experiments/gauss.py
+78
View File
@@ -0,0 +1,78 @@
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 22 11:38:49 2014
@author: rlabbe
"""
from __future__ import division, print_function
import math
import matplotlib.pyplot as plt
import numpy.random as random
class gaussian(object):
def __init__(self, mean, variance):
try:
self.mean = float(mean)
self.variance = float(variance)
except:
self.mean = mean
self.variance = variance
def __add__ (a, b):
return gaussian (a.mean + b.mean, a.variance + b.variance)
def __mul__ (a, b):
m = (a.variance*b.mean + b.variance*a.mean) / (a.variance + b.variance)
v = 1. / (1./a.variance + 1./b.variance)
return gaussian (m, v)
def __call__(self, x):
""" Impl
"""
return math.exp (-0.5 * (x-self.mean)**2 / self.variance) / \
math.sqrt(2.*math.pi*self.variance)
def __str__(self):
return "(%f, %f)" %(self.mean, self.sigma)
def stddev(self):
return math.sqrt (self.variance)
def as_tuple(self):
return (self.mean, self.variance)
def __tuple__(self):
return (self.mean, self.variance)
def __getitem__ (self,index):
""" maybe silly, allows you to access obect as a tuple:
a = gaussian(3,4)
print (tuple(a))
"""
if index == 0: return self.mean
elif index == 1: return self.variance
else: raise StopIteration
class KF1D(object):
def __init__ (self, pos, sigma):
self.estimate = gaussian(pos,sigma)
def update(self, Z,var):
self.estimate = self.estimate * gaussian (Z,var)
def predict(self, U, var):
self.estimate = self.estimate + gaussian (U,var)
def mul2 (a, b):
m = (a['variance']*b['mean'] + b['variance']*a['mean']) / (a['variance'] + b['variance'])
v = 1. / (1./a['variance'] + 1./b['variance'])
return gaussian (m, v)
#varying_error_kf( noise_factor=100)
experiments/gh.py
+136
View File
@@ -0,0 +1,136 @@
# -*- coding: utf-8 -*-
"""
Created on Thu May 15 16:07:26 2014
@author: RL
"""
from __future__ import division
import matplotlib.pyplot as plt
import numpy.random as random
def g_h_filter (data, x, dx, g, h, dt=1.):
results = []
for z in data:
x_est = x + (dx*dt)
residual = z - x_est
dx = dx + h * (residual / float(dt))
x = x_est + g * residual
print('gx',dx,)
results.append(x)
return results
'''
computation of x
x_est = weight + gain
residual = z - weight - gain
x = weight + gain + g (z - weight - gain)
w + gain + gz -wg -ggain
w -wg + gain - ggain + gz
w(1-g) + gain(1-g) +gz
(w+g)(1-g) +gz
'''
'''
gain computation
gain = gain + h/t* (z - weight - gain)
= gain + hz/t -hweight/t - hgain/t
= gain(1-h/t) + h/t(z-weight)
'''
'''
gain+ h*(z-w -gain*t)/t
gain + hz/t -hw/t -hgain
gain*(1-h) + h/t(z-w)
'''
def weight2():
w = 0
gain = 200
t = 10.
weight_scale = 1./6
gain_scale = 1./10
weights=[2060]
for i in range (len(weights)):
z = weights[i]
w_pre = w + gain*t
new_w = w_pre * (1-weight_scale) + z * (weight_scale)
print('new_w',new_w)
gain = gain *(1-gain_scale) + (z - w) * gain_scale/t
print (z)
print(w)
#gain = new_gain * (gain_scale) + gain * (1-gain_scale)
w = new_w
print ('w',w,)
print ('gain=',gain)
def weight3():
w = 160.
gain = 1.
t = 1.
weight_scale = 6/10.
gain_scale = 2./3
weights=[158]
for i in range (len(weights)):
z = weights[i]
w_pre = w + gain*t
new_w = w_pre * (1-weight_scale) + z * (weight_scale)
print('new_w',new_w)
gain = gain *(1-gain_scale) + (z - w) * gain_scale/t
print (z)
print(w)
#gain = new_gain * (gain_scale) + gain * (1-gain_scale)
w = new_w
print ('w',w,)
print ('gain=',gain)
weight3()
'''
#zs = [i + random.randn()*50 for i in range(200)]
zs = [158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 169.6, 167.4, 166.4, 171.0]
#zs = [2060]
data= g_h_filter(zs, 160, 1, .6, 0, 1.)
'''
data = g_h_filter([2060], x=0, dx=200, g=1./6, h = 1./10, dt=10)
print data
'''
print data
print data2
plt.plot(data)
plt.plot(zs, 'g')
plt.show()
'''
experiments/histogram.py
+57
View File
@@ -0,0 +1,57 @@
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 22 10:43:38 2014
@author: rlabbe
"""
p = [.2, .2, .2, .2, .2]
world = ['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pOvershoot = 0.1
pUndershoot = 0.1
pExact = 0.8
def normalize (p):
s = sum(p)
for i in range (len(p)):
p[i] = p[i] / s
def sense(p, Z):
q= []
for i in range (len(p)):
hit = (world[i] ==Z)
q.append(p[i] * (pHit*hit + pMiss*(1-hit)))
normalize(q)
return q
def move(p, U):
q = []
for i in range(len(p)):
pexact = p[(i-U) % len(p)] * pExact
pover = p[(i-U-1) % len(p)] * pOvershoot
punder = p[(i-U+1) % len(p)] * pUndershoot
q.append (pexact + pover + punder)
normalize(q)
return q
if __name__ == "__main__":
p = sense(p, 'red')
print p
pause()
for z in measurements:
p = sense (p, z)
p = move (p, 1)
print p
experiments/image_tracker.py
+47
View File
@@ -0,0 +1,47 @@
# -*- coding: utf-8 -*-
"""
Created on Sat May 24 14:42:55 2014
@author: rlabbe
"""
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random
f = KalmanFilter (dim=4)
dt = 1
f.F = np.mat ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
f.H = np.mat ([[1, 0, 0, 0],
[0, 0, 1, 0]])
f.Q *= 4.
f.R = np.mat([[50,0],
[0, 50]])
f.x = np.mat([0,0,0,0]).T
f.P *= 100.
xs = []
ys = []
count = 200
for i in range(count):
z = np.mat([[i+random.randn()*1],[i+random.randn()*1]])
f.predict ()
f.update (z)
xs.append (f.x[0,0])
ys.append (f.x[2,0])
plt.plot (xs, ys)
plt.show()
experiments/mkf_ellipse_test.py
+57
View File
@@ -0,0 +1,57 @@
# -*- coding: utf-8 -*-
"""
Created on Sun May 11 20:47:52 2014
@author: rlabbe
"""
from DogSensor import DogSensor
from filterpy.kalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import stats
def dog_tracking_filter(R,Q=0,cov=1.):
f = KalmanFilter (dim_x=2, dim_z=1)
f.x = np.matrix([[0], [0]]) # initial state (location and velocity)
f.F = np.matrix([[1,1],[0,1]]) # state transition matrix
f.H = np.matrix([[1,0]]) # Measurement function
f.R = R # measurement uncertainty
f.P *= cov # covariance matrix
f.Q = Q
return f
def plot_track(noise, count, R, Q=0, plot_P=True, title='Kalman Filter'):
dog = DogSensor(velocity=1, noise=noise)
f = dog_tracking_filter(R=R, Q=Q, cov=10.)
ps = []
zs = []
cov = []
for t in range (count):
z = dog.sense()
f.update (z)
#print (t,z)
ps.append (f.x[0,0])
cov.append(f.P)
zs.append(z)
f.predict()
p0, = plt.plot([0,count],[0,count],'g')
p1, = plt.plot(range(1,count+1),zs,c='r', linestyle='dashed')
p2, = plt.plot(range(1,count+1),ps, c='b')
plt.axis('equal')
plt.legend([p0,p1,p2], ['actual','measurement', 'filter'], 2)
plt.title(title)
for i,p in enumerate(cov):
print(i,p)
e = stats.sigma_ellipse (p, i+1, ps[i])
stats.plot_sigma_ellipse(e, axis_equal=False)
plt.xlim((-1,count))
plt.show()
if __name__ == "__main__":
plot_track (noise=30, R=5, Q=2, count=20)
experiments/noise.py
+13
View File
@@ -0,0 +1,13 @@
# -*- coding: utf-8 -*-
"""
Created on Fri May 2 10:28:27 2014
@author: rlabbe
"""
import numpy.random
def white_noise (sigma2=1.):
return sigma2 * numpy.random.randn()
if __name__ == "__main__":
assert white_noise(.0) == 0.
experiments/nonlinear_plots.py
+104
View File
@@ -0,0 +1,104 @@
# -*- coding: utf-8 -*-
"""
Created on Sun May 18 11:09:23 2014
@author: rlabbe
"""
from __future__ import division
import numpy as np
import matplotlib.pyplot as plt
from numpy.random import normal
def plot_transfer_func(data, f, lims,num_bins=1000):
ys = f(data)
#plot output
plt.subplot(2,2,1)
plt.hist(ys, num_bins, orientation='horizontal',histtype='step')
plt.ylim(lims)
plt.gca().xaxis.set_ticklabels([])
# plot transfer function
plt.subplot(2,2,2)
x = np.arange(lims[0], lims[1],0.1)
y = f(x)
plt.plot (x,y)
isct = f(0)
plt.plot([0,0,lims[0]],[lims[0],isct,isct],c='r')
plt.xlim(lims)
# plot input
plt.subplot(2,2,4)
plt.hist(data, num_bins, histtype='step')
plt.xlim(lims)
plt.gca().yaxis.set_ticklabels([])
plt.show()
normals = normal(loc=0.0, scale=1, size=5000000)
#rint h(normals).sort()
def f(x):
return 2*x + 1
def g(x):
return (cos(4*(x/2+0.7)))*sin(0.3*x)-0.9*x
return (cos(4*(x/3+0.7)))*sin(0.3*x)-0.9*x
#return -x+1.2*np.sin(0.7*x)+3
return sin(5-.2*x)
def h(x): return cos(.4*x)*x
plot_transfer_func (normals, g, lims=(-4,4),num_bins=500)
del(normals)
#plt.plot(g(np.arange(-10,10,0.1)))
'''
ys = f(normals)
r = np.linspace (min(normals), max(normals), num_bins)
h= np.histogram(ys, num_bins,density=True)
print h
print len(h[0]), len(h[1][0:-1])
#plot output
plt.subplot(2,2,1)
h = np.histogram(ys, num_bins,normed=True)
p, = plt.plot(h[0],h[1][1:])
plt.ylim((-10,10))
plt.xlim((max(h[0]),0))
# plot transfer function
plt.subplot(2,2,2)
x = np.arange(-10,10)
y = 1.2*x + 1
plt.plot (x,y)
plt.plot([0,0],[-10,f(0)],c='r')
plt.ylim((-10,10))
# plot input
plt.subplot(2,2,4)
h = np.histogram(normals, num_bins,density=True)
plt.plot(h[1][1:],h[0])
plt.xlim((-10,10))
plt.show()
'''
experiments/quaternion.py
+76
View File
@@ -0,0 +1,76 @@
# -*- coding: utf-8 -*-
"""
Created on Thu Jan 29 18:36:36 2015
@author: rlabbe
"""
import numpy as np
from math import sin, cos, atan2, asin
'''
psi = yaw
phi = roll
theta = pitch
'''
def e2q(vector):
roll = vector[0]
pitch = vector[1]
heading = vector[2]
sinhdg = sin(heading/2)
coshdg = cos(heading/2)
sinroll = sin(roll/2)
cosroll = cos(roll/2)
sinpitch = sin(pitch/2)
cospitch = cos(pitch/2)
q0 = cosroll*cospitch*coshdg + sinroll*sinpitch*sinhdg
q1 = sinroll*cospitch*coshdg - cosroll*sinpitch*sinhdg
q2 = cosroll*sinpitch*coshdg + sinroll*cospitch*sinhdg
q3 = cosroll*cospitch*sinhdg - sinroll*sinpitch*coshdg
return np.array([q0, q1, q2, q3])
def q2e(q):
roll = atan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]**2 + q[2]**2))
pitch = asin(2*(q[0]*q[2] - q[3]*q[1]))
hdg = atan2(2*(q[0]*q[3] + q[1]*q[2]), 1-2*(q[2]**2 + q[3]**2))
return np.array([roll, pitch, hdg])
def e2d(e):
return np.degrees(e)
def e2r(e):
return np.radians(e)
def add(q1,q2):
return np.multiply(q1,q2)
def add2(q1, q2):
Q1 = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]
Q2 = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]
Q3 = q1[0]*q2[2] + q1[2]*q2[0] + q1[3]*q2[1] - q1[1]*q2[3]
Q4 = q1[0]*q2[3] + q1[3]*q2[0] + q1[1]*q2[2] - q1[2]*q2[1]
return np.array([Q1, Q2, Q3, Q4])
e = e2r([10, 0, 0])
q = e2q(e)
print(q)
print(e2d(q2e(q)))
q2 = add2(q,q)
print(q2)
e2 = q2e(q2)
print(e2d(e2))
experiments/range_finder.py
+118
View File
@@ -0,0 +1,118 @@
# -*- coding: utf-8 -*-
"""
Created on Sat May 24 19:14:06 2014
@author: rlabbe
"""
from __future__ import division, print_function
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random
import math
class DMESensor(object):
def __init__(self, pos_a, pos_b, noise_factor=1.0):
self.A = pos_a
self.B = pos_b
self.noise_factor = noise_factor
def range_of (self, pos):
""" returns tuple containing noisy range data to A and B
given a position 'pos'
"""
ra = math.sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
rb = math.sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)
return (ra + random.randn()*self.noise_factor,
rb + random.randn()*self.noise_factor)
def dist(a,b):
return math.sqrt ((a[0]-b[0])**2 + (a[1]-b[1])**2)
def H_of (pos, pos_A, pos_B):
from math import sin, cos, atan2
theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0])
theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0])
return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0],
[-cos(theta_b), 0, -sin(theta_b), 0]])
# equivalently we can do this...
#dist_a = dist(pos, pos_A)
#dist_b = dist(pos, pos_B)
#return np.mat([[(pos[0]-pos_A[0])/dist_a, 0, (pos[1]-pos_A[1])/dist_a,0],
# [(pos[0]-pos_B[0])/dist_b, 0, (pos[1]-pos_B[1])/dist_b,0]])
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim=4)
dt = 1.0 # time step
'''
f1.F = np.mat ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
'''
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R = np.eye(2) * 1.
f1.Q = np.eye(4) * .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [],[]
pxs, pys = [],[]
d = DMESensor (pos_a, pos_b, noise_factor=1.)
pos = [0,0]
for i in range(count):
pos = (i,i)
ra,rb = d.range_of(pos)
rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0]))
print ('range =', ra,rb)
z = np.mat([[ra-rx],[rb-ry]])
print('z =', z)
f1.H = H_of (pos, pos_a, pos_b)
print('H =', f1.H)
##f1.update (z)
print (f1.x)
xs.append (f1.x[0,0]+i)
ys.append (f1.x[2,0]+i)
pxs.append (pos[0])
pys.append(pos[1])
f1.predict ()
print (f1.H * f1.x)
print (z)
print (f1.x)
f1.update(z)
print(f1.x)
p1, = plt.plot (xs, ys, 'r--')
p2, = plt.plot (pxs, pys)
plt.legend([p1,p2], ['filter', 'ideal'], 2)
plt.show()
experiments/satellite.ipynb
+100
View File
@@ -0,0 +1,100 @@
{
"metadata": {
"name": "",
"signature": "sha256:9fb5a469b0b0f27f1e721735fae76d74f0ad242621b94c3f528e9392b7deab82"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"import math\n",
"\n",
"def range_to(x1, x2):\n",
" dx = x1[0] - x2[0]\n",
" dy = x1[1] - x2[1]\n",
" print('dx',dx,dy)\n",
" return math.sqrt(dx*dx + dy*dy)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 40
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"s1 = (0., 2.) # satellite 1\n",
"s2 = (1., 2.) # satellite 2\n",
"p = (-0.5, 1.) # platform postion\n",
"\n",
"\n",
"s1 = (0., 200.) # satellite 1\n",
"s2 = (100., 220.) # satellite 2\n",
"p = (10, 1.) # platform postion\n",
"\n",
"r1 = range_to(s1, p)\n",
"r2 = range_to(s2, p)\n",
"\n",
"x1, y1 = s1[0], s1[1]\n",
"x2, y2 = s2[0], s2[1]\n",
"\n",
"A = -(y2 - y1) / (x2 - x1)\n",
"B = (r1**2 - r2**2 - x1**2 - y1**2 + x2**2 + y2**2) / (2*(x2 - x1))\n",
"\n",
"a = 1 + A**2\n",
"b = -2*A*x1 + 2*A*B - 2*y1\n",
"c = x1**2 - 2*x1*B + y1**2 - r1**2\n",
"\n",
"y = (-b - math.sqrt(b**2 - 4*a*c)) / (2*a)\n",
"x = A*y + B\n",
"\n",
"print('p', x,y)\n",
"print('err', x-p[0], y-p[1])"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"dx -10.0 199.0\n",
"dx 90.0 219.0\n",
"p 10.051726583768032 0.7413670811596572\n",
"err 0.05172658376803163 -0.2586329188403428\n"
]
}
],
"prompt_number": 55
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"A"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 56,
"text": [
"-0.2"
]
}
],
"prompt_number": 56
}
],
"metadata": {}
}
]
}
experiments/temp.py
+9
View File
@@ -0,0 +1,9 @@
# -*- coding: utf-8 -*-
"""
Spyder Editor
This is a temporary script file.
"""
import KalmanFilter
experiments/test_stats.py
+88
View File
@@ -0,0 +1,88 @@
# -*- coding: utf-8 -*-
"""
py.test module to test stats.py module.
Created on Wed Aug 27 06:45:06 2014
@author: rlabbe
"""
from __future__ import division
from math import pi, exp
import numpy as np
from stats import gaussian, multivariate_gaussian, _to_cov
from numpy.linalg import inv
from numpy import linalg
def near_equal(x,y):
return abs(x-y) < 1.e-15
def test_gaussian():
import scipy.stats
mean = 3.
var = 1.5
std = var**0.5
for i in np.arange(-5,5,0.1):
p0 = scipy.stats.norm(mean, std).pdf(i)
p1 = gaussian(i, mean, var)
assert near_equal(p0, p1)
def norm_pdf_multivariate(x, mu, sigma):
""" extremely literal transcription of the multivariate equation.
Slow, but easy to verify by eye compared to my version."""
n = len(x)
sigma = _to_cov(sigma,n)
det = linalg.det(sigma)
norm_const = 1.0 / (pow((2*pi), n/2) * pow(det, .5))
x_mu = x - mu
result = exp(-0.5 * (x_mu.dot(inv(sigma)).dot(x_mu.T)))
return norm_const * result
def test_multivariate():
from scipy.stats import multivariate_normal as mvn
from numpy.random import rand
mean = 3
var = 1.5
assert near_equal(mvn(mean,var).pdf(0.5),
multivariate_gaussian(0.5, mean, var))
mean = np.array([2.,17.])
var = np.array([[10., 1.2], [1.2, 4.]])
x = np.array([1,16])
assert near_equal(mvn(mean,var).pdf(x),
multivariate_gaussian(x, mean, var))
for i in range(100):
x = np.array([rand(), rand()])
assert near_equal(mvn(mean,var).pdf(x),
multivariate_gaussian(x, mean, var))
assert near_equal(mvn(mean,var).pdf(x),
norm_pdf_multivariate(x, mean, var))
mean = np.array([1,2,3,4])
var = np.eye(4)*rand()
x = np.array([2,3,4,5])
assert near_equal(mvn(mean,var).pdf(x),
norm_pdf_multivariate(x, mean, var))
experiments/ukf_baseball.py
+214
View File
@@ -0,0 +1,214 @@
# -*- coding: utf-8 -*-
"""
Created on Sun Feb 8 09:55:24 2015
@author: rlabbe
"""
from math import radians, sin, cos, sqrt, exp, atan2, radians
from numpy import array, asarray
from numpy.random import randn
import numpy as np
import math
import matplotlib.pyplot as plt
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.common import runge_kutta4
class BaseballPath(object):
def __init__(self, x0, y0, launch_angle_deg, velocity_ms,
noise=(1.0,1.0)):
""" Create 2D baseball path object
(x = distance from start point in ground plane,
y=height above ground)
x0,y0 initial position
launch_angle_deg angle ball is travelling respective to
ground plane
velocity_ms speeed of ball in meters/second
noise amount of noise to add to each position
in (x,y)
"""
omega = radians(launch_angle_deg)
self.v_x = velocity_ms * cos(omega)
self.v_y = velocity_ms * sin(omega)
self.x = x0
self.y = y0
self.noise = noise
def drag_force(self, velocity):
""" Returns the force on a baseball due to air drag at
the specified velocity. Units are SI
"""
B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.))
return B_m * velocity
def update(self, dt, vel_wind=0.):
""" compute the ball position based on the specified time
step and wind velocity. Returns (x,y) position tuple
"""
# Euler equations for x and y
self.x += self.v_x*dt
self.y += self.v_y*dt
# force due to air drag
v_x_wind = self.v_x - vel_wind
v = sqrt(v_x_wind**2 + self.v_y**2)
F = self.drag_force(v)
# Euler's equations for velocity
self.v_x = self.v_x - F*v_x_wind*dt
self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt
return (self.x, self.y)
radar_pos = (100,0)
omega = 45.
def radar_sense(baseball, noise_rng, noise_brg):
x, y = baseball.x, baseball.y
rx, ry = radar_pos[0], radar_pos[1]
rng = ((x-rx)**2 + (y-ry)**2) ** .5
bearing = atan2(y-ry, x-rx)
rng += randn() * noise_rng
bearing += radians(randn() * noise_brg)
return (rng, bearing)
ball = BaseballPath(x0=0, y0=1, launch_angle_deg=45,
velocity_ms=60, noise=[0,0])
'''
xs = []
ys = []
dt = 0.05
y = 1
while y > 0:
x,y = ball.update(dt)
xs.append(x)
ys.append(y)
plt.plot(xs, ys)
plt.axis('equal')
plt.show()
'''
dt = 1/30.
def hx(x):
global radar_pos
dx = radar_pos[0] - x[0]
dy = radar_pos[1] - x[2]
rng = (dx*dx + dy*dy)**.5
bearing = atan2(-dy, -dx)
#print(x)
#print('hx:', rng, np.degrees(bearing))
return array([rng, bearing])
def fx(x, dt):
fx.ball.x = x[0]
fx.ball.y = x[2]
fx.ball.vx = x[1]
fx.ball.vy = x[3]
N = 10
ball_dt = dt/float(N)
for i in range(N):
fx.ball.update(ball_dt)
#print('fx', fx.ball.x, fx.ball.v_x, fx.ball.y, fx.ball.v_y)
return array([fx.ball.x, fx.ball.v_x, fx.ball.y, fx.ball.v_y])
fx.ball = BaseballPath(x0=0, y0=1, launch_angle_deg=45,
velocity_ms=60, noise=[0,0])
y = 1.
x = 0.
theta = 35. # launch angle
v0 = 50.
ball = BaseballPath(x0=x, y0=y, launch_angle_deg=theta,
velocity_ms=v0, noise=[.3,.3])
kf = UKF(dim_x=4, dim_z=2, dt=dt, hx=hx, fx=fx, kappa=0)
#kf.R *= r
kf.R[0,0] = 0.1
kf.R[1,1] = radians(0.2)
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
kf.x = array([x, vx, y, vy])
kf.R*= 0.01
#kf.R[1,1] = 0.01
kf.P *= 10
f1 = kf
t = 0
xs = []
ys = []
while y > 0:
t += dt
x,y = ball.update(dt)
z = radar_sense(ball, 0, 0)
#print('z', z)
#print('ball', ball.x, ball.v_x, ball.y, ball.v_y)
f1.predict()
f1.update(z)
xs.append(f1.x[0])
ys.append(f1.x[2])
f1.predict()
p1 = plt.scatter(x, y, color='r', marker='o', s=75, alpha=0.5)
p2, = plt.plot (xs, ys, lw=2, marker='o')
#p3, = plt.plot (xs2, ys2, lw=4)
#plt.legend([p1,p2, p3],
# ['Measurements', 'Kalman filter(R=0.5)', 'Kalman filter(R=10)'],
# loc='best', scatterpoints=1)
plt.show()
experiments/ukf_range.py
+111
View File
@@ -0,0 +1,111 @@
# -*- coding: utf-8 -*-
"""
Created on Sun Feb 8 09:34:36 2015
@author: rlabbe
"""
from numpy import array, asarray
from numpy.random import randn
import math
from filterpy.kalman import UnscentedKalmanFilter as UKF
class RadarSim(object):
""" Simulates the radar signal returns from an object flying
at a constant altityude and velocity in 1D.
"""
def __init__(self, dt, pos, vel, alt):
self.pos = pos
self.vel = vel
self.alt = alt
self.dt = dt
def get_range(self):
""" Returns slant range to the object. Call once for each
new measurement at dt time from last call.
"""
# add some process noise to the system
self.vel = self.vel + .1*randn()
self.alt = self.alt + .1*randn()
self.pos = self.pos + self.vel*self.dt
# add measurment noise
err = self.pos * 0.05*randn()
slant_dist = math.sqrt(self.pos**2 + self.alt**2)
return slant_dist + err
dt = 0.05
def hx(x):
return (x[0]**2 + x[2]**2)**.5
pass
def fx(x, dt):
result = x.copy()
result[0] += x[1]*dt
return result
f = UKF(3, 1, dt= dt, hx=hx, fx=fx, kappa=1)
radar = RadarSim(dt, pos=-1000., vel=100., alt=1000.)
f.x = array([0, 90, 1005])
f.R = 0.1
f.Q *= 0.002
xs = []
track = []
for i in range(int(20/dt)):
z = radar.get_range()
track.append((radar.pos, radar.vel, radar.alt))
f.predict()
f.update(array([z]))
xs.append(f.x)
xs = asarray(xs)
track = asarray(track)
time = np.arange(0,len(xs)*dt, dt)
plt.figure()
plt.subplot(311)
plt.plot(time, track[:,0])
plt.plot(time, xs[:,0])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('position (m)')
plt.subplot(312)
plt.plot(time, track[:,1])
plt.plot(time, xs[:,1])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('velocity (m/s)')
plt.subplot(313)
plt.plot(time, track[:,2])
plt.plot(time, xs[:,2])
plt.ylabel('altitude (m)')
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylim((900,1600))
plt.show()
experiments/zarchan_ball.ipynb
+50
View File
@@ -0,0 +1,50 @@
{
"metadata": {
"name": "",
"signature": "sha256:cfd803b3edb531968a1e9983b9d281824050b61d08da023ead5045daf7c8c4e7"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"import sympy\n",
"from sympy import Symbol\n",
"\n",
"from sympy.abc import x, g, b\n",
"\n",
"xdd = (0.0034*g*sympy.exp(-x/22000)*sympy.diff(x)**2) / (2*b) - g\n",
"y=sympy.symbols('y',cls=sympy.Function)\n",
"\n",
"xdd = sympy.diff(y(x), x, x) - (0.0034*g*sympy.exp(-x/22000)*sympy.diff(x)**2) + g\n",
"\n",
"\n",
"sympy.pprint(xdd)\n"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
" -x \n",
" \u2500\u2500\u2500\u2500\u2500 2 \n",
" 22000 d \n",
"g - 0.0034\u22c5g\u22c5\u212f + \u2500\u2500\u2500(y(x))\n",
" 2 \n",
" dx \n"
]
}
],
"prompt_number": 10
}
],
"metadata": {}
}
]
}