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

Added chart of multiple Gaussians

Browse Source
This commit is contained in:
Roger Labbe 2015-06-24 21:00:57 -07:00
parent 46b3be3139
commit 10a62649df
4 changed files with 247 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

216
05_Multivariate_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

37
code/nonlinear_plots.py
View File

@ -6,11 +6,14 @@ Created on Sun May 18 11:09:23 2014
"""
from __future__ import division
import matplotlib.pyplot as plt
import numpy as np
from numpy.random import normal
import scipy.stats
from filterpy.common import multivariate_gaussian
from matplotlib import cm
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
from numpy.random import normal, multivariate_normal
import scipy.stats
def plot_nonlinear_func(data, f, gaussian, num_bins=300):
@ -215,7 +218,7 @@ def plot_bivariate_colormap(xs, ys):
def plot_monte_carlo_mean(xs, ys, f, mean_fx, label):
def plot_monte_carlo_mean(xs, ys, f, mean_fx, label, plot_colormap=True):
fxs, fys = f(xs, ys)
computed_mean_x = np.average(fxs)
@ -225,6 +228,7 @@ def plot_monte_carlo_mean(xs, ys, f, mean_fx, label):
plt.gca().grid(b=False)
plot_bivariate_colormap(xs, ys)
plt.scatter(xs, ys, marker='.', alpha=0.02, color='k')
plt.xlim(-20, 20)
plt.ylim(-20, 20)
@ -237,6 +241,7 @@ def plot_monte_carlo_mean(xs, ys, f, mean_fx, label):
marker='v', s=300, c='r', label='Linearized Mean')
plt.scatter(computed_mean_x, computed_mean_y,
marker='*',s=120, c='r', label='Computed Mean')
plot_bivariate_colormap(fxs, fys)
plt.ylim([-10, 200])
plt.xlim([-100, 100])
@ -261,6 +266,28 @@ def plot_cov_ellipse_colormap(cov=[[1,1],[1,1]]):
def plot_multiple_gaussians(xs, ps, x_range, y_range, N):
""" given a list of 2d states (x,y) and 2x2 covariance matrices, produce
a surface plot showing all of the gaussians"""
xs = np.asarray(xs)
x = np.linspace (x_range[0], x_range[1], N)
y = np.linspace (y_range[0], y_range[1], N)
xx, yy = np.meshgrid(x, y)
zv = np.zeros((N, N))
for mean, cov in zip(xs, ps):
zs = np.array([multivariate_gaussian(np.array([i ,j]), mean, cov)
for i, j in zip(np.ravel(xx), np.ravel(yy))])
zv += zs.reshape(xx.shape)
ax = plt.figure().add_subplot(111, projection='3d')
ax.plot_surface(xx, yy, zv, rstride=1, cstride=1, lw=.5, edgecolors='#191919',
antialiased=True, shade=True, cmap=cm.autumn)
ax.view_init(elev=40., azim=250)
if __name__ == "__main__":
plot_cov_ellipse_colormap(cov=[[2, 1.2], [1.2, 2]])
'''

56
code/particle_filter.py
View File

@ -17,7 +17,7 @@ import random
class ParticleFilter(object):
def __init__(self, N, x_range, y_range):
self.particles = np.zeros((N, 4))
self.particles = np.zeros((N, 3)) # x, y, speed, hdg
self.N = N
self.x_range = x_range
self.y_range = y_range
@ -26,12 +26,11 @@ class ParticleFilter(object):
self.weights = np.array([1./N] * N)
self.particles[:, 0] = uniform(0, x_range, size=N)
self.particles[:, 1] = uniform(0, y_range, size=N)
self.particles[:, 3] = uniform(0, 2*np.pi, size=N)
self.particles[:, 2] = uniform(0, 2*np.pi, size=N)
def create_particles(self, mean, variance):
""" create particles with the specied mean and 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)
@ -40,11 +39,11 @@ class ParticleFilter(object):
return [uniform(0, self.x_range), uniform(0, self.y_range), 0, 0]
def assign_speed_by_gaussian(self, speed, var):
'''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)
self.particles[:, 2] = np.random.normal(speed, var, self.N)'''
def control(self, dx):
self.particles[:, 0] += dx[0]
@ -56,7 +55,7 @@ class ParticleFilter(object):
specified time duration t"""
h = math.atan2(hdg[1], hdg[0])
h = randn(self.N) * .4 + h
vs = vel + randn(self.N) * 0.1
#vs = vel + randn(self.N) * 0.1
vx = vel * np.cos(h)
vy = vel * np.sin(h)
@ -94,7 +93,7 @@ class ParticleFilter(object):
def resample(self):
p = np.zeros((self.N, 4))
p = np.zeros((self.N, 3))
w = np.zeros(self.N)
cumsum = np.cumsum(self.weights)
@ -121,17 +120,24 @@ def plot(pf, xlim=100, ylim=100, weights=True):
if weights:
a = plt.subplot(221)
a.cla()
plt.xlim(0, ylim)
plt.ylim(0, 1)
#plt.ylim(0, 1)
a.set_yticklabels('')
plt.scatter(pf.particles[:, 0], pf.weights, marker='.', s=1)
a.set_ylim(bottom=0)
a = plt.subplot(224)
a.cla()
a.set_xticklabels('')
plt.scatter(pf.weights, pf.particles[:, 1], marker='.', s=1)
plt.ylim(0, xlim)
plt.xlim(0, 1)
a.set_xlim(left=0)
#plt.xlim(0, 1)
a = plt.subplot(223)
a.cla()
else:
plt.cla()
plt.scatter(pf.particles[:, 0], pf.particles[:, 1], marker='.', s=1)
@ -142,41 +148,47 @@ def plot(pf, xlim=100, ylim=100, weights=True):
if __name__ == '__main__':
pf = ParticleFilter(5000, 100, 100)
pf.particles[:,3] = np.random.randn(pf.N)*np.radians(10) + np.radians(45)
pf = ParticleFilter(50000, 100, 100)
pf.particles[:,2] = np.random.randn(pf.N)*np.radians(10) + np.radians(45)
z = np.array([20, 20])
pf.create_particles(mean=z, variance=40)
#pf.create_particles(mean=z, variance=40)
mu0 = np.array([0., 0.])
plot(pf, weights=False)
fig = plt.gcf()
fig.show()
fig.canvas.draw()
for x in range(50):
z[0] += 1.0 + randn()*0.3
z[1] += 1.0 + randn()*0.3
z[0] = x+1 + randn()*0.3
z[1] = x+1 + randn()*0.3
pf.move2((1,1))
pf.weight(z, 5.2)
# pf.weight((z[0] + randn()*0.2, z[1] + randn()*0.2), 5.2)
pf.resample()
pf.weight(z=z, var=.8)
neff = pf.neff()
#print('neff', neff)
if neff < 1000:
pf.resample()
mu, var = pf.estimate()
if x == 0:
mu0 = mu
print(mu - z)
print('neff', pf.neff())
#print(mu - z)
#print(var)
plot(pf, weights=False)
plt.plot(z[0], z[1], marker='v', c='r', ms=10)
plot(pf, weights=True)
#plt.plot(z[0], z[1], marker='v', c='r', ms=10)
plt.plot(x+1, x+1, marker='*', c='r', ms=10)
plt.scatter(mu[0], mu[1], c='g', s=100)#,
#s=min(500, abs((1./np.sum(var)))*20), alpha=0.5)
plt.plot([0,100], [0,100])
plt.tight_layout()
fig.canvas.draw()
#pf.assign_speed_by_gaussian(1, 1.5)

55
code/pf_internal.py Normal file
View File

@ -0,0 +1,55 @@
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 multivariate_normal
from nonlinear_plots import plot_monte_carlo_mean
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) +
np.sqrt(norm(x, 0.8, 0.06)) +0.1 * (1 - sigmoid(x, 0.45, 0.15)))
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()