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

Wholesale changes to connect chapters together.

Browse Source 
I made a lot of changes so that each chapter makes clear that
they are all implementing the same basic bayesian algorithm.

This required a lot of editting, and it doesn't make sense to
try to do that atomically, hence this huge check in.

I made a lot of edits, and haven't copy editted anything. i'm
sure I introduced a lot of problems and discontinuities.
This commit is contained in:
Roger Labbe
2015-06-20 15:52:16 -07:00
parent 5ffe5c67eb
commit 51c9a8283e
14 changed files with 2072 additions and 1648 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
code/DogSensor.py
View File

@@ -1,26 +0,0 @@
# -*- coding: utf-8 -*-
"""
Created on Sun May 11 13:21:39 2014
@author: rlabbe
"""
from __future__ import print_function, division
import numpy.random as random
import math
class DogSensor(object):
def __init__(self, x0=0, velocity=1, noise_var=0.0):
""" x0 - initial position
velocity - (+=right, -=left)
noise_var - noise variance, 0== no noise
"""
self.x = x0
self.velocity = velocity
self.noise = math.sqrt(noise_var)
def sense(self):
self.x = self.x + self.velocity
return self.x + random.randn() * self.noise

44
code/DogSimulation.py Normal file
View File

@@ -0,0 +1,44 @@
# -*- coding: utf-8 -*-
"""
Created on Sun May 11 13:21:39 2014
@author: rlabbe
"""
from __future__ import print_function, division
from numpy.random import randn
import math
class DogSimulation(object):
def __init__(self, x0=0, velocity=1,
measurement_variance=0.0, process_variance=0.0):
""" x0 - initial position
velocity - (+=right, -=left)
measurement_variance - variance in measurement m^2
process_variance - variance in process (m/s)^2
"""
self.x = x0
self.velocity = velocity
self.measurement_noise = math.sqrt(measurement_variance)
self.process_noise = math.sqrt(process_variance)
def move(self, dt=1.0):
'''Compute new position of the dog assuming `dt` seconds have
passed since the last update.'''
# compute new position based on velocity. Add in some
# process noise
velocity = self.velocity + randn() * self.process_noise
self.x += velocity * dt
def sense_position(self):
# simulate measuring the position with noise
measurement = self.x + randn() * self.measurement_noise
return measurement
def move_and_sense(self, dt=1.0):
self.move(dt)
return self.sense_position()

124
code/mkf_internal.py
View File

@@ -12,30 +12,56 @@ from mpl_toolkits.mplot3d import Axes3D
from numpy.random import multivariate_normal
import stats
def show_residual_chart():
plt.xlim([0.9,2.5])
plt.ylim([1.5,3.5])
plt.scatter ([1,2,2],[2,3,2.3])
plt.scatter ([2],[2.8],marker='o')
ax = plt.axes()
ax.annotate('', xy=(2,3), xytext=(1,2),
def show_residual_chart():
est_y = ((164.2-158)*.8 + 158)
ax = plt.axes(xticks=[], yticks=[], frameon=False)
ax.annotate('', xy=[1,159], xytext=[0,158],
arrowprops=dict(arrowstyle='->',
ec='r', lw=3, shrinkA=6, shrinkB=5))
ax.annotate('', xy=[1,159], xytext=[1,164.2],
arrowprops=dict(arrowstyle='-',
ec='k', lw=1, shrinkA=8, shrinkB=8))
ax.annotate('', xy=(1., est_y), xytext=(0.9, est_y),
arrowprops=dict(arrowstyle='->', ec='#004080',
lw=2,
shrinkA=3, shrinkB=4))
ax.annotate('prediction', xy=(2.04,3.), color='#004080')
ax.annotate('measurement', xy=(2.05, 2.28))
ax.annotate('prior estimate', xy=(1, 1.9))
ax.annotate('residual', xy=(2.04,2.6), color='#e24a33')
ax.annotate('new estimate', xy=(2,2.8),xytext=(2.1,2.8),
arrowprops=dict(arrowstyle='->', ec="k", shrinkA=3, shrinkB=4))
ax.annotate('', xy=(2,3), xytext=(2,2.3),
arrowprops=dict(arrowstyle="-",
ec="#e24a33",
lw=2,
shrinkA=5, shrinkB=5))
plt.title("Kalman Filter Predict and Update")
plt.axis('equal')
plt.scatter ([0,1], [158.0,est_y], c='k',s=128)
plt.scatter ([1], [164.2], c='b',s=128)
plt.scatter ([1], [159], c='r', s=128)
plt.text (1.0, 158.8, "prediction ($x_t)$", ha='center',va='top',fontsize=18,color='red')
plt.text (1.0, 164.4, "measurement ($z$)",ha='center',va='bottom',fontsize=18,color='blue')
plt.text (0, 157.8, "prior estimate ($\hat{x}_{t-1}$)", ha='center', va='top',fontsize=18)
plt.text (1.02, est_y-1.5, "residual", ha='left', va='center',fontsize=18)
plt.text (0.9, est_y, "new estimate ($\hat{x}_{t}$)", ha='right', va='center',fontsize=18)
plt.xlabel('time')
ax.yaxis.set_label_position("right")
plt.ylabel('state')
plt.xlim(-0.5, 1.5)
plt.show()
def plot_gaussian_multiply():
xs = np.arange(-5, 10, 0.1)
mean1, var1 = 0, 5
mean2, var2 = 5, 1
mean, var = stats.mul(mean1, var1, mean2, var2)
ys = [stats.gaussian(x, mean1, var1) for x in xs]
plt.plot(xs, ys, label='M1')
ys = [stats.gaussian(x, mean2, var2) for x in xs]
plt.plot(xs, ys, label='M2')
ys = [stats.gaussian(x, mean, var) for x in xs]
plt.plot(xs, ys, label='M1 x M2')
plt.legend()
plt.show()
@@ -329,6 +355,61 @@ def plot_correlation_covariance():
plt.show()
import book_plots as bp
def plot_track(ps, zs, cov,
plot_P=True, y_lim=None,
title='Kalman Filter'):
count = len(zs)
actual = np.linspace(0, count - 1, count)
cov = np.asarray(cov)
std = np.sqrt(cov[:,0,0])
std_top = np.minimum(actual+std, [count + 10])
std_btm = np.maximum(actual-std, [-50])
std_top = actual+std
std_btm = actual-std
bp.plot_track(actual)
bp.plot_measurements(range(1, count + 1), zs)
bp.plot_filter(range(1, count + 1), ps)
plt.plot(std_top, linestyle=':', color='k', lw=2)
plt.plot(std_btm, linestyle=':', color='k', lw=2)
plt.fill_between(range(len(std_top)), std_top, std_btm,
facecolor='yellow', alpha=0.2, interpolate=True)
plt.legend(loc=4)
if y_lim is not None:
plt.ylim(y_lim)
else:
plt.ylim((-50, count + 10))
plt.xlim((0,count))
plt.title(title)
plt.show()
if plot_P:
ax = plt.subplot(121)
ax.set_title("$\sigma^2_x$")
plot_covariance(cov, (0, 0))
ax = plt.subplot(122)
ax.set_title("$\sigma^2_y$")
plot_covariance(cov, (1, 1))
plt.show()
def plot_covariance(P, index=(0, 0)):
ps = []
for p in P:
ps.append(p[index[0], index[1]])
plt.plot(ps)
def test_fill():
plt.fill_between([0,1,2,3,4], [1,1,1,1,1], [4,4,4,4,4])
plt.plot([0,6], [6,1])
# plt.ylim(2,5)
plt.show()
if __name__ == "__main__":
#show_position_chart()
#plot_3d_covariance((2,7), np.array([[8.,0],[0,4.]]))
@@ -336,5 +417,6 @@ if __name__ == "__main__":
#show_residual_chart()
#show_position_chart()
show_x_error_chart(4)
#show_x_error_chart(4)
test_fill()

8
code/nonlinear_plots.py
View File

@@ -22,14 +22,10 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
#b = f(x) - x*m
# compute new mean and variance based on EKF equations
ys = f(data)
x0 = gaussian[0]
in_std = np.sqrt(gaussian[1])
y = f(x0)
#m = np.mean(ys)
std = np.std(ys)
in_lims = [x0-in_std*3, x0+in_std*3]
@@ -40,7 +36,6 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
h = np.histogram(ys, num_bins, density=False)
plt.subplot(2,2,4)
plt.plot(h[0], h[1][1:], lw=4, alpha=0.5)
print(max(h[0]))
plt.ylim(out_lims[1], out_lims[0])
plt.gca().xaxis.set_ticklabels([])
plt.title('output')
@@ -58,8 +53,6 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
plt.plot(pdf * max(h[0])/max(pdf), xs, lw=1, color='k')
print(max(norm.pdf(xs)))'''
# plot transfer function
plt.subplot(2,2,3)
x = np.arange(in_lims[0], in_lims[1], 0.1)
@@ -82,7 +75,6 @@ def plot_nonlinear_func(data, f, gaussian, num_bins=300):
plt.title('input')
plt.show()
print("fuck")
import math