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

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

File diff suppressed because one or more lines are too long

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()

File diff suppressed because one or more lines are too long

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": {}

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)

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

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()