{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
"\n",
"\n"
],
"text/plain": [
""
]
},
"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,'../kf_book') \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": 2,
"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.fill(1.0 / self.N)\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": 7,
"metadata": {
"collapsed": false,
"scrolled": true
},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAARIAAAETCAYAAAD6a4mGAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nrkJggg==\n",
"text/plain": [
""
]
},
"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": [
""
]
}
],
"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.3"
}
},
"nbformat": 4,
"nbformat_minor": 0
}