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

Lots of material on resampling.

Browse Source 
Happy 4th of July!
This commit is contained in:
Roger Labbe
2015-07-04 20:12:33 -07:00
parent afe168c7b3
commit f8896bbb80
5 changed files with 727 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
code/RobotLocalizationParticleFilter.py
View File

@@ -70,33 +70,22 @@ class RobotLocalizationParticleFilter(object):
def resample(self):
p = np.zeros((self.N, 3))
w = np.zeros(self.N)
cumulative_sum = np.cumsum(self.weights)
cumulative_sum[-1] = 1. # avoid round-off error
indexes = np.searchsorted(cumulative_sum, random(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)
# resample according to indexes
self.particles = self.particles[indexes]
self.weights = self.weights[indexes]
self.weights /= np.sum(self.weights) # normalize
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)
self.particles = self.particles[indexes]
self.weights = self.weights[indexes]
self.weights /= np.sum(self.weights)
def estimate(self):

164
code/pf_internal.py
View File

@@ -1,3 +1,7 @@
from filterpy.monte_carlo import stratified_resample
import matplotlib as mpl
import matplotlib.pyplot as plt
import numpy as np
from numpy.random import randn, random, uniform, multivariate_normal, seed
#from nonlinear_plots import plot_monte_carlo_mean
@@ -7,7 +11,6 @@ from RobotLocalizationParticleFilter import *
class ParticleFilter(object):
def __init__(self, N, x_dim, y_dim):
@@ -24,7 +27,6 @@ class ParticleFilter(object):
self.particles[:, 2] = uniform(0, 2*np.pi, size=N)
def predict(self, u, std):
""" move according to control input u with noise std"""
@@ -81,7 +83,6 @@ class ParticleFilter(object):
return mu, var
def plot_random_pd():
def norm(x, x0, sigma):
return np.exp(-0.5 * (x - x0) ** 2 / sigma ** 2)
@@ -101,7 +102,6 @@ def plot_random_pd():
plt.plot(x, y2)
def plot_monte_carlo_ukf():
def f(x,y):
@@ -164,7 +164,6 @@ 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
@@ -175,6 +174,7 @@ def Gaussian(mu, sigma, x):
return g
def test_gaussian(N):
for i in range(N):
mean, std, x = randn(3)
@@ -224,8 +224,6 @@ def show_two_pf_plots():
plt.tight_layout()
def test_pf():
#seed(1234)
@@ -261,7 +259,6 @@ def test_pf():
plt.scatter(mu[0], mu[1], color='g', s=100)
plt.tight_layout()
plt.pause(dt)
#print(mu - pos)
def test_pf2():
@@ -292,6 +289,155 @@ def test_pf2():
plt.plot(xs[:, 0], xs[:, 1])
plt.show()
def plot_cumsum(a):
N = len(a)
cmap = mpl.colors.ListedColormap([[0., .4, 1.],
[0., .8, 1.],
[1., .8, 0.],
[1., .4, 0.]]*(int(N/4) + 1))
cumsum = np.cumsum(np.asarray(a) / np.sum(a))
cumsum = np.insert(cumsum, 0, 0)
fig = plt.figure(figsize=(6,3))
ax = fig.add_axes([0.05, 0.475, 0.9, 0.15])
norm = mpl.colors.BoundaryNorm(cumsum, cmap.N)
bar = mpl.colorbar.ColorbarBase(ax, cmap=cmap,
norm=norm,
drawedges=False,
spacing='proportional',
orientation='horizontal')
if N > 10:
bar.set_ticks([])
plt.show()
def plot_stratified_resample(a):
N = len(a)
cmap = mpl.colors.ListedColormap([[0., .4, 1.],
[0., .8, 1.],
[1., .8, 0.],
[1., .4, 0.]]*(int(N/4) + 1))
cumsum = np.cumsum(np.asarray(a) / np.sum(a))
cumsum = np.insert(cumsum, 0, 0)
fig = plt.figure(figsize=(6,3))
ax = fig.add_axes([0.05, 0.475, 0.9, 0.15])
norm = mpl.colors.BoundaryNorm(cumsum, cmap.N)
bar = mpl.colorbar.ColorbarBase(ax, cmap=cmap,
norm=norm,
drawedges=False,
spacing='proportional',
orientation='horizontal')
xs = np.linspace(0., 1.-1./N, N)
ax.vlines(xs, 0, 1, lw=2)
# make N subdivisions, and chose a random position within each one
b = (random(N) + range(N)) / N
plt.scatter(b, [.5]*len(b), s=60, facecolor='k', edgecolor='k')
bar.set_ticks([])
plt.title('stratified resampling')
plt.show()
def plot_systematic_resample(a):
N = len(a)
cmap = mpl.colors.ListedColormap([[0., .4, 1.],
[0., .8, 1.],
[1., .8, 0.],
[1., .4, 0.]]*(int(N/4) + 1))
cumsum = np.cumsum(np.asarray(a) / np.sum(a))
cumsum = np.insert(cumsum, 0, 0)
fig = plt.figure(figsize=(6,3))
ax = fig.add_axes([0.05, 0.475, 0.9, 0.15])
norm = mpl.colors.BoundaryNorm(cumsum, cmap.N)
bar = mpl.colorbar.ColorbarBase(ax, cmap=cmap,
norm=norm,
drawedges=False,
spacing='proportional',
orientation='horizontal')
xs = np.linspace(0., 1.-1./N, N)
ax.vlines(xs, 0, 1, lw=2)
# make N subdivisions, and chose a random position within each one
b = (random() + np.array(range(N))) / N
plt.scatter(b, [.5]*len(b), s=60, facecolor='k', edgecolor='k')
bar.set_ticks([])
plt.title('systematic resampling')
plt.show()
def plot_multinomial_resample(a):
N = len(a)
cmap = mpl.colors.ListedColormap([[0., .4, 1.],
[0., .8, 1.],
[1., .8, 0.],
[1., .4, 0.]]*(int(N/4) + 1))
cumsum = np.cumsum(np.asarray(a) / np.sum(a))
cumsum = np.insert(cumsum, 0, 0)
fig = plt.figure(figsize=(6,3))
ax = fig.add_axes([0.05, 0.475, 0.9, 0.15])
norm = mpl.colors.BoundaryNorm(cumsum, cmap.N)
bar = mpl.colorbar.ColorbarBase(ax, cmap=cmap,
norm=norm,
drawedges=False,
spacing='proportional',
orientation='horizontal')
# make N subdivisions, and chose a random position within each one
b = random(N)
plt.scatter(b, [.5]*len(b), s=60, facecolor='k', edgecolor='k')
bar.set_ticks([])
plt.title('multinomial resampling')
plt.show()
def plot_residual_resample(a):
N = len(a)
a_norm = np.asarray(a) / np.sum(a)
cumsum = np.cumsum(a_norm)
cumsum = np.insert(cumsum, 0, 0)
cmap = mpl.colors.ListedColormap([[0., .4, 1.],
[0., .8, 1.],
[1., .8, 0.],
[1., .4, 0.]]*(int(N/4) + 1))
fig = plt.figure(figsize=(6,3))
ax = fig.add_axes([0.05, 0.475, 0.9, 0.15])
norm = mpl.colors.BoundaryNorm(cumsum, cmap.N)
bar = mpl.colorbar.ColorbarBase(ax, cmap=cmap,
norm=norm,
drawedges=False,
spacing='proportional',
orientation='horizontal')
indexes = residual_resample(a_norm)
bins = np.bincount(indexes)
for i in range(1, N):
n = bins[i-1] # number particles in this sample
if n > 0:
b = np.linspace(cumsum[i-1], cumsum[i], n+2)[1:-1]
plt.scatter(b, [.5]*len(b), s=60, facecolor='k', edgecolor='k')
bar.set_ticks([])
plt.title('residual resampling')
plt.show()
if __name__ == '__main__':
plot_residual_resample([.1, .2, .3, .4, .2, .3, .1])
#example()
#show_two_pf_plots()
test_pf()
a = [.1, .2, .1, .6]
#plot_cumsum(a)
#test_pf()