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

Various changes. Main thing is removed the slow gaussian class from

Browse Source 
the kalman filters. It was annoying anyway - have to pack and unpack
it everywhere when calling around.
This commit is contained in:
Roger Labbe 2014-05-04 17:33:39 -05:00
parent 8305200ff0
commit 499f8d91f5
7 changed files with 308 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

186
Kalman Filters.ipynb
View File

File diff suppressed because one or more lines are too long

104
KalmanFilter1D.py
View File

@ -4,32 +4,32 @@ Created on Thu May 1 19:48:54 2014
@author: rlabbe
"""
from __future__ import division
import gauss as g;
import math
import matplotlib.pyplot as plt
import noise
import numpy.random as random
import numba
class KalmanFilter1D(object):
def __init__ (self, est_0=g.gaussian(0,1000)):
self.estimate = est_0
def __init__ (self, x0, var):
self.mean = x0
self.variance = var
@jit
def estimate(self, z, z_variance):
self.mean = (self.variance*z + z_variance*self.mean) / (self.variance + z_variance)
self.variance = 1. / (1./self.variance + 1./z_variance)
def update_estimate(self, Z):
self.estimate = self.estimate * Z
def project_ahead(self, U):
self.estimate = self.estimate + U
def project(self, u, u_variance):
self.mean += u
self.variance += u_variance
def fixed_error_kf(measurement_error, motion_error, noise_factor = 1.0):
mu = 0
def _fixed_error_kf(measurement_error, motion_error, noise_factor = 1.0):
mean = 0
sig = 1000
measurements = [x+5 for x in range(100)]
f = KalmanFilter1D (g.gaussian(mu,sig))
measurements = [x for x in range(100)]
f = KalmanFilter1D (mean,sig)
ys = []
errs = []
@ -37,14 +37,14 @@ def fixed_error_kf(measurement_error, motion_error, noise_factor = 1.0):
for i in range(len(measurements)):
r = noise.white_noise (noise_factor)
m = measurements[i] + r
f.update_estimate (g.gaussian(m, measurement_error))
z = measurements[i] + r
f.estimate (z, measurement_error)
xs.append(m)
ys.append(f.estimate.mu)
errs.append (f.estimate.sigma)
xs.append(z)
ys.append(f.mean)
errs.append (f.variance)
f.project_ahead (g.gaussian(20, motion_error))
f.project (1, motion_error)
plt.clf()
@ -55,30 +55,28 @@ def fixed_error_kf(measurement_error, motion_error, noise_factor = 1.0):
#plt.errorbar (x=range(len(ys)), color='b', y=ys, yerr=errs)
plt.show()
def varying_error_kf(noise_factor=1.0):
def _varying_error_kf(noise_factor=1.0):
motion_sig = 2.
mu = 0
mean = 0
sig = 1000
measurements = [x for x in range(100)]
f = KF1D (mu,sig)
f = KalmanFilter1D (mean,sig)
ys = []
us = []
errs = []
xs = []
for i in range(len(measurements)):
r = random.randn() * noise_factor
m = measurements[i] + r
print (r)
f.update (m, abs(r*10))
xs.append(m)
#print ("measure:" + str(f.estimate))
ys.append(f.estimate.mu)
errs.append (f.estimate.sigma)
f.predict (1.0, motion_sig)
#print ("predict:" + str(f.estimate))
f.estimate (m, abs(r*10))
xs.append(m)
ys.append(f.mean)
errs.append (f.variance)
f.project (1.0, motion_sig)
plt.clf()
plt.plot (measurements, 'r')
@ -87,29 +85,29 @@ def varying_error_kf(noise_factor=1.0):
plt.show()
def _test_foo ():
def _test_sin ():
sensor_error = 1
movement = .1
movement_error = .1
pos = g.gaussian(1,500)
pos = (1,500)
zs = []
ps = []
filter_ = KalmanFilter1D(pos)
m_1 = filter_.estimate.mu
filter_ = KalmanFilter1D(pos[0],pos[1])
m_1 = filter_.mean
for i in range(300):
filter_.update_estimate(g.gaussian(movement, movement_error))
filter_.project(movement, movement_error)
Z = math.sin(i/12.) + math.sqrt(abs(noise.white_noise(.02)))
movement = filter_.estimate.mu - m_1
m_1 = filter_.estimate.mu
movement = filter_.mean - m_1
m_1 = filter_.mean
print movement, filter_.estimate.sigma
zs.append(Z)
filter_.project_ahead (g.gaussian(Z, sensor_error))
ps.append(filter_.estimate[0])
filter_.estimate (Z, sensor_error)
ps.append(filter_.mean)
p1, = plt.plot(zs,c='r', linestyle='dashed')
@ -119,11 +117,19 @@ def _test_foo ():
if __name__ == "__main__":
if 1:
if 0:
# use same seed to get repeatable results
random.seed(10)
fixed_error_kf(measurement_error=100.,
motion_error=.1,
noise_factor=50)
#_test_foo()
if 1:
plt.figure()
_varying_error_kf ()
if 1:
plt.figure()
_fixed_error_kf(measurement_error=1.,
motion_error=.1,
noise_factor=50)
if 1:
plt.figure()
_test_sin()

119
Multidimensional Kalman Filters.ipynb
View File

File diff suppressed because one or more lines are too long

54
Untitled0.ipynb
View File

@ -92,7 +92,8 @@
"cell_type": "code",
"collapsed": false,
"input": [
"print numpy.array(3)"
"print numpy.array(3)\n",
"print pylab.rcParams['figure.figsize']"
],
"language": "python",
"metadata": {},
@ -101,11 +102,58 @@
"output_type": "stream",
"stream": "stdout",
"text": [
"3\n"
"3\n",
"(6.0, 4.0)\n"
]
}
],
"prompt_number": 17
"prompt_number": 2
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"class KalmanFilter1D(object):\n",
"\n",
" def __init__ (self, x0, var):\n",
" self.mean = x0\n",
" self.variance = var\n",
"\n",
" def estimate(self, z, z_variance):\n",
" self.mean = \\\n",
" (self.variance*z + z_variance*self.mean) / \\\n",
" self.variance + z_variance\n",
" self.variance = 1. / (1./self.variance+ 1./z_variance)\n",
"\n",
"f = KalmanFilter1D(1,2)\n",
"f.estimate(z = 2,\n",
" z_variance = 3)\n",
"\n",
"print f.mean, f.variance"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"6 1.2\n"
]
}
],
"prompt_number": 6
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import scipy.interpolate\n"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 9
}
],
"metadata": {}

52
gauss.py
View File

@ -11,32 +11,49 @@ import numpy.random as random
class gaussian(object):
def __init__ (self,m,s):
self.mu = float(m)
self.sigma = float(s)
def __init__(self, mean, variance):
try:
self.mean = float(mean)
self.variance = float(variance)
def __add__ (a,b):
return gaussian (a.mu + b.mu, a.sigma + b.sigma)
except:
self.mean = mean
self.variance = variance
def __mul__ (a,b):
m = (a.sigma*b.mu + b.sigma*a.mu) / (a.sigma + b.sigma)
s = 1. / (1./a.sigma + 1./b.sigma)
return gaussian (m,s)
def __add__ (a, b):
return gaussian (a.mean + b.mean, a.variance + b.variance)
def __mul__ (a, b):
m = (a.variance*b.mean + b.variance*a.mean) / (a.variance + b.variance)
v = 1. / (1./a.variance + 1./b.variance)
return gaussian (m, v)
def __call__(self, x):
""" Impl
"""
return math.exp (-0.5 * (x-self.mean)**2 / self.variance) / \
math.sqrt(2.*math.pi*self.variance)
def __call__(self,x):
return math.exp (-0.5 * (x-self.mu)**2 / self.sigma) / \
math.sqrt(2.*math.pi*self.sigma)
def __str__(self):
return "(%f, %f)" %(self.mu, self.sigma)
return "(%f, %f)" %(self.mean, self.sigma)
def stddev(self):
return math.sqrt (self.variance)
def as_tuple(self):
return (self.mean, self.variance)
def __tuple__(self):
return (self.mean, self.variance)
def __getitem__ (self,index):
""" maybe silly, allows you to access obect as a tuple:
a = gaussian(3,4)
print (tuple(a))
"""
if index == 0: return self.mu
elif index == 1: return self.sigma
if index == 0: return self.mean
elif index == 1: return self.variance
else: raise StopIteration
class KF1D(object):
@ -52,7 +69,10 @@ class KF1D(object):
def mul2 (a, b):
m = (a['variance']*b['mean'] + b['variance']*a['mean']) / (a['variance'] + b['variance'])
v = 1. / (1./a['variance'] + 1./b['variance'])
return gaussian (m, v)
#varying_error_kf( noise_factor=100)

8
gaussian.py
View File

@ -13,6 +13,14 @@ def gaussian(x, mean, var):
"""
return math.exp((-0.5*(x-mean)**2)/var) / math.sqrt(_two_pi*var)
def mul (a_mu, a_var, b_mu, b_var):
m = (a_var*b_mu + b_var*a_mu) / (a_var + b_var)
v = 1. / (1./a_var + 1./b_var)
return (m, v)
def add (a_mu, a_var, b_mu, b_var):
return (a_mu+b_mu, a_var+b_var)
def multivariate_gaussian(x, mu, cov):
""" This is designed to work the same as scipy.stats.multivariate_normal

12
mkf_internal.py
View File

@ -4,12 +4,14 @@ Created on Thu May 1 16:56:49 2014
@author: rlabbe
"""
import matplotlib.pyplot as plt
def show_residual_chart():
xlim([0.9,2.5]);ylim([0.5,2.5])
plt.xlim([0.9,2.5])
plt.ylim([0.5,2.5])
scatter ([1,2,2],[1,2,1.3])
scatter ([2],[1.8],marker='o')
plt.scatter ([1,2,2],[1,2,1.3])
plt.scatter ([2],[1.8],marker='o')
ax = plt.axes()
ax.annotate('', xy=(2,2), xytext=(1,1),
arrowprops=dict(arrowstyle='->', ec='b',shrinkA=3, shrinkB=4))
@ -23,5 +25,5 @@ def show_residual_chart():
arrowprops=dict(arrowstyle="<->",
ec="r",
shrinkA=5, shrinkB=5))
title("Kalman Filter Prediction Update Step")
show()
plt.title("Kalman Filter Prediction Update Step")
plt.show()