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

Fixed assignment to F. dt=.1, but used dt=1.

Browse Source
This commit is contained in:
Roger Labbe 2015-07-22 11:13:07 -07:00
parent 364a2357d7
commit 49a464bcfa
4 changed files with 132 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
06-Multivariate-Kalman-Filters.ipynb
View File

File diff suppressed because one or more lines are too long

16
code/book_plots.py
View File

@ -59,7 +59,8 @@ def set_limits(x, y):
plt.gca().set_ylim(y)
def plot_measurements(xs, ys=None, color='r', lw=2, label='Measurements', **kwargs):
def plot_measurements(xs, ys=None, color='k', lw=2, label='Measurements',
lines=False, **kwargs):
""" Helper function to give a consistant way to display
measurements in the book.
"""
@ -76,11 +77,16 @@ def plot_measurements(xs, ys=None, color='r', lw=2, label='Measurements', **kwar
if connect:
plt.plot(range(len(xs)), xs, lw=1, c=c, alpha=alpha)'''
if ys is not None:
plt.plot(xs, ys, color=color, lw=lw, ls='--', label=label, **kwargs)
if lines:
if ys is not None:
plt.plot(xs, ys, color=color, lw=lw, ls='--', label=label, **kwargs)
else:
plt.plot(xs, color=color, lw=lw, ls='--', label=label, **kwargs)
else:
plt.plot(xs, color=color, lw=lw, ls='--', label=label, **kwargs)
if ys is not None:
plt.scatter(xs, ys, edgecolor=color, facecolor='none', lw=2, label=label, **kwargs)
else:
plt.scatter(range(len(xs)), xs, edgecolor=color, facecolor='none', lw=2, label=label, **kwargs)
def plot_residual_limits(Ps, stds=1.):

11
code/mkf_internal.py
View File

@ -356,11 +356,12 @@ def plot_correlation_covariance():
import book_plots as bp
def plot_track(ps, zs, cov, std_scale=1,
plot_P=True, y_lim=None,
plot_P=True, y_lim=None, dt=1.,
xlabel='time', ylabel='position',
title='Kalman Filter'):
count = len(zs)
actual = np.linspace(0, count - 1, count)
actual = np.linspace(0, count - 1, count) * dt
cov = np.asarray(cov)
std = std_scale*np.sqrt(cov[:,0,0])
std_top = np.minimum(actual+std, [count + 10])
@ -378,6 +379,8 @@ def plot_track(ps, zs, cov, std_scale=1,
plt.fill_between(range(len(std_top)), std_top, std_btm,
facecolor='yellow', alpha=0.2, interpolate=True)
plt.legend(loc=4)
plt.xlabel(xlabel)
plt.ylabel(ylabel)
if y_lim is not None:
plt.ylim(y_lim)
else:
@ -389,10 +392,10 @@ def plot_track(ps, zs, cov, std_scale=1,
if plot_P:
ax = plt.subplot(121)
ax.set_title("$\sigma^2_x$")
ax.set_title("$\sigma^2_x$ (pos variance)")
plot_covariance(cov, (0, 0))
ax = plt.subplot(122)
ax.set_title("$\sigma^2_y$")
ax.set_title("$\sigma^2_y$ (vel variance)")
plot_covariance(cov, (1, 1))
plt.show()

29
code/nonlinear_plots.py
View File

@ -270,7 +270,6 @@ 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)
@ -285,11 +284,11 @@ def plot_multiple_gaussians(xs, ps, x_range, y_range, N):
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)
ax.view_init(elev=40., azim=230)
if __name__ == "__main__":
plot_cov_ellipse_colormap(cov=[[2, 1.2], [1.2, 2]])
#plot_cov_ellipse_colormap(cov=[[2, 1.2], [1.2, 2]])
'''
from numpy.random import normal
import numpy as np
@ -308,4 +307,26 @@ if __name__ == "__main__":
#plot_transfer_func (data, g, lims=(-3,3), num_bins=100)
plot_nonlinear_func (data, g, gaussian=x0,
num_bins=100)
'''
'''
Ps = np.array([[[ 2.85841814, 0.71772898],
[ 0.71772898, 0.93786824]],
[[ 3.28939458, 0.52634978],
[ 0.52634978, 0.13435503]],
[[ 2.40532661, 0.29692055],
[ 0.29692055, 0.07671416]],
[[ 2.23084082, 0.27823192],
[ 0.27823192, 0.07488681]]])
Ms = np.array([[ 0.68040795, 0.17084572],
[ 8.46201389, 1.15070342],
[ 13.7992229 , 0.96022707],
[ 19.95838208, 0.87524265]])
plot_multiple_gaussians(Ms, Ps, (-5,25), (-5, 5), 75)