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:
parent
8305200ff0
commit
499f8d91f5
File diff suppressed because one or more lines are too long
@ -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
@ -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
52
gauss.py
@ -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)
|
||||
|
@ -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
|
||||
|
@ -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()
|
Loading…
Reference in New Issue
Block a user