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

Checkpoint. Added a lot to the particle filter chapter.

Browse Source
This commit is contained in:
Roger Labbe committed 2015-06-28 13:38:12 -07:00
1 parent 7cf3980e8c
commit 1b92397175
4 files changed
+698 -296
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

11_Particle_Filters.ipynb
+290 -211
View File
@@ -16,7 +16,7 @@
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 1,
"metadata": {
"collapsed": false
},
@@ -250,7 +250,7 @@
"<IPython.core.display.HTML object>"
]
},
"execution_count": 4,
"execution_count": 1,
"metadata": {},
"output_type": "execute_result"
}
@@ -310,16 +310,16 @@
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 2,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuoAAAEWCAYAAAAw37JZAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuoAAAEWCAYAAAAw37JZAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x7ade390>"
"<matplotlib.figure.Figure at 0x446d208>"
]
},
"metadata": {},
@@ -380,16 +380,16 @@
},
{
"cell_type": "code",
"execution_count": 14,
"execution_count": 3,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAwgAAAEQCAYAAAAUH1PIAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAwgAAAEQCAYAAAAUH1PIAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x8912a58>"
"<matplotlib.figure.Figure at 0x818d9e8>"
]
},
"metadata": {},
@@ -408,16 +408,17 @@
"\n",
"Several iterations later you can see that all the particles have clustered around the robot. This is due to the **resampling** step. Resampling discards particles that are very improbable (very low weight) and replaces them with particles with higher probability. There are multiple algorithms for this, and we will discuss them later. \n",
"\n",
"I haven't fully shown *why* this works nor fully explained the algorithms for weighting and resampling, but it should make intuitive sense. Make a bunch of random particles, move them so they follow the robot, weight them according to how well they match the measurements, only let the likely ones live, and continue. It seems like it should work, and it does. "
"I haven't fully shown *why* this works nor fully explained the algorithms for particle weighting and resampling, but it should make intuitive sense. Make a bunch of random particles, move them so they follow the robot, weight them according to how well they match the measurements, only let the likely ones live, and continue. It seems like it should work, and it does. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Suppose we want to know the area under the curve $y= e^{1/x}$ in the interval [1,2]. We can compute the area by taking the definite integral. Go ahead and compute the answer; I'll wait. If you are wise you did not take that challenge; $y= e^{1/x}$ has no integral and so you can not find the answer this way.\n",
"## Probability distributions via MC\n",
"Suppose we want to know the area under the curve $y= \\mathrm{e}^{\\sin(x)}$ in the interval [0, $\\pi$]. We can compute the area with the definite integral $\\int_0^\\pi \\mathrm{e}^{\\sin(x)}\\, \\mathrm{d}x$. Go ahead and compute the answer; I'll wait. If you are wise you did not take that challenge; $y= \\mathrm{e}^{\\sin(x)}$ can not be integrated and so you can not find the answer this way.\n",
"\n",
"However, this is trivial to compute using a Monte Carlo (MC) technique. Create a bounding box that contains the curve in the desired interval, generate random pairs $(x,y)$, and count how many fall under the curve. Multiply the area of the bounding box by the ratio of points that were under the curve vs the total number of points and you will have computed the are under the curve. As you tend towards infinite points you can achieve any arbitrary precision. In practice, a few thousand points will give you a fairly accurate result.\n",
"However, this is trivial to compute using a Monte Carlo technique. Create a bounding box that contains the curve in the desired interval, generate random pairs $(x,y)$, and count how many fall under the curve. Multiply the area of the bounding box by the ratio of points that were under the curve vs the total number of points and you will have computed the are under the curve. As you tend towards infinite points you can achieve any arbitrary precision. In practice, a few thousand points will give you a fairly accurate result.\n",
"\n",
"Think of how powerful this technique is. You can use it to numerically integrate any function. The function can be of any arbitrary difficulty, including being non integrable and noncontinuous, and yet we can trivially find the answer. \n",
"\n",
@@ -446,7 +447,7 @@
},
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 4,
"metadata": {
"collapsed": false
},
@@ -455,15 +456,15 @@
"name": "stdout",
"output_type": "stream",
"text": [
"mean pi(N=20000)= 3.1252\n",
"err pi(N=20000)= 0.0164\n"
"mean pi(N=20000)= 3.1684\n",
"err pi(N=20000)= -0.0268\n"
]
},
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuIAAAEWCAYAAAAjCPKtAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzsvX1wW+d17vtsfAUEKIAN9IEEDpVCNyDbaVPQdRI7jdG4\nlHSur3zlnNRF6STjorcSJ7lnMo1ze1O58tRNa/cwbpJp8oc7Q6kx3DupVdSZWKrZJpZYT+GeOLE9\nJZLOXBPMEUZkCxnSJXoIiAC3iI99/3j3Wng3KDuWTYc+9PrNeEwCG3u/2ODYz7vwrGcZlmVZEARB\nEARBEAThp4prqxcgCIIgCIIgCG9HRIgLgiAIgiAIwhYgQlwQBEEQBEEQtgAR4oIgCIIgCIKwBYgQ\nFwRBEARBEIQtQIS4IAiCIAiCIGwBIsQFQRAEQRAEYQvYVCGez+dx+PBh3HDDDXC5XHjsscde9fgL\nFy7A5XJt+Ofpp5/ezGUJgiAIgiAIwlsOz2aerNFo4P3vfz9+67d+C/fccw8Mw3hNr/vud7+LX/ql\nX+Lff+ZnfmYzlyUIgiAIgiAIbzk2VYjffvvtuP322wEAmUzmNb/une98J3bv3r2ZSxEEQRAEQRCE\ntzRvCY/4xz/+cezZswcf+chH8K1vfWurlyMIgiAIgiAIbzpbKsR37NiBr3zlK/jbv/1b/MM//APG\nx8fxm7/5m/jmN7+5lcsSBEEQBEEQhDedTbWmXC+RSAT33nsv/37jjTeiWq3i4Ycfxic/+UnHsbVa\n7ae9PEEQBEEQBEHYNMLhsOP3t4Q1RecDH/gAfvzjH2/1MgRBEARBEAThTeUtJ8QLhQLe/e53b/Uy\nBEEQBEEQBOFNZdPjC6ma3e12sbi4iEKhgEgkgve85z2477778MILL+DcuXMAgMceeww+nw/JZBIu\nlwt/93d/h0ceeQQPP/zwq16nv6y/mbz44osAgJtuuulNu8Z2Re7dG0Pu3xtD7t/rR+7dG0Pu3xtD\n7t/rR+7dG+Ondf9ezV69qUL8hRdewK/92q8BAAzDwAMPPIAHHngAmUwG3/jGN1CpVFAqlfh4wzDw\n4IMPYnFxEW63GyMjI3j00UfxiU98YjOXJQiCIAiCIAhvOTZViH/0ox9Ft9t9xecfffRRx+/33HMP\n7rnnns1cgiAIgiAIgiD8T8FbziMuCIIgCIIgCG8HRIgLgiAIgiAIwhYgQlwQBEEQBEEQtgAR4oIg\nCIIgCIKwBYgQFwRBEARBEIQtQIS4IAiCIAiCIGwBIsQFQRAEQRAEYQsQIS4IgiAIgiAIW4AIcUEQ\nBEEQBEHYAkSIC4IgCIIgCMIWIEJcEARBEARBELYAEeKCIAiCIAiCsAWIEBcEQRAEQRCELUCEuCAI\ngiAIgiBsASLEBUEQBEEQBGELECEuCIIgCIIgCFuACHFBEARBEARB2AJEiAuCIAiCIAjCFiBCXBAE\nQRAEQRC2ABHigiAIgiAIgrAFiBAXBEEQBEEQhC1AhLggCIIgCIIgbAEixAVBEARBEARhCxAhLgiC\nIAiCIAhbgAhxQRAEQRAEQdgCRIgLgiAIgiAIwhYgQlwQBEEQBEEQtgAR4oIgCIIgCIKwBYgQFwRB\nEARBEIQtYFOFeD6fx+HDh3HDDTfA5XLhscce+4mv+dd//Vf86q/+KgKBAG644Qb8yZ/8yWYuSRAE\nQRAEQRDekmyqEG80Gnj/+9+Pr33taxgYGIBhGK96fL1ex4EDB/Cud70LL774Ir72ta/hz/7sz/DV\nr351M5clCIIgCIIgCG85PJt5sttvvx233347ACCTyfzE47/5zW/CNE089thjeMc73oGf//mfx/z8\nPL761a/i85///GYuTRAEQRAEQRDeUmypR/y5557Drbfeine84x382MGDB3Hx4kUsLi5u4coEQRAE\nQRAE4c1lS4V4pVLBnj17HI/R75VKZSuWJAiCIAiCIAg/FTbVmnK9/CQP+Svx4osvbvJKtuYa2xW5\nd28MuX9vjB/85/8L3bnzcI3tg/v43Wjd9SAAwDW2DwDgPn43Og89zsfo6K/rPPS447n+17mP3w0A\njse6c+f5Wv3Hte56EKjW+Xyu/WPXvGZ37jwf59o/1nsP1Tpc+8fQPTcHAPA+83DveA16T/pa3Mfv\nxot3PYjlah07IyGMje3D3Nx5lMvL+H9iO/HEE/fjLvs+jdmvP378bjz00OM4d24OkUgITzxxv+Oe\n6OfXf+9fO6+xvKwev+NDr/i5EP33+Sd9XgDQfeoHgN8HRELq/jxx/6ue65XuGf/d3PaFjfe5vAy0\nO+pFgwP8GQJA99wcnouE+LrC9SP/7Xv9yL17Y7zZ9+9973vfKz63pUI8Go1uqHxfunSJnxMEQdgs\ndIH7as93584D/YKz73X6MSTquk/9QIk0jxvdah0w19FduozuUz+A644P9V5srgPtNrpP/jdgb3Tj\nefuu1brrQSUA/T71fMMEAKc4JAFaXka3WscPIyHsLC/D7/fh3+fO4/++7Qv4L6trAICr5joWly7D\nsiwAwNLSZdxmi86ILWLn5s7jttu+ANNcR7vdwerqGm677Qv4M/t5ALihWoe/YcKs1rFr/xjM8jK8\nnS7cbpdaT/976nQBy0J37jy6dz0ILKr/1nfLy0BsJ6BvaM7NqftXraN7+jl1oqDfeX/MdfVvOp4E\ncrUONEy0bvmc+r3dAWChu3ipJ9yXLqvnhnerz/zcnLp/9ufv1s+/AXXfsNpUawv61bGdLm8CBEEQ\nXitbKsRvueUW/P7v/z6uXr3KPvGzZ88iFoth7969r/i6m2666U1bE+2K3sxrbFfk3r0x5P69Mej+\n7dq1Czi4C8u5WbRnnseezCHU8wWsLSwBAKyZ5+GPx5C88CQAoDA6gfVKFUbThNVqAw0TxkoDO9Pj\nWM7NArUrgOHCO14q83GugB++l8oAgNDBmwEAl7MzgNcDfzyG9UoVXcOAZXWBdhfveKnM13t+6AA6\ntSsAAONilc8Vmj4HHLwZ9XwBANQ5Zp4H2h24Q0F8cOUsCqMTMD1uWO0O1maex4V4DMl9NyCUSmIK\nwH/OzQIA/tzvB/bdwPfG7W7gYcOAx+NGPB5DqVTu3bh2B42GiWDQjwsXnsTk5BT8/jLc7gaCwQE0\nmyba7Q7cbrc6L4BUKok/zxeQLpURj+3GHd/+CrKjEwgsLKEUHEA6PQ4AiN90Ewp+PxDzI5RK4nJ2\nBhaJYMOA4XHDFfBjp30Pl1d+BDzzI7jdbiA4gG7TVBsGw8BAbLf6DK0uAPVt6sDIXiS//RU8P3QA\nXa8HuNqC1e6oY9pdGF4vLMMA7E3HgN+P5Le/gueMWwAAew7ejEsnz6jnYrux7m7A5/cjNH0O9X03\nqM9g/33YlR5H3V8G7Hu9nJtFp7Zq31s3OgAwvBu32J+xcH3If/teP3Lv3hg/rftXq9Ve8blNFeKN\nRgM//vGPAQDdbheLi4soFAqIRCJ4z3veg/vuuw8vvPACzp07BwD4xCc+gS9+8YvIZDK4//77USwW\n8aUvfQl/9Ed/tJnLEgThbULnocex/MyP4ItGAABWq60EMqDEmaHaYsxSGYXRCYRSSSW2bHFN4ope\n322q6rPhcTt+90UjWK9U+brLuVn44zEASkDT681SWQlDAKXJKT7eHd4BXzSCtYUldGqrWKtdgVkq\nY3fmkPN1tgjdaQvb71SqiLc72O1x4z8ATEGJYgDI5wvIRyNIpZJI2dfJ5WYRjUaQTo/zz/Pzp+Dz\n3Yp2uwOPx41QKIihoSDK5WWMjk6gYr+vlZWzAIBJbd15e5OQy82i2TSRi8eQSiXxuaED6oBQELXa\nKlZOPAmf14sKgLh9vv81X+B7QffUH48hZK9fv6/0b6vdgTs82LvflgXAgOHt/a+rNDmFnenx3mZL\nO2a3vQmjz2ptYYlFOGBvnux7TH8HhP77cm6W/0bi08fUOQH+XC49/jQEQRBeD5varPnCCy/gxhtv\nxI033gjTNPHAAw/gxhtvxAMPPABANWCWSiU+PhQK4ezZs7h48SJuuukmfPazn8Xv/d7v4d57793M\nZQmC8DaBbCKhVNIWwxasVks9afT+c+cK+GGWyricnUGn3oDV7mBnehzu8CDcoSCS86cAAP54DO7w\nDhaMuzOHsDtzSDt/T6QBTvEWSiXV60NBPq6eL8AXjcAXjSCUSqrnDAOAAVfAj+XcLMxSGa6AX4l/\nw4A7FER8+hgAJbw/5XHjrzOH8J3MIaRSSSXAbYFc0TYHObs6TsfQzwDQbndgWRbi8RhXr+n1TXv9\no6MTGLIF9rR9fZ1AQFXHs9kZ1OsNAEA0GoFhGPgSDPxJq41sdgZT9rpfrlRx1ePGniOHAUt9LuuV\nKi5nZ3A5O6O+fYBzk0MbIBLrhtcDd3gQuzOH4Ar4sbawxK8LpZIYSAzDHR7EwMhe3Lz+LK+VPisl\n0i0+l9XuqHOGgvBFI7yZ0v9O6DFXwI9u01TfSpTKDpH+yjYWQRCEV2dTK+If/ehH0e12X/H5Rx99\ndMNjv/ALv4B/+qd/2sxlCILwNsU1tg++l8qo5wswdfuFjWHbOgCwhYRsIoCqeFutNp63BSgJuPVK\nlQU3ifJQKsk2ErNU5iq24XFfs8rbbZpsa9Cfg30NOh+t2xXwo9k0cSEawcnJKZw8eQaWZWFkZC+m\np49hcnKKBTbgFOEAWFADcAj20dEJJBLDSKWSLLB37Pg1+P0+RKMRlEplNJsm21fy+QJ8vlsBABm7\nYk/k8wWurEft95FIDAMAFhaW0Gq1Ua83EAoFcS+J4XwBJwB4vV62//RXooFetbmeL/AGpv+zAIBO\nvYHl3Cw/T/ebvvFIzp9CaXIKy7lZuMOD6jNud+CyNxIdexORnD+lbEP27/rmitaznLine truncated
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuIAAAEWCAYAAAAjCPKtAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x7ec8278>"
"<matplotlib.figure.Figure at 0x81bedd8>"
]
},
"metadata": {},
@@ -506,7 +507,7 @@
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 5,
"metadata": {
"collapsed": false
},
@@ -515,7 +516,7 @@
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuAAAAEWCAYAAAAn/SKQAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzs3XlYVPXiBvB3FgYHRJRVwAVEQEVEBDfcFVFwQXOvLE1z\nqcwlvWXZ7VdZ3fJmtrmlpW3mLi644AIKqIgIIaCoKCIyCIqA7MzM7w/sKFcFVODAzPt5Hp97z/ec\nmXmHOTEvZ858j0Sr1WpBRERERER1Qip2ACIiIiIifcICTkRERERUh1jAiYiIiIjqEAs4EREREVEd\nYgEnIiIiIqpDLOBERERERHWIBZyIiIiIqA5Vq4CvXLkSDg4OUCqV8PLyQlhY2BO3DQkJQUBAAGxt\nbWFsbAx3d3f88ssvj2wjlUof+ZeUlPR8z4aIiIiIqJ6rsoBv3rwZ8+bNw5IlSxATEwNvb2/4+fkh\nNTX1sdufPHkS7u7u2L59O+Lj4zF79mzMmDEDmzZtemTbhIQEqFQq4V/btm2f/xkREREREdVjkqqu\nhNm9e3d07twZa9asEcacnZ0xduxYfP7559V6kAkTJkCtVmPbtm0Ayo+ADxw4EJmZmTA3N3+O+ERE\nREREDUulR8BLSkoQHR0NX1/fCuO+vr6IiIio9oPk5OTAzMzskXEvLy/Y2trCx8cHISEh1b4/IiIi\nIqKGSl7ZyqysLKjValhbW1cYt7KygkqlqtYD7N27F0ePHq1Q2G1tbbF69Wp07doVxcXF+O233zBo\n0CCEhoaid+/ez/A0iIiIiIgahkoL+PMKDw/HSy+9hO+//x5eXl7CuLOzM5ydnYXlHj164Nq1a1i2\nbFmFAp6Tk1Ob8YiIiIiIapWpqekjY5WegmJhYQGZTIaMjIwK4xkZGbCxsan0wcLCwuDv749PP/0U\nM2fOrDJct27dcOnSpSq3IyIiIiJqyCot4AqFAp6enjh06FCF8eDgYHh7ez/xdsePH4e/vz8+/vhj\nvP3229UKEhMTA1tb22ptS0RERETUUFV5CsqCBQswefJkdOvWDd7e3li9ejVUKhVmzZoFAFi8eDHO\nnDmDw4cPAyif4WTYsGF46623MGnSJOFccZlMBktLSwDAihUr4ODggA4dOqCkpAS///47AgMDsWPH\njifmeNzhe3o+UVFRAFDh9CDSH3z9ifsAcR/Qb3z9a09Vp1FXWcDHjx+P27dvY+nSpUhPT4ebmxuC\ngoLQsmVLAIBKpUJycrKw/caNG1FUVIRly5Zh2bJlwri9vb2wXWlpKRYtWoQbN25AqVSiY8eOCAoK\nwtChQ5/pSRIRERERNRRVzgMupof/euAR8JrHv3z1G19/4j5A3Af0G1//2lNVh63WpeiJiIiIiKhm\nsIATEREREdUhFnAiIiIiojrEAk5EREREVIdYwImIiIiI6hALOBERERFRHWIBJyIiIiKqQyzgRERE\nRER1iAWciIiIiKgOsYATEREREdUhFnAiIiIiojrEAk5EREREVIfkYgcgIqKapdaokZWjQtbd9PL/\nzVEhr+Au7hXm4l5hLoqK86HWalBcXASNVoNdMY1gKG8EhYEhlIbGMDU2g2ljc5g2NoNVU1s0N2+J\nZiaWkEp4zIaIqCawgBMRNXDZeZm4dOM8UlSXkHrrCtIyr6JUXVLt25eUFVa5jUJuCBuL1rBv7gwH\nm3ZwsGmHZiYWzxObiEhvsYATETUwJWXFuHg9FglXzyIp9W9k5qTXyWOmqJKQokpCaMxeAIBVU1u0\nt++CDvaecLTrAIXcsNZzEBHpAhZwIqIGoLSsBHHJkYi5HIGEa9EoKS2qdHvTxuawamoLy6bNYW5q\ng6aNzdFY2QSNlU2gNDSGTCrH+bjzkEgkcO3oipKyYpSUFqGg6B7u3ruNnPw7yM7LQsadVKTfSUV+\nYe4jj3Hr7k3cirmJ0Ji9MDRoBLc23dHFuTfate4Mucygtn4UREQNHgs4EVE9lnrrCk7FH0HUxVAU\nFuc/dhsDuQJtbNvD0bYDWlm3RUsrR5gYNa3yvpWKxgBQrVNJ8gru4nrGZVxNv4Cr6RdxLf1ihdNc\nikuLEHUxFFEXQ6E0NEbXdv3Ry20IbMxbVfOZEhHpDxZwIqJ6prSsFNFJxxESsxdpmVcfu41VU1t0\ncuyB9vYesG/eDgby2j3ibGLUFK4OXnB18AJQfkrKlbQEJF6LRvy1s8i8e1PYtrA4H8dj9+F47D44\n2nZAL7chcG/rXesZiYgaChZwIqJ6Ir8oD+F/H8Dx2CDkFmQ/st7c1Brd2g1AZ6deaG7WAhKJRISU\n5RRyQ7Rv7YH2rT0wuu9rSMu6iuiLYYhOOoE7eZnCdlduJuDKzQSYhm3EgC4B6NXRF4YKpWi5iYjq\nAxZwIiKR5Rfm4vDZnTgRG4SSsuIK6wzkCnRu640eroPgaOdaL6cClEgkaGHZBi0s22B4r5dxKTUO\n4XEH8XfyaWg0agBATv4d7DrxCw6d2Ya+7v7o13k4jBuZiJyciEgcLOBERCIpLM7HsXO7cezcbhSX\nVJwK0NTYDH07D0evjr4watRYpIRPTyqRwqWVO1xauSM3Pxsn4w/jxENH9AuK8nDg9GaEnNsDH68X\n0L/zCCgMOHsKEekXFnAiojqmVpfheGwQDp7ZioKivArrbMxbYZDnaHRx7t3gZxJpYtwMQ7qNw8Au\nAYhMPIbDZ3fgdk4GAKCopAB7I37Hidgg+PWYhO4dBkImlYmcmIiobrCAExHVocSUc9gRuh4Z2Tcq\njFubtYB/jxfh3rZHvTzN5HkYyBXo5TYEPVx9EHMpHPtPb8at7DQA5aem/HXkR4Sc241xA2bAqYWb\nyGmJiGofCzgRUR3IvJuOncd/xvmrZyqMm5taw6/7RHi59IVUx48Ay6QyeLr0RWenXjgVfxj7T/+F\n3PzyU1NUd1Lx/fYP4enSF6P6TIGpsZnIaYmIag8LOBFRLVJr1DgWHYj9p/6qMG+2oUIJv+4T0Nd9\nWIM/1eRpyaQy9HIbAq92/RBybg8OR21H8f0LC529eBznr56Bf49J6Oc+TOf/KCEi/cQCTkRUS25k\nJuPPwz/gxq1kYUwCCbp3GIjh3pPRxLjqi+XoMkODRhjSbRy6dxiIXSc2IDrpBACguKQQO4//jHNJ\n4Xhp8BxYm7UQOSkRUc1iASciqmFl6lIcOL0Zh6N2QKPVCON2lg6YOPANtG7uJGK6+qdpY3NM8XsH\n3h0HY+uxtcL58ddUF/Hln/MxrOeLGOAxkkfDiUhnsIATEdUg1Z1U/HrgG9zIfHDUWy4zgF/3iRjY\nJQAyGX/tPolzy05496VvcDhqBw5GboVaU4YydSkCwzYi5lIEJg+ZD6tmtmLHJCJ6bnwnICKqAVqt\nFif+3o/AExsqnOvtaOeKSYPegFUzOxHTNRxymQGGdp8Atzbd8cfh74TTd1IyLuGrTQswrv8MdGs/\nQNSrgBIRPS8WcCKi55RXkIM/Dn2LhJRoYUwuM8CIXpPRr/NwnZtWsC7YWdrjnfFf4cjZndh/ejPU\nmjKUlBbhj+DvkJhyDhMGzoLS0FjsmEREz4QFnIjoOSTfvIBf9i9Dzr3bwpiteWu8MnQ+bC3sxQum\nA2QyOXy7jUN7e09sPPC1MHd4dNIJXFNdxFS/RTyfnogaJB6WISJ6BlqtFsfO7cZ32z+oUL4HeIzE\nOxOXsXzXoJZWbbBo0tfo4eojjN3JvYUV2xYj4vwhEZMRET0bHgEnInpKhcUF+PPw94i9fFIYMzJs\njMlD5sHVwUvEZLrL0KARXvR5C+1adcbmIytRWFIAtboMfx1ZiWuqJIzrPwMGcoXYMYmIqoUFnIjo\nKWRkp+Gn3Z/h1t2bwlgraydM9V8I8ybWIibTD12ce6OllSPW7/sSN7OuAQBOxR9GWuZVvDbsX3wN\niKhB4CkoRETVlJhyDsv/WlShfPfp5I+5Yz9n8atDlk1tsGD8l/Bq108YS711Bcs2LcSlG3EiJiMi\nqh4WcCKiKmi1WoSc24PVgZ+isKQAAGAgV+CVIfMxbsAMGMj161Ly9YHCwBCTfedhbP8ZwgV6Cory\n8OPO/8PJ88EipyMiqhxPQSEiqkSZuhRbj63FyfgHpc60sTleH74YrazbipiMJBIJ+rr7o4VlG/y8\n70vkFmRDo1Fj05EfcetuGkZ4T+bVM4moXqrWEfCVK1fCwcEBSqUSXl5eCAsLe+K2ISEhCAgIgK2t\nLYyNjeHu7o5ffvnlke1CQ0Ph6ekJpVIJR0dHrFmz5tmfBRFRLSgovoeVuz6uUL5bN3fGwonLWL7r\nkTa27bBgwlewe2jmmSNnd2H9vi9RXFIoXjAioieosoBv3rwZ8+bNw5IlSxATEwNvb2/4+fkhNTX1\nsdufPHkS7u7u2L59O+Lj4zF79mzMmDEDmzZtEra5evUq/P390bt3b8TExGDx4sWYM2cOduzYUXPP\njIjoOWTnZeLbre/j8o3zwljXdv3x9pilMDU2EzEZPY5ZE0vMG/cFOrbpJozFJUdixbb3kZN/R8Rk\nRESPkmi1Wm1lG3Tv3h2dO3eucITa2dkZY8eOxeeff16tB5kwYQLUajW2bdsGAHj33Xexa9cuXLx4\nUdjm9ddfR3x8PCIiIoSxnJwc4f+bmppW7xlRtUVFRQEAvLw4bZo+4uv/ZDezrmFV4KcV5vce7v0y\nBnuN0alLoOviPqDRqLE7/Dccjd4ljJk1scLsUR/BupmdiMnqJ13cB6j6+PrXnqo6bKVHwEtKShAd\nHQ1fX98K476+vhWKcnVCmJk9OGJ08uTJx95nVFQU1Gp1te+XiKimJaXGYcXW94XyLZPKMXnIfPh2\nHatT5VtXSaUyjOozBRMHvQmppPwt7k7uLazY8h6uqZJETkdEVK7SAp6VlQW1Wg1r6Line truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x7aabc50>"
"<matplotlib.figure.Figure at 0x81c2198>"
]
},
"metadata": {},
@@ -540,7 +541,7 @@
},
{
"cell_type": "code",
"execution_count": 5,
"execution_count": 6,
"metadata": {
"collapsed": false
},
@@ -549,7 +550,7 @@
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAr8AAAD9CAYAAAC1Fh+gAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x8781400>"
"<matplotlib.figure.Figure at 0x872b0f0>"
]
},
"metadata": {},
@@ -575,25 +576,24 @@
"\n",
"All of this brings us to the particle filter. Consider tracking a robot or a car in an urban environment. For consistency I will use the robot localization problem from the ends of the EKF and UKF chapters. In this problem we tracked a robot that had a sensor that could detect the range and bearing to landmarks. \n",
"\n",
"This is a book on Bayesian filters, and I hope by now you expect us to use the standard Bayesian algorithm of update and predict cycles. We will. \n",
"\n",
"Taking insight from the discussion above we start by creating several thousand **particles**. Each particle has a position that represents a possible belief of where the robot is in the scene, and perhaps a heading and velocity. Suppose that when we initialize the filter we have no knowledge of the location of the robot. We would want to scatter the particles uniformly over the entire scene. If there was a large clump of particles near a specific location that would imply that we were more certain that the robot is there. If you think of all of the particles representing a probability distribution, locations where there are more particles represent a higher belief, and locations with fewer particles represents a lower belief.\n",
"\n",
"Think back to the *Discrete Bayes* chapter. In that chapter we modeled positions in a hallway as discrete and uniformly spaced. This is very similar except the particles are randomly distributed.\n",
"\n",
"Each particle needs a weight - a probability that it represents the true position of the robot. At initialization we have no reason to favor one particle over another, so we would typically assign a weight of $1/n$, where $n$ is the number of particles. We did the same thing in the discrete Bayes chapter. When we initialized the filter we assigned a probability of 1/N to each hallway position. The reason for $1/n$ is simple - the sum of all probabilities must equal one, and of course $\\sum\\limits_{i=1}^n \\frac{1}{n} = 1$.\n",
"\n",
"We will be operating on many particles so it is imperitive that we are as efficient in our programming as possible. That means storing the particles in an NumPy `array` and making full use of its vectorization capabilities.\n",
"We will be operating on many particles so it is imperative that we are as efficient in our programming as possible. That means storing the particles in an NumPy `array` and making full use of its vectorization capabilities.\n",
"\n",
"To track our robot we need to maintain states for x, y, and heading. We will store `N` particles in a `(N, 3)` shaped array. The three columns contain x, y, and heading, in that order.\n",
"\n",
"In this problem the robot can move on a plane of some arbitrary dimension, with the lower right corner at (0,0).\n",
"\n",
"Here is a partial implementation of this problem. The particles are initially randomly distributed over the space, and are given a random heading. "
]
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 7,
"metadata": {
"collapsed": true
},
@@ -604,16 +604,20 @@
"\n",
"class ParticleFilter(object):\n",
"\n",
" def __init__(self, N, x_dim, y_dim):\n",
" self.particles = np.zeros((N, 3)) # x, y, heading\n",
" def __init__(self, N, x_dim, y_dim, landmarks, measure_std_error):\n",
" self.N = N\n",
" self.x_dim = x_dim\n",
" self.y_dim = y_dim\n",
" self.landmarks = landmarks\n",
" self.R = measure_std_error\n",
"\n",
" # assign reasonable default.\n",
" self.weights = np.array([1./N] * N)\n",
" self.particles[:, 0] = uniform(0, x_range, size=N)\n",
" self.particles[:, 1] = uniform(0, y_range, size=N)\n",
" # distribute particles randomly with uniform weight\n",
" self.weights = np.empty(N)\n",
" self.weights.fill(1./N)\n",
"\n",
" self.particles = np.empty((N, 3)) # x, y, heading \n",
" self.particles[:, 0] = uniform(0, x_dim, size=N)\n",
" self.particles[:, 1] = uniform(0, y_dim, size=N)\n",
" self.particles[:, 2] = uniform(0, 2*np.pi, size=N)"
]
},
@@ -629,10 +633,16 @@
"\n",
"I cannot easily modify a class definition in Jupyter Notebook, so I will show you the method in text. The entire class is in pf_internal, and we will be importing when we run the filter. \n",
"\n",
" def predict(self, u, std):\n",
" \"\"\" move according to control input u with noise std\"\"\"\n",
" def predict(self, u, std, dt=1.):\n",
" \"\"\" move according to control input u (heading change, velocity) \n",
" with noise std\"\"\"\n",
"\n",
" self.particles[:, 0:2] += u + randn(self.N, 2) * std"
" self.particles[:, 2] += u[0] + randn(self.N) * std[0]\n",
" self.particles[:, 2] %= 2 * np.pi\n",
"\n",
" d = u[1]*dt + randn(self.N) * std[1]\n",
" self.particles[:, 0] += np.cos(self.particles[:, 2]) * d\n",
" self.particles[:, 1] += np.sin(self.particles[:, 2]) * d"
]
},
{
@@ -654,11 +664,22 @@
"\n",
"We will want to do the same thing with our particles. Each particle has a position. We can also assign it a weight or probability based on how well it matches the measurement. We will want to multiply that new probability into the current weight of the particle. If we then normalize the weights of the particles those that are closest to the robot will generally have a higher weight than ones far from the robot.\n",
"\n",
" def update(self, z):\n",
" self.weights.fill(1.)\n",
" for i, landmark in enumerate(self.landmarks):\n",
" dist = np.linalg.norm(self.particles[:, 0:2] - landmark, axis=1)\n",
" self.weights *= scipy.stats.norm(dist, self.R).pdf(z[i])\n",
" self.weights /= sum(self.weights) # normalize\n",
"\n",
"For reference, in the literature this part of the algorithm is called *Sequential Importance Sampling*, or SIS.\n",
"\n",
"And that is the general framework for the particle filter. The particles and their weights represent the probability distribution of our belief. This algorithm works well for multiple objects; clusters of particles will group around each object. We can compute the position of the robot by computing the mean of the point positions multiplied by their weights. This is more complicated if we have multiple objects as we have to run some sort of clustering algorithm to determine which particles belong to which object, but that isn't hard. \n",
"And that is the general framework for the particle filter. The particles and their weights represent the probability distribution of our belief. This algorithm works well for multiple objects; clusters of particles will group around each object. We can compute the position of the robot by computing the mean of the point positions multiplied by their weights. If we have multiple objects we have to run some sort of clustering algorithm to determine which particles belong to which object, but that isn't hard. \n",
"\n",
"This is a complete implementation of a particle filter using the Bayesian framework that we have used throughout the book. If you were to implement it you would find that it rarely performs well. I'd rather get to a working example quickly, so I will skip implementing it and just tell you about the problems. If you do a historical survey of particle filters you will see that it took some time to discover and fix all of these issues. "
"This is a nearly complete implementation of a particle filter using the Bayesian framework that we have used throughout the book. It is not quite functional in practice due to some problems which I will address in subsequent sections.\n",
"\n",
"I want to emphasize that as with the other filters we are using Bayes Theorem in the update step. Compare the Bayes equation to `update()` and you will see why I assert this.\n",
"\n",
"$$P(A|B) = \\frac{P(B|A) P(A)}{P(B)}$$"
]
},
{
@@ -667,54 +688,84 @@
"source": [
"### Particle Resampling\n",
"\n",
"I mentioned problems that the SIS algorithm has. One is called the **degeneracy problem**. We initialize a large area with uniformly distributed particles. The number of particles near the robot are likely to be low. As we perform the algorithm above any particle modestly far from the robot will quickly acquire an extremely low weight, and only one or two particles near the robot with has an appreciable weight. The filter requires hundreds to thousands of particles to adequately sample the probability distribution. Only one or two near the robot are not enough.\n",
"\n",
"The first problem we face is called the **degeneracy problem**. We initialize a large area with uniformly distributed particles. The number of particles near the robot are likely to be low. As we perform the algorithm above any particle modestly far from the robot will quickly acquire an extremely low weight, and only one or two particles near the robot with has an appreciable weight. The filter depends on many hundreds to thousands of particles to depict the probability distribution. Only one or two near the robot will cause it to fail.\n",
"This problem is solved by some form of **resampling** of the particles. If most of the particles have a very small weight this means that most particles are not a very good representation of the probability distribution of the robot. The simplest resampling algorithm endevours to discard particles with very low probability (low weights) and replaces them with particles with higher probability. These particles are taken from the current particle set. This means that some of our particles will be duplicated, but this is not a problem because during the predict step we will be adding random noise to each, so they will separate from their twins. \n",
"\n",
"This problem is solved by some form of **resampling** of the particles. Suppose we notice that the number of particles that have relatively high weights is small. We want to "
"One way to accomplish this is to sample from the current particle set `N` times, making a new set of particles from the sample. For this to work the probability of selecting any given particle should be proportional to its weight. The easiest way to accomplish this is to use NumPy's `cumsum` function. `cumsum` computes the *cumulative* sum of an array. That is, element one is the sum of elements zero and one, element two is the sum of elements zero and one, etc. Here is my implementation of this algorithm:\n",
"\n",
" def resample(self):\n",
" p = np.zeros((self.N, 3))\n",
" w = np.zeros(self.N)\n",
"\n",
" cumsum = np.cumsum(self.weights)\n",
" for i in range(self.N):\n",
" index = np.searchsorted(cumsum, random())\n",
" p[i] = self.particles[index]\n",
" w[i] = self.weights[index]\n",
"\n",
" self.particles = p\n",
" self.weights = w / np.sum(w)\n",
" \n",
"You will not necessarily want to resample every iteration. For example, if you received no new measurements you have not received any information from which the resample can benefit. We can determine when to resample by using something called the **effective N**, which is meant to approximate the number of particles with have an appreciable weight which meaningfully contributes to the probability distribution. The equation for this is\n",
"\n",
"$$\\hat{N}_{eff} = \\frac{1}{\\sum w^2}$$\n",
"\n",
"and we can implement this in Python with\n",
"\n",
" def neff(self):\n",
" return 1. / np.sum(np.square(self.weights))\n",
"\n",
"\n",
"If $\\hat{N}_{eff}$ falls below some threshold it is time to resample. A useful starting point is $\\frac{1}{2}N$, but this varies by problem. It is also possible for $\\hat{N}_{eff} = N$, which means the particle set has collapsed to one point (each has equal weight). It may not be theoretically pure, but if that happens I create a new distribution of particles in the hopes of generating particles with more diversity. If this happens to you often, you may need to increase the number of particles, or otherwise adjust your filter. We will talk more of this later."
]
},
{
"cell_type": "code",
"execution_count": null,
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"outputs": [],
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## some title\n",
"## A Full Example\n",
"\n",
"The particle filter is easy to understand, and I could explain the entire algorithm to you now. But I want you to understand it within the context of the other Bayesian filters in this book. When you design a filter you first have to choose which filter to use, and understanding the relative strengths and weaknesses of each is important.\n",
"There is much more to cover, but we have covered enough to implement a full particle filter. The filter in this form is called the **Sampling Importantance Resampling filter**, or SIR.\n",
"\n",
"So let's start with a chart we first saw in the *Nonlinear Filtering* chapter. We started with a Gaussian with a mean of 0 and a variance of 1 and created 500,000 random points fitting that distribution. We passed each of those points through a nonlinear function, and plotted the result."
"The code above has been placed in the file *./code/RobotLocalizationParticleFilter.py* which we will import. If you want to read the source code, comment out the %load command in the next cell and press ctrl-return to execute it.\n",
"\n",
"To implement a particle filter we need to create the landmarks and the filter. We then execute a loop, successively calling `predict`, `update`, and `resample`. The class includes a method named `estimate` which computes the weighted mean and covariance of all the particles"
]
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 8,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"# uncomment to view source code\n",
"# %load ./code/RobotLocalizationParticleFilter"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"final position error, variance: [ 18.00486271 18.00176074] [ 0.00458291 0.00448719]\n"
]
},
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuIAAAFfCAYAAAALX6kJAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAtsAAAEWCAYAAAC37ltfAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x7f1fc18>"
"<matplotlib.figure.Figure at 0x8700400>"
]
},
"metadata": {},
@@ -722,122 +773,79 @@
}
],
"source": [
"import numpy as np\n",
"from numpy.random import normal\n",
"from nonlinear_plots import plot_nonlinear_func\n",
"from RobotLocalizationParticleFilter import *\n",
"from numpy.linalg import norm\n",
"\n",
"gaussian = (1., 1.)\n",
"data = normal(loc=gaussian[0], scale=gaussian[1], size=500000)\n",
"def run_pf1(N, iters=18, sensor_std_err=.1, \n",
" do_plot=True, plot_particles=False,\n",
" xlim=(0, 20), ylim=(0, 20),\n",
" initial_x=None):\n",
" landmarks = np.array([[-1, 2], [5, 10], [12,14], [18,21]])\n",
" NL = len(landmarks)\n",
" pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, sensor_std_err)\n",
" if initial_x is not None:\n",
" pf.create_gaussian_particles(mean=initial_x, var=(5, 5, np.pi/4))\n",
" \n",
" if plot_particles:\n",
" plt.scatter(pf.particles[:, 0], pf.particles[:, 1], alpha=.25, color='g')\n",
" \n",
" xs = []\n",
" pos = np.array([0., 0.])\n",
" for x in range(iters):\n",
" pos += (1, 1) # robot position\n",
"\n",
"def g(x):\n",
" return .2*x**3 - .8*x**2\n",
" # distance from robot to each landmark\n",
" zs = norm(landmarks - pos, axis=1) + randn(NL)*sensor_std_err\n",
"\n",
"with book_format.figsize(y=5):\n",
" plot_nonlinear_func(data, g, gaussian)"
" # move diagonally forward to (x+1, x+1)\n",
" pf.predict((0.00, 1.414), (.2, .05))\n",
" pf.update(z=zs)\n",
" if pf.neff() < N/2:\n",
" pf.resample()\n",
"\n",
" mu, var = pf.estimate()\n",
" xs.append(mu)\n",
" if plot_particles:\n",
" plt.scatter(pf.particles[:, 0], pf.particles[:, 1], color='k', marker=',', s=1)\n",
" p1 = plt.scatter(pos[0], pos[1], marker='+', color='k', s=120)\n",
" p2 = plt.scatter(mu[0], mu[1], marker='s', color='r')\n",
" \n",
" xs = np.array(xs)\n",
" plt.plot(xs[:, 0], xs[:, 1])\n",
" plt.legend([p1, p2], ['Actual', 'PF'], loc=4)\n",
" plt.xlim(*xlim)\n",
" plt.ylim(*ylim)\n",
" print('final position error, variance:', mu, var)\n",
"\n",
"from numpy.random import seed\n",
"seed(2) \n",
"run_pf1(N=5000, plot_particles=False)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The EKF deals with this problem by linearizing the function at $x$ by evaluating the derivative at x: $f'(x)$. This computes the slope of the function at $x$, which is a linear function. If the function is smooth and 'fairly' linear locally the EKF produces a reasonable but suboptimal result. We can see from the chart above how much error is incurred with this linearization. \n",
"Most of this code is devoted to initialization and plotting. The entirety of the particle filter processing consists of these lines:\n",
"\n",
"The UKF handles larger nonlinearities by taking $2n+1$ samples near $x$. These samples are passed through the function, and the mean and standard deviation of the transformed points are used as the new estimate for the system state. In general this provides better estimates than the EKF at the cost of more computation. \n",
" # distance from robot to each landmark\n",
" zs = norm(landmarks - pos, axis=1) + randn(NL)*sensor_std_err\n",
"\n",
"The EKF represents the nonlinearity of the function with one point, and the UKF represents it with $2n+1$ points. This allows them to handle the nonlinearity of the function, but in the end both assume a Gaussian distribution by taking the mean and standard deviation of the sampled points. \n",
" # move diagonally forward to (x+1, x+1)\n",
" pf.predict(u=(0.00, 1.414), std=(.2, .05))\n",
" pf.update(z=zs)\n",
" if pf.neff() < N/2:\n",
" pf.resample()\n",
"\n",
"It has probably occurred to you that the code that I used to graph the solution above constitutes an accurate solution, and it is. In other words, if we take a *large enough* sample from any probability distribution and pass them through our function the result will be an accurate representation of the new mean and probability distribution. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Monte Carlo Method\n",
"The first line takes advantage of `numpy.linalg.norm`, which computes the Euclidian distance of a vector. In other words, we compute the distance of the robot to each landmark, and add in sensor noise so that this is a realistic simulation. Uou wouldn't add noise if this was real sensor data, of course!\n",
"\n",
"The next line predicts the position of the particles with the assumption that the robot is moving in a straight line (`u[0] == 0`) and moving 1 unit in both the x and y axis (`u[1]==1.414`). The standard deviation for the error in the turn is 0.2, and the standard deviation for the distance is 0.05. When this call returns the particles will all have been moved forward, but the weights are no longer correct as they have not been updated.\n",
"\n",
"** old stuff - to be deleted/incorporated **\n",
"The next line incorporates the measurement into the filter. This does not alter the particle positions, it only alters the weights. If you recall the weight of the particle is computed as the probability that it matches the Gaussian of the sensor error model. The further the particle from the measured distance the less likely it is to be a good representation.\n",
"\n",
"All of this brings us to the particle filter. Consider tracking a robot or a car in an urban environment. For consistency I will use the robot localization problem from the ends of the EKF and UKF chapters. In this problem we tracked a robot that had a sensor that could detect the range and bearing to landmarks. \n",
"The final two lines example the effective particle count ($\\hat{N}_{eff})$. If it falls below $\\frac{1}{2}N$ we perform resampling to try to ensure our particles form a good representation of the actual probability distribution.\n",
"\n",
"Taking insight from the discussion above we start by creating a several thousand **particles**. Each particle has a position that represents a possible belief of where the robot is in the scene. Suppose that when we initialize the filter we have no knowledge of the location of the robot. We would want to scatter the particles uniformly over the entire scene. If there was a large clump of particles near a specific location that would imply that we were more certain that the robot is there. If you think of all of the particles representing a probability distribution, locations where there are more particles represent a higher belief, and locations with fewer particles represents a lower belief.\n",
"\n",
"Next we get a set of measurements - one for each landmark currently in view. How should these measurements be used to alter our probability distribution as modeled by the particles?\n",
"\n",
"Think back to the *Discrete Bayes* chapter. In that chapter we modeled positions in a hallway as discrete and uniformly spaced. We assigned a probability to each position. When a new measurement came in we multiplied the current probability of that position by the probability that the measurement matched that location. We implemented that like this:\n",
"\n",
" def update(map_, belief, z, prob_correct):\n",
" scale = prob_correct / (1. - prob_correct)\n",
" for i, val in enumerate(map_):\n",
" if val == z:\n",
" belief[i] *= scale\n",
" normalize(belief)\n",
"\n",
"We will want to do the same thing with our particles. Each particle has a position. We can also assign it a weight or probability. We can compute the probability of the measurements matching that \n",
"\n",
"Here is a function that creates a particle in a square with a random velocity. We ignore units, trusting that the caller will use self consistant units such as MKS. Each particle is represented as a list with the structure `[x_position, y_position, speed, direction]`"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"from random import uniform\n",
"from math import pi\n",
"def make_particle(xrange=(0, 100), yrange=(0, 100), max_velocity=2.):\n",
" return [uniform(*xrange), uniform(*yrange), \n",
" uniform(0, max_velocity), uniform(0, 2*pi)]"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here we create 1000 random plots and plot them."
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"particles = np.asarray([make_particle((0, 100), (0, 100), 2) for i in range(1000)])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now we can write code that moves the particle over a period of time."
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"def move(particles, t=1.):\n",
" vx = particles[:, 2]*np.cos(particles[:, 3])\n",
" vy = particles[:, 2]*np.sin(particles[:, 3])\n",
" particles[:, 0] = (particles[:, 0] + vx*t) % 100\n",
" particles[:, 1] = (particles[:, 1] + vy*t) % 100"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"So let's plot the particles, move them, and plot them again"
"Now let's look at this with all the particles plotted. Seeing this happen interactively is much more useful, but this format still gives us useful information. I plotted the original random distribution of points in a very pale green and large circles to help distinguish them from the subsequent iterations where the particles are plotted with black pixels. The number of particles makes it hard to see the details, so I limited the number of iterations to 8 so we can zoom in and look more closely."
]
},
{
@@ -847,11 +855,18 @@
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"final position error, variance: [ 8.06115913 7.89412269] [ 0.0029203 0.00299334]\n"
]
},
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAr8AAAD9CAYAAAC1Fh+gAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAs4AAAEWCAYAAAB2aRHzAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x7e1c080>"
"<matplotlib.figure.Figure at 0x86ef940>"
]
},
"metadata": {},
@@ -859,34 +874,30 @@
}
],
"source": [
"def plot_particles(particles, x_range=(0, 100), y_range=(0, 100)):\n",
" with book_format.figsize(4,4):\n",
" plt.scatter(particles[:, 0], particles[:, 1])\n",
" plt.axis('equal')\n",
" plt.xlim(*x_range)\n",
" plt.ylim(*y_range)\n",
" plt.gca().get_xaxis().set_ticks([])\n",
" plt.gca().get_yaxis().set_ticks([])\n",
"\n",
"plt.subplot(121)\n",
"plot_particles(particles)\n",
"move(particles, t=10)\n",
"plt.subplot(122)\n",
"plot_particles(particles)"
"seed(2) \n",
"run_pf1(N=5000, iters=8, plot_particles=True, xlim=(0,8), ylim=(0,8))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
"source": [
"From the plot it looks like there are only a few particles at the first two robot positions. This is not true; there are 5,000 particles, but due to resampling most are duplicates of each other. The reason for this is the Gaussian for the sensor is very narrow. I'll address this in detail below. For now, looking at the second step at x=2 we can see that the particles have dispersed a bit. This dispersion is due to the motion model noise. All particles are projected forward according to the control input `u`, but noise is added to each particle proportional to the error in the control mechanism in the robot. By the third step the particles have dispersed enough to make a convincing cloud of particles around the robot. \n",
"\n",
"The shape of the particle cloud is an ellipse. This is not a coincidence. The sensors and robot control are both modeled as Gaussian, so the probability distribution of the system is also a Gaussian. The particle filter is a sampling of the probability distribution, so the cloud should be an ellipse.\n",
"\n",
"It is important to recognize that the particle filter algorithm *does not require* the sensors or system to be Gaussian or linear. Because we represent the probability distribution with a cloud of particles we can handle any probability distribution and strongly nonlinear problems. There can be discontinuities and hard limits in the probability model. For example "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"So we have a random distribution of particles, how do we use them to track objects? Think of these particles as *beliefs*. We want each particle to represent a possible belief for the movement of a tracked object. Each particle only contains a position and velocity, so we need a separate data structure to contain the probability that the particle is tracking an object. Much like we did in the **Discrete Bayes** chapter we can use a NumPy array to store the probabilities. \n",
"### Effect of Sensor Errors on the Filter\n",
"\n",
"We have not yet incorported any measurements, so if you recall from the Bayesian math sections we would call this our *prior*, as it contains the probability *prior* to adding a measurement. With no measurements we have no reason to believe any particular particle is more likely to be correct than any other particle, so it is reasonable to assign the same probability to each. We want a *probability distribution*, which means that the sum of the probabilities of all the particles must be one. Here is some code to do that."
"The first few iterations of the filter resulted in many duplicate particles. This happens because the model for the sensors is Gaussian, and we gave it a small standard deviation of $\\sigma=0.1$. This is counterintuitive at first. The Kalman filter performs *better* when the noise is smaller, yet the particle filter can perform worse. We can reason why this is true. The standard deviation is just 0.1. This means that if the robot is at (1, 1) and a particle is at (2, 2) the particle is 14 standard deviations away from the sensor. This gives it a near zero probability, and it is extremely unlikely to survive after the resampling. \n",
"\n",
"This is *very important* to understand - a very accurate sensor can lead to poor performance of the filter because few of the particles will be a good sample of the probability distribution. There are a few fixes available to us. First, we can artificially increase the sensor noise standard deviation so the particle filter will accept more points as matching the robots probability distribution. This is non-optimal because some of those points will be a poor match. The real problem is that there just isn't enough points being generated such that enough are near the robot. Increasing `N` usually fixes this problem. This decision is not cost free as increasing the number of particles significantly increase the computation time. Still, let's look at the result of using 100,000 particles."
]
},
{
@@ -895,21 +906,49 @@
"metadata": {
"collapsed": false
},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"final position error, variance: [ 8.03991809 7.93388899] [ 0.00275743 0.00305754]\n"
]
},
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAs4AAAEWCAYAAAB2aRHzAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x8733e48>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"N=len(particles)\n",
"p = np.array([1/N]*N)"
"seed(2) \n",
"run_pf1(N=100000, iters=8, plot_particles=True, xlim=(0,8), ylim=(0,8))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We have 1,000 particles so the probability for each particle is 0.001 (1/1000). \n",
"There are many more particles at x=1, and we have a convincing cloud at x=2. Clearly the filter is performing better, but if you are running this in your browser you must have noticed how slow the filter ran. \n",
"\n",
"Now how do we incorporate measurements? Say we run our computer vision software on the first video frame and it reports that there is a blob at (43.2, 21.8). We can evaluate how likely each particle is taking that measurement into account. I don't want to get bogged down in the math at the moment, but recall that we would write that as $P(particle|blob)$. I bring that up to emphasize that this is a *Bayesian* filter- we incorporate new information to adjust our current beliefs. \n",
"Another approach is to be smarter about generating the initial particle cloud. Suppose we know that the robot is near (0,0). This is not exact, as the simulation actually places the robot at (1,1), but it is close. If instead of creating a uniform cloud of particles over the entire map we made a normally distributed cloud near (0, 0) there is a much greater chance of the particles matching the robot's position. The particle filter code includes the method `create_gaussian_particles()`; feel free to alter the code above to use this function as in the code snippet below. However, we will be using this in the next section to help with a different problem, so feel free to wait.\n",
"\n",
"We don't yet know how to compute the new probabilities for the particles, but we can reason that a particle at (0,0), far from the measurement, would have a very low probability of representing the object, whereas a particle at (42.9, 21.9) should have a relatively high probability of representing the object."
" pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, sensor_std_err)\n",
" pf.create_gaussian_particles(mean=(0, 0), var=(10,10))\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Filter Degeneracy From Inadequate Samples\n",
"\n",
"The filter as written is far from perfect. Here is how it performs with a different random seed."
]
},
{
@@ -919,60 +958,100 @@
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"final position error, variance: [ 13.65122791 -15.53457295] [ 57.83174853 37.12102252]\n"
]
},
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAuYAAAEWCAYAAAAq41LXAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzsvVuMJOd53v+rqq6uPp97uue8M7Ozs8td7i65XJ6WpkSK\ndGBbVuwksAIHSC4S+MaGZfvOyIUEw7BhwDYQw8jF/yax4QsrDhIxNiRZFI9aUVyRyz2fZ+c8Pd3T\n53PXORfFKk7Pzi4pR/pHdvoBCJLdPdVff/V97/e+z/O+bwm2bduMMMIII4wwwggjjDDCCP9XIf7f\nHsAII4wwwggjjDDCCCOMMHLMRxhhhBFGGGGEEUYY4acCI8d8hBFGGGGEEUYYYYQRfgowcsxHGGGE\nEUYYYYQRRhjhpwAjx3yEEUYYYYQRRhhhhBF+CjByzEcYYYQRRhhhhBFGGOGnACPHfIQRRhhhhBFG\nGGGEEX4K8GNzzP/wD/+Qs2fPEo/HGRsb40tf+hI3btx44HNf+9rXmJycJBQK8dJLL3Hz5s0f1xBG\nGGGEEUYYYYQRRhjhHy1+bI75O++8w2/8xm/wgx/8gDfffBOfz8crr7xCvV73PvNHf/RH/Omf/il/\n/ud/zgcffMDY2BivvvoqnU7nxzWMEUYYYYQRRhhhhBFG+EcJ4Sf15M9ut0s8Hue1117jF37hF7Bt\nm4mJCX7zN3+T3/3d3wVgMBgwNjbGH//xH/Nrv/ZrP4lhjDDCCCOMMMIII4wwwj8K/MRyzFutFpZl\nkUwmAVhdXaVUKvGzP/uz3mcCgQAvvvgi77333k9qGCOMMMIII4wwwggjjPCPAj8xx/wrX/kKTzzx\nBM899xwAxWIRgFwuN/S5sbEx770RRhhhhBFGGGGEEUb4fxW+n8RFf+d3fof33nuP8+fPIwjCp35+\n/2eazeZPYlgjjDDCCCOMMMIII4zw/wvi8fiP/Dc/dsb8t3/7t/n617/Om2++yaFDh7zX8/k8AKVS\naejzpVLJe2+EEUYYYYQRRhhhhBH+X8WP1TH/yle+4jnlR44cGXpvbm6OfD7Pd77zHe+1wWDA+fPn\nef7553+cwxhhhBFGGGGEEUYYYYR/dPixpbL8+q//On/1V3/FN77xDeLxuJc3Ho1GCYfDCILAb/3W\nb/EHf/AHHD16lMXFRX7/93+faDTKr/7qrz70uvF4HN3UeWvtLQSclBcbm5cOvYQsyeimznpzHQDd\n1NlqbSFLsvf/c8k5DqcOP3Lse68/0AfcKN/gaPooxW7Ru5YkSPzK8V8h5A8Nfb7QLrDV3EKSJOqD\nOiklRT6ax7AMyt0yp3Kn8Ek+dFMHAQQESp0SxU6R49njRAPRA8e6XFtmtb7qfX9f71PqlMiFc1wu\nXUZA4FT+FDY22Hif2zs3Lm6Vb/GN299ARGS5toxpm5ybOYckSKRDaW5XbiMIAoZpkAllsLGp9qoI\nCNyp3iERSJAL53g89zivLrzKenOd1foqu91ddto7IEA+kicXzj0w33vvz2x8FoB31t5hu73NZHSS\nYrfIVnOL6fg0uXDOe/1zhz6HLMkPzMNafQ1BEJhNzGKYBh8UPiAbyuITfQfO6VRsitXGKoVWgduV\n2yQCCc6Mn8EWnHkTEPju6ndp9BvEA3G+t/49bGxigRhBKcjh5GGenHiSc7PnmI3Pops6/+3Gf0MQ\nBMZCY0iSRKwawyf6SMwnhsb6Wdffp61J975Ox6ZZa6xxffc6lm1xp3KHdDDNy3MvI0mSd997Wo//\ndOE/Ue1VqffriKLIvzz2L5FE6cDxuPdorb7GWmONhtrAJ/pQdZVStwQ2SKJEJpTBsAwkUeLnF3/e\n22sDfcBfXvlLmoMms4lZ+kafgTng5xZ+jn9/5t9737NcW+Ze5R7Xy9cREDAsg1wkx5ePf5nzm+ed\n10yDUrfEM1PPMBufpdApsFZfQ7d0gnIQgPXGOrZtcyh5iLXGGu+uvctYeIxMOINhGvzzo/+cY9lj\nQ79x7zoyLIMPtz9kPDrOWGiMy6XLnMieYLe3y83yTT5/6PNUe1V2OjucnTjr7d8f9V4etPa/tfwt\n3l17FwGBiBJBEiTOjJ/BJ/kO3LsHXWe5tsyFrQv4JB/5SB7btn+ksemmzt+d/zsAvvjCF4fW697v\n2fv6W2tvUWgVKHaKSKLEE/kn2GpteXsRoK/1EQQBn+hjMjbJbHyWv7nxN1zfvU4+6qii281tRFHk\nC3NfAOBK6QqZUIap+BTXStfQDI36oE4mlOH42HGu717ndO70I+fHhbvu3T1jWAZfWvoSIX/Im/+9\ne0q3dOYScwCsNlaRRZnt1jY7nR2eyD/B5eJl7lbvkgqmyIQynMyf5NX5T+zfZ9nnnzanB51pANeK\n1/jq21+l1C1R6VbwST5ePvQyk7FJTuVPeXvhwuYFip0ifbPvnS1drUsulKMyqCCKImOhMdLBNCdy\nJ9hqbvHuxrtE/BFaaguA56efZ72+ji3YtNU2yUCSI5kjWLbFLx39JY5ljz0wXvc3A0P7qtgp8uzU\ns95c7F//+3/zMxPP8J8//M8ICJiWyf36fX752C8T8AWG5uRv3vobrtevMz8/781tOpDmWvkajUGD\neq+OLdi8MPMCE9EJ7348bP7/IdhrJ3VTJ+gPeq9PxabYbG0eeD97Wo+/vPKXfFj4kJAvhCiKHB87\nzr95/N884E8ctBY+DT2tx9dvfB0BgVwk59kDWZKZiEzw9vrb7HZ2AUgFUyymF5El+YH5OGgcL0y/\n4NnmzzK2oTn62GZvt7bZbG4yFZ9iKjaFbuoICNjYyJI89H4+kmerucVUbIrnp5+n0CkAMBGZ4L3N\n99hqbTEVn8InfmKTZ+OzD6zNXDjH9d3rGJbBWGSMqHKwn/Ww377V2sKyLer9Oj7Jh2EZZIIZXph9\n4TP5k71O7zPdu4dB+trXvva1/6MrfIxf/MVfRNd1/uIv/oI/+ZM/8f5RFIXPfe5zAJw7dw5VVfm9\n3/s9/uzP/oxkMslf//VfMzk5OXQtVVW9/w4EAkiixEx8BkEQSAaTnM6f9pzyt9beojFo0Bg0qPar\nWJYFgGVb2Niczp9GEqWh6+umzmpjlVKnRKVX4VrpmudwXN+9jmEaXNi+wGZ7k0KrwHZ7G0mQ2Ght\ncGLsBLIke+PRDI1Cu4AkSmw3tyl1SzQHTar9Kj7Rx73aPQK+AC21RV/vkw1niSgRCu0ClX6FfCSP\nKIhYtkUymCQVTAFQ69doDBpIooRhGVzauURX6yKIAgNjgCiKSKJETIkxm5glHUoPzc1eJAIJulqX\n3d4uFhbTsWkeyz5G0Bdkt7fLfHIe27YJ+8OcnTiLIAhUehVs26bcK6OZGkcyR4gGoiwkF2iqTRqD\nBhF/hJ3ODpqh0dE6aKZGLpyjrbWJ+qNYtjV0f+5V77HSWMGyLZZry2y3t1Ekha7e5Uj6CD7JR1gO\nMxWboq21qfVr6KZOS21591DxKXS0DmF/GIBsKMvJ/EnmknO8Mv8KhU4By7a8+39m/AwLyQWSwSSn\n8qd4LPsY2UiWmBKjq3dRZIWAFKCttxERaWtt+nofxacgiRJ9vU88ECcoB7lXvcdyfZm22ma3u4th\nG3xx8YtUd6sALB1aYrWxOvT9j1p/tX6NqD/6wPvgOAqNQQNZkpFECcu2aKttREFkPDrObmcXy7Y4\nmj2KbjlzFJSDZMNZ1pvr+EU/ESVCV++SCWWo9CpYtsWzU88eaIwbgwaFdoFC2zGECM4emk3Mkg1n\nAfBJPiRR4mTuJANjgIXFVnOL/3Hnf6CaKmv1NeeQFmU0S2MxvTj0+2v9Gi21xXhkHEEQUHwKE9EJ\nx5A3nKDgxu4NdFOn1q/x5tqb+EU/lm1xuXSZTCgDQEttkYvkkCWZ5qDJam0VzdawbRuf5HPu8cdj\ndrF3PxXaBVpqy9lvoRQJJcGN8g36Rh9FUqgP6iykFih1StjYhOXwj3wvdVPn9ZXXuVW+xUZzg1K3\nhGZo/K87/8uzVfdr9wnIAWcMwQSWbSEIgmcD9t6fSrfC7cptPtr5CN3U6Rt9JiIT+CTfA7bj0yCJ\nEt1Kl7g/zvTU9AProDFosNpYZSY+gyRK3lqMB+KUuiVMy8TGZiwyRlh29qFmaFwuXfbsWVNtspBa\nICgH2WhuYGNzt3KXYrdIXInT0TqUeiWq/SqVXoX15joBXwBRFAn6gt5aeXzsccJK2NsD++dnP5KB\nJB2tw+HUYf71iX/NVHyKVDCFJEpD50jEHyEZSCJLMpZt0dW6yJJMR+/QVttUe1VEQSQVTBFX4uQi\nOU6MnSAXyQ2tJWBo/nVT5271LleKV+hoHS4XL9NSW96cjkfGWW+u01SbHM8exyf5DrTbqWAKn+jj\nRvkGcSXOeGScVDDFrz/161wpXaGpNlFEhWK3iF/0I0kSPa1HuVsmIAW437jvrDlTo9QtkYvkmIhN\nOGtcVMhFcnz+0Oc5mjmKZmqYmMzGZumbfZpqk3wkz+n8ac5Onh1aA7IkY9s2HxU/ojlwasEsLCYi\nEyzXlsmH83T0DquNVeYSc2TDWW/+D7JphXaBsfAYsiSTCCY4kT1BY9AgpsQ4N32OgBwAoF1uU9Nq\n1KjR0Tp0tA6ZSIax0BhdvYuFxWR0kqOZoximQVttY2GRCCSGxvB/AkmUSAVTWFhDZ5Jrm4Gh3+au\n1eXaMqVOiYnYBA21gSRIZMNZmmqTmfjMkD/xsDP8UZAlmRNjJ4gH4qRDaZ4cf5JcJOd997tr7zIw\nB/SMHqVOiaOZoyxllh6YD/f+CILg+TERJcKZ8TOfaWz7z5Ll2jL5aJ6IHGG7vU1YDhP2O7Z0Kjbl\nzWFbbdNSW8QCMRKBBGG/4wNcL1+nMWhQ6VV47c5rGJbBbnfXW88AyWDS80fctfnBzLine truncated
"text/plain": [
"-1.88266670220060"
"<matplotlib.figure.Figure at 0x8796fd0>"
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
"output_type": "display_data"
}
],
"source": [
"from sympy import *\n",
"from sympy.abc import x\n",
"\n",
"\n",
"a = cos(4*(x/2 + 0.7)) * sin(0.3*x) - 1.6*x\n",
"da = diff(a,x)\n",
"da.subs(x, 0)"
"seed(3) \n",
"run_pf1(N=5000, plot_particles=True, ylim=(-20, 20))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here the initial sample of points did not generate any points near the robot. The particle filter does not create new points during the resample operation, so it ends up duplicating points which are not a representative sample of the probability distribution. The problem quickly spirals out of control. The particles are not a good match for the landscape measurement so they become dispersed in a highly nonlinear, curved distribution, and the particle filter diverges from reality. No particles are available near the robot, so it cannot ever converge.\n",
"\n",
"Let's make use of the `create_gaussian_particles()` method to try to generate more points near the robot. We can do this by using the `initial_x` parameter to specify a location to create the particles."
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {
"collapsed": true
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"final position error, variance: [ 17.92304664 18.07173658] [ 0.00451171 0.00429415]\n"
]
},
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAtsAAAEWCAYAAAC37ltfAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x887ad30>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"objects in a two or three dimension space. via computer vision, radar, or other sensing technique. There might be zero, one, or multiple objects in view at once. We may or may not be able to mathematically describe the behavior of the tracked objects. If we are tracking passenger jets with radar we can model the behavior because large aircraft do not change direction or speed rapidly. If we are tracking people in a crowd, a ball on a football field, or fighter jets our prediction model will range from difficult to impossible. Finally, our measurements are likely to have very strong outliers. A bird flying in front of a camera tracking a sporting event is quite likely to fool the computer vision by creating spurious detections - the bird will be misidentified as a player or the ball. In radar design the designer has to account for noise - either domestic noise or hostile jamming in the case of war environments.\n",
"\n",
"So our problem is multi-modal, has a non-Gaussian to nonexistent process model, and has non-Gaussian measurements. Finally, both the measurements and behavior of the tracked objects are likely to be nonlinear.\n",
"\n",
"None of the filters that we have learned so far are particularly good at solving this problem. \n",
"The Kalman filter provides optimal output if and only if the data and object behavior is Gaussian and linear. The extended Kalman filter (EKF) and unscented Kalman filter (UKF) \n",
"\n",
"The discrete Bayes filter is multimodal and can handle nonlinear \n",
"\n",
"The filters that we have encountered so far is this book are very powerful, but they have significant limitations and difficulties. The discrete Bayes filter can track multiple objects at once because it is multi-modal, but it does not model the behavior of the tracked objects and it requires vast amounts of compute power and memory to represent large problems. The linear Kalman filter assumes that both the model and the measurements are Gaussian, and that both are linear. Of course, the world is not linear and data is rarely truly Gaussian. If these things are true then the Kalman filter produces an optimal solution, but in practice this is rare. Nonlinear Kalman filters, such as the unscented Kalman filter and extended Kalman filter deal with the nonlinearities in the process model and measurements by linearizing the problem. This allows the Kalman filter to work in nonlinear domains, but the linearization introduces inaccuracies that, in the worst case, causes the filter to diverge. There are filters which we haven't described in this book that are designed to handle data which is not fully Gaussian, but they again involve compromises.\n",
"\n",
"Particle filtering is a technique that largely transcends these problems. As we will see it is still non-optimal, but "
"seed(3) \n",
"run_pf1(N=5000, plot_particles=True, initial_x=(1,1, np.pi/4))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## How the Particle Filter Works"
"This works great. You should always try to create particles near the initial position if you have any way to roughly estimate it. Do not be *too* careful - if you generate all the points very near a single position the particles may not be dispersed enough to capture the nonlinearities in the system. This is a fairly linear system, so we could get away with a smaller variance in the distribution. Clearly this depends on your problem. Increasing the number of particles is always a good way to get a better sample, but the processing cost may be a higher price than you are willing to pay."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Assume that we are trying to track objects in a two dimensional space. The particle filter starts by creating a random set of *particles*. Each particle is assigned a random location and velocity. Each particle represents one possible "
"## Resampling Methods\n",
"\n",
"How we resample the particles effects the performance of the filter. For example, suppose we resampled particles by generating a random number from 1 to $N$ and used that as the index of the particle we resample. This would lead us to choosing many particles with a very low weight, and the resulting set of particles would be a terrible representation of the problem's probability distribution. \n",
"\n",
"There is plenty of room for research and novel algorithms, but research and industry have settled on handful of algorithms that work well in practice. We desire an algorithm that has several properties. It should preferentially select particles that have a higher probability. It should select a representative population of the higher probability particles. It should include enough lower probability particles to give the filter a chance of detecting strongly nonlinear behavior. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
}
],
"metadata": {
code/RobotLocalizationParticleFilter.py
+263
View File
@@ -0,0 +1,263 @@
# -*- coding: utf-8 -*-
"""
Created on Sat Jun 27 19:45:45 2015
@author: Roger
"""
import numpy as np
import numpy.random
from numpy.random import randn, random, uniform
import scipy.stats
class RobotLocalizationParticleFilter(object):
def __init__(self, N, x_dim, y_dim, landmarks, measure_std_error):
self.particles = np.empty((N, 3)) # x, y, heading
self.N = N
self.x_dim = x_dim
self.y_dim = y_dim
self.landmarks = landmarks
self.R = measure_std_error
# distribute particles randomly with uniform weight
self.weights = np.empty(N)
#self.weights.fill(1./N)
self.particles[:, 0] = uniform(0, x_dim, size=N)
self.particles[:, 1] = uniform(0, y_dim, size=N)
self.particles[:, 2] = uniform(0, 2*np.pi, size=N)
def create_uniform_particles(self, x_range, y_range, hdg_range):
self.particles[:, 0] = uniform(x_range[0], x_range[1], size=N)
self.particles[:, 1] = uniform(y_range[0], y_range[1], size=N)
self.particles[:, 2] = uniform(hdg_range[0], hdg_range[1], size=N)
self.particles[:, 2] %= 2 * np.pi
def create_gaussian_particles(self, mean, var):
self.particles[:, 0] = mean[0] + randn(self.N)*var[0]
self.particles[:, 1] = mean[1] + randn(self.N)*var[1]
self.particles[:, 2] = mean[2] + randn(self.N)*var[2]
self.particles[:, 2] %= 2 * np.pi
def predict(self, u, std, dt=1.):
""" move according to control input u (heading change, velocity)
with noise std"""
self.particles[:, 2] += u[0] + randn(self.N) * std[0]
self.particles[:, 2] %= 2 * np.pi
d = u[1]*dt + randn(self.N) * std[1]
self.particles[:, 0] += np.cos(self.particles[:, 2]) * d
self.particles[:, 1] += np.sin(self.particles[:, 2]) * d
def update(self, z):
self.weights.fill(1.)
for i, landmark in enumerate(self.landmarks):
distance = np.linalg.norm(self.particles[:, 0:2] - landmark, axis=1)
self.weights *= scipy.stats.norm(distance, self.R).pdf(z[i])
#self.weights *= Gaussian(distance, self.R, z[i])
self.weights += 1.e-300
self.weights /= sum(self.weights) # normalize
def neff(self):
return 1. / np.sum(np.square(self.weights))
def resample(self):
p = np.zeros((self.N, 3))
w = np.zeros(self.N)
cumsum = np.cumsum(self.weights)
for i in range(self.N):
index = np.searchsorted(cumsum, random())
p[i] = self.particles[index]
w[i] = self.weights[index]
self.particles = p
assert np.sum(w) != 0
assert w[0] is not np.nan
self.weights = w / np.sum(w)
def resample_from_index(self, indexes):
assert len(indexes) == self.N
p = np.zeros((self.N, 3))
w = np.zeros(self.N)
for i, index in enumerate(indexes):
p[i] = self.particles[index]
w[i] = self.weights[index]
self.particles = p
self.weights = w / np.sum(w)
def estimate(self):
""" returns mean and variance """
pos = self.particles[:, 0:2]
mu = np.average(pos, weights=self.weights, axis=0)
var = np.average((pos - mu)**2, weights=self.weights, axis=0)
return mu, var
def mean(self):
""" returns weighted mean position"""
return np.average(self.particles[:, 0:2], weights=self.weights, axis=0)
def residual_resample(w):
N = len(w)
w_ints = np.floor(N*w).astype(int)
residual = w - w_ints
residual /= sum(residual)
indexes = np.zeros(N, 'i')
k = 0
for i in range(N):
for j in range(w_ints[i]):
indexes[k] = i
k += 1
cumsum = np.cumsum(residual)
cumsum[N-1] = 1.
for j in range(k, N):
indexes[j] = np.searchsorted(cumsum, random())
return indexes
def residual_resample2(w):
N = len(w)
w_ints =np.floor(N*w).astype(int)
R = np.sum(w_ints)
m_rdn = N - R
Ws = (N*w - w_ints)/ m_rdn
indexes = np.zeros(N, 'i')
i = 0
for j in range(N):
for k in range(w_ints[j]):
indexes[i] = j
i += 1
cumsum = np.cumsum(Ws)
cumsum[N-1] = 1 # just in case
for j in range(i, N):
indexes[j] = np.searchsorted(cumsum, random())
return indexes
def systemic_resample(w):
N = len(w)
Q = np.cumsum(w)
indexes = np.zeros(N)
t = np.linspace(0, 1-1/N, N) + random()/N
i, j = 0, 0
while i < N and j < N:
while Q[j] < t[i]:
j += 1
indexes[i] = j
i += 1
return indexes
def Gaussian(mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
g = (np.exp(-((mu - x) ** 2) / (sigma ** 2) / 2.0) /
np.sqrt(2.0 * np.pi * (sigma ** 2)))
for i in range(len(g)):
g[i] = max(g[i], 1.e-229)
return g
if __name__ == '__main__':
DO_PLOT_PARTICLES = False
from numpy.random import seed
import matplotlib.pyplot as plt
#plt.figure()
seed(5)
for count in range(1):
print()
print(count)
#numpy.random.set_state(fail_state)
#if count == 12:
# #fail_state = numpy.random.get_state()
# DO_PLOT_PARTICLES = True
N = 4000
sensor_std_err = .1
landmarks = np.array([[-1, 2], [2,4], [10,6], [18,25]])
NL = len(landmarks)
#landmarks = [[-1, 2], [2,4]]
pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, sensor_std_err)
#pf.create_gaussian_particles([3, 2, 0], [5, 5, 2])
pf.create_uniform_particles((0,20), (0,20), (0, 6.28))
if DO_PLOT_PARTICLES:
plt.scatter(pf.particles[:, 0], pf.particles[:, 1], alpha=.2, color='g')
xs = []
for x in range(18):
zs = []
pos=(x+1, x+1)
for landmark in landmarks:
d = np.sqrt((landmark[0]-pos[0])**2 + (landmark[1]-pos[1])**2)
zs.append(d + randn()*sensor_std_err)
zs = np.linalg.norm(landmarks - pos, axis=1) + randn(NL)*sensor_std_err
# move diagonally forward to (x+1, x+1)
pf.predict((0.00, 1.414), (.2, .05))
pf.update(z=zs)
if x == 0:
print(max(pf.weights))
#while abs(pf.neff() -N) < .1:
# print('neffing')
# pf.create_uniform_particles((0,20), (0,20), (0, 6.28))
# pf.update(z=zs)
#print(pf.neff())
#indexes = residual_resample2(pf.weights)
indexes = systemic_resample(pf.weights)
pf.resample_from_index(indexes)
#pf.resample()
mu, var = pf.estimate()
xs.append(mu)
if DO_PLOT_PARTICLES:
plt.scatter(pf.particles[:, 0], pf.particles[:, 1], alpha=.2)
plt.scatter(pos[0], pos[1], marker='*', color='r')
plt.scatter(mu[0], mu[1], marker='s', color='r')
plt.pause(.01)
xs = np.array(xs)
plt.plot(xs[:, 0], xs[:, 1])
plt.show()
code/book_plots.py
-4
View File
@@ -153,10 +153,6 @@ def hinton(W, maxweight=None):
if reenable:
plt.ion()
if __name__ == "__main__":
hinton(np.random.randn(20, 20))
plt.title('Example Hinton diagram - 20x20 random normal')
plt.show()
if __name__ == "__main__":
p = [0.2245871, 0.06288015, 0.06109133, 0.0581008, 0.09334062, 0.2245871,
code/pf_internal.py
+145 -81
View File
@@ -1,62 +1,10 @@
import numpy as np
import pylab as plt
from matplotlib.patches import Circle, Rectangle, Polygon, Arrow, FancyArrow
import book_plots
import numpy as np
from numpy.random import randn, random, uniform, multivariate_normal, seed
from nonlinear_plots import plot_monte_carlo_mean
import scipy
#from nonlinear_plots import plot_monte_carlo_mean
import pylab as plt
import scipy.stats
from RobotLocalizationParticleFilter import *
def plot_random_pd():
def norm(x, x0, sigma):
return np.exp(-0.5 * (x - x0) ** 2 / sigma ** 2)
def sigmoid(x, x0, alpha):
return 1. / (1. + np.exp(- (x - x0) / alpha))
x = np.linspace(0, 1, 100)
y2 = (0.1 * np.sin(norm(x, 0.2, 0.05)) + 0.25 * norm(x, 0.6, 0.05) +
.5*norm(x, .5, .08) +
np.sqrt(norm(x, 0.8, 0.06)) +0.1 * (1 - sigmoid(x, 0.45, 0.15)))
with plt.xkcd():
#plt.setp(plt.gca().get_xticklabels(), visible=False)
#plt.setp(plt.gca().get_yticklabels(), visible=False)
plt.axes(xticks=[], yticks=[], frameon=False)
plt.plot(x, y2)
def plot_monte_carlo_ukf():
def f(x,y):
return x+y, .1*x**2 + y*y
mean = (0, 0)
p = np.array([[32, 15], [15., 40.]])
# Compute linearized mean
mean_fx = f(*mean)
#generate random points
xs, ys = multivariate_normal(mean=mean, cov=p, size=3000).T
fxs, fys = f(xs, ys)
plt.subplot(121)
plt.gca().grid(b=False)
plt.scatter(xs, ys, marker='.', alpha=.2, color='k')
plt.xlim(-25, 25)
plt.ylim(-25, 25)
plt.subplot(122)
plt.gca().grid(b=False)
plt.scatter(fxs, fys, marker='.', alpha=0.2, color='k')
plt.ylim([-10, 200])
plt.xlim([-100, 100])
plt.show()
@@ -76,30 +24,6 @@ class ParticleFilter(object):
self.particles[:, 2] = uniform(0, 2*np.pi, size=N)
def create_particles(self, mean, variance):
""" create particles with the specified mean and variance"""
self.particles[:, 0] = mean[0] + randn(self.N) * np.sqrt(variance)
self.particles[:, 1] = mean[1] + randn(self.N) * np.sqrt(variance)
def create_particle(self):
""" create particles uniformly distributed over entire space"""
return [uniform(0, self.x_dim), uniform(0, self.y_dim), 0, 0]
'''def assign_speed_by_gaussian(self, speed, var):
""" move every particle by the specified speed (assuming time=1.)
with the specified variance, assuming Gaussian distribution. """
self.particles[:, 2] = np.random.normal(speed, var, self.N)'''
def control(self, dx):
self.particles[:, 0] += dx[0]
self.particles[:, 1] += dx[1]
self.particles[:, 1] = (self.particles[:, 1] + vy*dt)
def predict(self, u, std):
""" move according to control input u with noise std"""
@@ -158,6 +82,57 @@ class ParticleFilter(object):
def plot_random_pd():
def norm(x, x0, sigma):
return np.exp(-0.5 * (x - x0) ** 2 / sigma ** 2)
def sigmoid(x, x0, alpha):
return 1. / (1. + np.exp(- (x - x0) / alpha))
x = np.linspace(0, 1, 100)
y2 = (0.1 * np.sin(norm(x, 0.2, 0.05)) + 0.25 * norm(x, 0.6, 0.05) +
.5*norm(x, .5, .08) +
np.sqrt(norm(x, 0.8, 0.06)) +0.1 * (1 - sigmoid(x, 0.45, 0.15)))
with plt.xkcd():
#plt.setp(plt.gca().get_xticklabels(), visible=False)
#plt.setp(plt.gca().get_yticklabels(), visible=False)
plt.axes(xticks=[], yticks=[], frameon=False)
plt.plot(x, y2)
def plot_monte_carlo_ukf():
def f(x,y):
return x+y, .1*x**2 + y*y
mean = (0, 0)
p = np.array([[32, 15], [15., 40.]])
# Compute linearized mean
mean_fx = f(*mean)
#generate random points
xs, ys = multivariate_normal(mean=mean, cov=p, size=3000).T
fxs, fys = f(xs, ys)
plt.subplot(121)
plt.gca().grid(b=False)
plt.scatter(xs, ys, marker='.', alpha=.2, color='k')
plt.xlim(-25, 25)
plt.ylim(-25, 25)
plt.subplot(122)
plt.gca().grid(b=False)
plt.scatter(fxs, fys, marker='.', alpha=0.2, color='k')
plt.ylim([-10, 200])
plt.xlim([-100, 100])
plt.show()
def plot_pf(pf, xlim=100, ylim=100, weights=True):
@@ -189,6 +164,26 @@ def plot_pf(pf, xlim=100, ylim=100, weights=True):
plt.ylim(0, ylim)
def Gaussian(mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
g = (np.exp(-((mu - x) ** 2) / (sigma ** 2) / 2.0) /
np.sqrt(2.0 * np.pi * (sigma ** 2)))
for i in range(len(g)):
g[i] = max(g[i], 1.e-29)
return g
def test_gaussian(N):
for i in range(N):
mean, std, x = randn(3)
std = abs(std)
d = Gaussian(mean, std, x) - scipy.stats.norm(mean, std).pdf(x)
assert abs(d) < 1.e-8, "{}, {}, {}, {}, {}, {}".format(d, mean, std, x, Gaussian(mean, std, x), scipy.stats.norm(mean, std).pdf(x))
def show_two_pf_plots():
""" Displays results of PF after 1 and 10 iterations for the book.
Note the book says this solves the full robot localization problem.
@@ -229,5 +224,74 @@ def show_two_pf_plots():
plt.tight_layout()
def test_pf():
#seed(1234)
N = 10000
R = .2
landmarks = [[-1, 2], [20,4], [10,30], [18,25]]
#landmarks = [[-1, 2], [2,4]]
pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, R)
plot_pf(pf, 20, 20, weights=False)
dt = .01
plt.pause(dt)
for x in range(18):
zs = []
pos=(x+3, x+3)
for landmark in landmarks:
d = np.sqrt((landmark[0]-pos[0])**2 + (landmark[1]-pos[1])**2)
zs.append(d + randn()*R)
pf.predict((0.01, 1.414), (.2, .05))
pf.update(z=zs)
pf.resample()
#print(x, np.array(list(zip(pf.particles, pf.weights))))
mu, var = pf.estimate()
plot_pf(pf, 20, 20, weights=False)
plt.plot(pos[0], pos[1], marker='*', color='r', ms=10)
plt.scatter(mu[0], mu[1], color='g', s=100)
plt.tight_layout()
plt.pause(dt)
#print(mu - pos)
def test_pf2():
N = 1000
sensor_std_err = .2
landmarks = [[-1, 2], [20,4], [-20,6], [18,25]]
pf = RobotLocalizationParticleFilter(N, 20, 20, landmarks, sensor_std_err)
xs = []
for x in range(18):
zs = []
pos=(x+1, x+1)
for landmark in landmarks:
d = np.sqrt((landmark[0]-pos[0])**2 + (landmark[1]-pos[1])**2)
zs.append(d + randn()*sensor_std_err)
# move diagonally forward to (x+1, x+1)
pf.predict((0.00, 1.414), (.2, .05))
pf.update(z=zs)
pf.resample()
mu, var = pf.estimate()
xs.append(mu)
xs = np.array(xs)
plt.plot(xs[:, 0], xs[:, 1])
plt.show()
if __name__ == '__main__':
show_two_pf_plots()
#show_two_pf_plots()
test_pf()