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

Added animation to PF chapter.

Browse Source
This commit is contained in:
Roger Labbe committed 2015-12-10 18:13:41 -08:00
1 parent 41d8d246e0
commit 5794485341
3 files changed
+538 -445
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

animations/particle_animate.ipynb
+447 -342
View File
@@ -1,342 +1,447 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
"<style>\n",
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
"@import url('http://fonts.googleapis.com/css?family=Vollkorn');\n",
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"\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: 16pt;\n",
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: inline;\n",
" white-space: wrap;\n",
" } \n",
" h3 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h3 {\n",
" font-weight: 200;\n",
" font-size: 14pt;\n",
" line-height: 100%;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 2em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" h4 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h4 {\n",
" font-weight: 100;\n",
" font-size: 14pt;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" h5 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h5 {\n",
" font-weight: 200;\n",
" font-style: normal;\n",
" color: #1d3b84;\n",
" font-size: 16pt;\n",
" margin-bottom: 0em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Arimo',verdana,arial,sans-serif;\n",
" line-height: 125%;\n",
" font-size: 120%;\n",
" width:740px;\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",
" div.output_wrapper{\n",
" margin-top:0.2em;\n",
" margin-bottom:0.2em;\n",
"}\n",
"\n",
" code{\n",
" font-size: 70%;\n",
" }\n",
" .rendered_html code{\n",
" background-color: transparent;\n",
" }\n",
" ul{\n",
" margin: 2em;\n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li li{\n",
" padding-left: 0.2em; \n",
" margin-bottom: 0.2em; \n",
" margin-top: 0.2em; \n",
" }\n",
" ol{\n",
" margin: 2em;\n",
" }\n",
" ol li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.5em; \n",
" }\n",
" ul li{\n",
" padding-left: 0.5em; \n",
" margin-bottom: 0.5em; \n",
" margin-top: 0.2em; \n",
" }\n",
" a:link{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:visited{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:hover{\n",
" font-weight: bold;\n",
" color: #1d3b84;\n",
" }\n",
" a:focus{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" a:active{\n",
" font-weight: bold;\n",
" color:#447adb;\n",
" }\n",
" .rendered_html :link {\n",
" text-decoration: underline; \n",
" }\n",
" .rendered_html :hover {\n",
" text-decoration: none; \n",
" }\n",
" .rendered_html :visited {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :focus {\n",
" text-decoration: none;\n",
" }\n",
" .rendered_html :active {\n",
" text-decoration: none;\n",
" }\n",
" .warning{\n",
" color: rgb( 240, 20, 20 )\n",
" } \n",
" hr {\n",
" color: #f3f3f3;\n",
" background-color: #f3f3f3;\n",
" height: 1px;\n",
" }\n",
" blockquote{\n",
" display:block;\n",
" background: #fcfcfc;\n",
" border-left: 5px solid #c76c0c;\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" width:680px;\n",
" padding: 10px 10px 10px 10px;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" blockquote p {\n",
" margin-bottom: 0;\n",
" line-height: 125%;\n",
" font-size: 100%;\n",
" }\n",
"</style>\n",
"<script>\n",
" MathJax.Hub.Config({\n",
" TeX: {\n",
" extensions: [\"AMSmath.js\"]\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
" },\n",
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
" \"HTML-CSS\": {\n",
" scale:85,\n",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 1,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"from __future__ import division, print_function\n",
"%matplotlib inline\n",
"import sys\n",
"sys.path.insert(0,'..') # allow us to format the book\n",
"sys.path.insert(0,'../code') \n",
"# use same formatting as rest of book so that the plots are\n",
"# consistant with that look and feel.\n",
"import book_format\n",
"book_format.load_style(directory='..')"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAQgAAAD9CAYAAABA3unhAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x5a2cda0>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from random import uniform\n",
"import matplotlib.pylab as pylab\n",
"from matplotlib import animation\n",
"from gif_animate import animate\n",
"import matplotlib as mpl\n",
"from math import pi\n",
"\n",
"def make_particle():\n",
" return [uniform(0,100), uniform(0,100), uniform(0, 2), uniform(0, 2*pi)]\n",
"\n",
"\n",
"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\n",
" \n",
"particles = np.asarray([make_particle() for i in range(1000)])\n",
"\n",
"def panimate(frame):\n",
" move(particles, t=1.)\n",
"\n",
" plt.cla()\n",
" plt.scatter(particles[:, 0], particles[:, 1])\n",
" plt.xlim(0, 100)\n",
" plt.ylim(0, 100)\n",
" plt.gca().get_xaxis().set_ticks([])\n",
" plt.gca().get_yaxis().set_ticks([])\n",
"\n",
" \n",
"animate('13_particle_move.gif', panimate, \n",
" frames=500, interval=50, figsize=(4, 4))"
]
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": [
"<img src='13_particle_move.gif'>"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.4.3"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
{
"cells": [
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
"<style>\n",
"@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');\n",
"@import url('http://fonts.googleapis.com/css?family=Vollkorn');\n",
"@import url('http://fonts.googleapis.com/css?family=Arimo');\n",
"@import url('http://fonts.googleapis.com/css?family=Fira_sans');\n",
"\n",
".CodeMirror pre {\n",
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
"}\n",
" div.cell{\n",
" width: 900px;\n",
" margin-left: 0% !important;\n",
" margin-right: auto;\n",
" }\n",
" div.text_cell code {\n",
" background: transparent;\n",
" color: #000000;\n",
" font-weight: 600;\n",
" font-size: 11pt;\n",
" font-style: bold;\n",
" font-family: 'Source Code Pro', Consolas, monocco, monospace;\n",
" }\n",
" h1 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
"\t}\n",
"\t\n",
" div.input_area {\n",
" background: #F6F6F9;\n",
" border: 1px solid #586e75; \n",
" }\n",
"\n",
" .text_cell_render h1 {\n",
" font-weight: 200;\n",
" font-size: 30pt;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h2 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" text-align: left;\n",
" }\n",
" .text_cell_render h2 {\n",
" font-weight: 200;\n",
" font-size: 16pt;\n",
" font-style: italic;\n",
" line-height: 100%;\n",
" color:#c76c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 1.5em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" } \n",
" h3 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h3 {\n",
" font-weight: 200;\n",
" font-size: 14pt;\n",
" line-height: 100%;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 2em;\n",
" display: block;\n",
" white-space: wrap;\n",
" text-align: left;\n",
" }\n",
" h4 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
" .text_cell_render h4 {\n",
" font-weight: 100;\n",
" font-size: 14pt;\n",
" color:#d77c0c;\n",
" margin-bottom: 0.5em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" h5 {\n",
" font-family: 'Open sans',verdana,arial,sans-serif;\n",
" }\n",
"\n",
" .text_cell_render h5 {\n",
" font-weight: 200;\n",
" font-style: normal;\n",
" color: #1d3b84;\n",
" font-size: 16pt;\n",
" margin-bottom: 0em;\n",
" margin-top: 0.5em;\n",
" display: block;\n",
" white-space: nowrap;\n",
" }\n",
" div.text_cell_render{\n",
" font-family: 'Fira sans', verdana,arial,sans-serif;\n",
" line-height: 150%;\n",
" font-size: 110%;\n",
" font-weight: 400;\n",
" text-align:justify;\n",
" text-justify:inter-word;\n",
" }\n",
" div.output_subarea.output_text.output_pyout {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_subarea.output_stream.output_stdout.output_text {\n",
" overflow-x: auto;\n",
" overflow-y: scroll;\n",
" max-height: 50000px;\n",
" }\n",
" div.output_wrapper{\n",
" margin-top:0.2em;\n",
" margin-bottom:0.2em;\n",
"}\n",
"\n",
" code{\n",
" font-size: 6pt;\n",
"\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",
" equationNumbers: { autoNumber: \"AMS\", useLabelIds: true}\n",
" },\n",
" tex2jax: {\n",
" inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
" displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
" },\n",
" displayAlign: 'center', // Change this to 'center' to center equations.\n",
" \"HTML-CSS\": {\n",
" scale:95,\n",
" availableFonts: [],\n",
" preferredFont:null,\n",
" webFont: \"TeX\",\n",
" styles: {'.MathJax_Display': {\"margin\": 4}}\n",
" }\n",
" });\n",
"</script>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"from __future__ import division, print_function\n",
"%matplotlib inline\n",
"import sys\n",
"sys.path.insert(0,'..') # allow us to format the book\n",
"sys.path.insert(0,'../code') \n",
"# use same formattibng as rest of book so that the plots are\n",
"# consistant with that look and feel.\n",
"import book_format\n",
"book_format.load_style(directory='..')"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"import numpy as np\n",
"from numpy.random import randn, random, uniform, seed\n",
"import scipy.stats\n",
"\n",
"class ParticleFilter(object):\n",
"\n",
" def __init__(self, N, x_dim, y_dim):\n",
" self.particles = np.empty((N, 3)) # x, y, heading\n",
" self.N = N\n",
" self.x_dim = x_dim\n",
" self.y_dim = y_dim\n",
"\n",
" # distribute particles randomly with uniform weight\n",
" self.weights = np.empty(N)\n",
" self.weights.fill(1./N)\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)\n",
"\n",
"\n",
" def predict(self, u, std):\n",
" \"\"\" move according to control input u with noise std\"\"\"\n",
"\n",
" self.particles[:, 2] += u[0] + randn(self.N) * std[0]\n",
" self.particles[:, 2] %= 2 * np.pi\n",
"\n",
" d = u[1] + randn(self.N)\n",
" self.particles[:, 0] += np.cos(self.particles[:, 2]) * d\n",
" self.particles[:, 1] += np.sin(self.particles[:, 2]) * d\n",
"\n",
" self.particles[:, 0:2] += u + randn(self.N, 2) * std\n",
"\n",
"\n",
" def weight(self, z, var):\n",
" dist = np.sqrt((self.particles[:, 0] - z[0])**2 +\n",
" (self.particles[:, 1] - z[1])**2)\n",
"\n",
" # simplification assumes variance is invariant to world projection\n",
" n = scipy.stats.norm(0, np.sqrt(var))\n",
" prob = n.pdf(dist)\n",
"\n",
" # particles far from a measurement will give us 0.0 for a probability\n",
" # due to floating point limits. Once we hit zero we can never recover,\n",
" # so add some small nonzero value to all points.\n",
" prob += 1.e-12\n",
" self.weights += prob\n",
" self.weights /= sum(self.weights) # normalize\n",
"\n",
"\n",
" def neff(self):\n",
" return 1. / np.sum(np.square(self.weights))\n",
"\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",
"\n",
" def estimate(self):\n",
" \"\"\" returns mean and variance \"\"\"\n",
" pos = self.particles[:, 0:2]\n",
" mu = np.average(pos, weights=self.weights, axis=0)\n",
" var = np.average((pos - mu)**2, weights=self.weights, axis=0)\n",
"\n",
" return mu, var"
]
},
{
"cell_type": "code",
"execution_count": 30,
"metadata": {
"collapsed": false,
"scrolled": true
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAARIAAAETCAYAAAD6a4mGAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
"text/plain": [
"<matplotlib.figure.Figure at 0x8a1a908>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"from pf_internal import plot_pf\n",
"\n",
"seed(1234)\n",
"N = 3000\n",
"pf = ParticleFilter(N, 20, 20)\n",
"xs = np.linspace (1, 10, 20)\n",
"ys = np.linspace (1, 10, 20)\n",
"zxs = xs + randn(20)\n",
"zys = xs + randn(20)\n",
"\n",
"def animatepf(i):\n",
" if i == 0:\n",
" plot_pf(pf, 10, 10, weights=False)\n",
" \n",
" idx = int((i-1) / 3)\n",
" x, y = xs[idx], ys[idx]\n",
" z = [x + randn()*0.2, y + randn()*0.2]\n",
"\n",
" step = (i % 3) + 1\n",
" if step == 2:\n",
" pf.predict((0.5, 0.5), (0.2, 0.2))\n",
" pf.weight(z=z, var=.6)\n",
" plot_pf(pf, 10, 10, weights=False)\n",
" plt.title('Step {}: Predict'.format(idx+1))\n",
" elif step == 3:\n",
" pf.resample()\n",
" plot_pf(pf, 10, 10, weights=False)\n",
" plt.title('Step {}: Resample'.format(idx+1))\n",
"\n",
" else:\n",
" mu, var = pf.estimate()\n",
" plot_pf(pf, 10, 10, weights=False)\n",
" plt.scatter(mu[0], mu[1], color='g', s=100, label='PF')\n",
" plt.scatter(x, y, marker='x', color='r', s=180, lw=3, label='Robot')\n",
" plt.title('Step {}: Estimate'.format(idx+1))\n",
" #plt.scatter(mu[0], mu[1], color='g', s=100, label=\"PF\")\n",
" #plt.scatter([x+1], [x+1], marker='x', color='r', s=180, label=\"True\", lw=3)\n",
" plt.legend(scatterpoints=1, loc=2)\n",
" plt.tight_layout()\n",
"\n",
"from gif_animate import animate\n",
"animate('particle_filter_anim.gif', animatepf, \n",
" frames=40, interval=800, figsize=(4, 4))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<img src='particle_filter_anim.gif'>"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.5.0"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
animations/particle_filter_anim.gif
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 316 KiB