. Results are at 3.19
#!/usr/bin/env python
# (C) 2000 Huaiyu Zhu <hzhu@users.sourceforge.net>. Licence: GPL
# $Id: test_kalman.py,v 1.4 2001/08/26 12:40:44 hzhu Exp $
"""
test_kalman.py - testing class LinearSystem with kalman filtering
System description:
state: x+ = A*x + u + w
measurement: z = H*x + v
Much of the code is for recording history and displaying.
"""
from MatPy.Matrix import eye, zeros, norm, Matrix_cr, Matrix_r
from MatPy.Stats.distribs import randn
from MatPy.DynSys.kalman import LinearSystem
from MatPy.efuncs import triu
from MatPy.Graphics.trajplot import TrajPlot
class RandomWalk(LinearSystem):
""" RandomWalk: Wiener processes with given kernels """
def init_params(self, n):
""" sets system parameters:
A = [I,I*dt ; I 0] state (p, v) transition
dt : time interval
n : dimension of positions
"""
zz = zeros((n,n))
ee = eye(n)
dt = 0.04
self.A = Matrix_cr([[ee, ee*dt], [zz, ee]])
self.Q2 = q = Matrix_cr([[randn((n,n))/n, zz], [zz, randn((n,n))/n]])
self.Q = q * q.T
self.H = Matrix_r([ee, zz])
self.R2 = r = randn((n,n)) * .4
self.R = r * r.T
self.RI = self.R.I
self.n = n*2
self.m = n
return self
def init_history(self):
""" initialize history of system """
self.g = g = TrajPlot(2, names=["System", "Estimate"],
wait_time=0.1)
g.title("Kalman filter: Trajectories of state and estimate")
g.axis_equal = 1
self.e = [];
print " "*10, " state ", " "*20, "estimate", " "*10, " error "
def add_history(self, x_sys, x_est):
""" add on step to history, print, then plot """
# get past history
e = self.e
e.append(norm(x_sys - x_est))
# record new data, and plot the first two axes
xx = Matrix((x_sys[0], x_est[0]))
yy = Matrix((x_sys[1], x_est[1]))
self.g.plotadd(xx, yy)
# print state, estimate or their distance
print x_sys.T, x_est.T, "%.4g" % e[i]
def show_history(self, waittime):
""" show entire history """
# plot errors
g = Gplot()
g.title("Kalman filter: Distances between states and estimates")
g.plot([Matrix(self.e)])
wait(waittime)
#------------------------------------------------------------------
""" Simulate linear system with Kalman filtering """
from MatPy.gplot import Gplot, wait
from MatPy.Matrix import Matrix
from MatPy.efuncs import abs
linsys = RandomWalk().init_params(n=2)
linsys.init_state()
linsys.init_history()
x = randn(linsys.x.shape)
for i in range(20):
x, u = linsys.evolve(x)
z = linsys.measure(x)
tx = linsys.filter(z, u)
linsys.add_history(x, tx)
if __name__ == "__main__": waittime = None
else: waittime = 2
linsys.show_history(waittime)
Result obtained with
>>> from MatPy.tests import test_kalman
state estimate error
[ 0.511 2.11 -3.58 -1.05 ] [ 0.468 2.23 -1.22 0.33 ] 2.737
[-0.257 1.97 -3.36 -2.91 ] [ 0.252 2.49 -0.259 0.106 ] 4.382
[ 0.376 4.11 -2.4 -1.6 ] [ 0.237 5.37 -0.0149 1.06 ] 3.791
[ 0.0281 4.09 -3.8 -0.969 ] [ 0.0747 4.52 -1.53 0.855 ] 2.947
[-0.203 4.6 -4.1 -2.59 ] [ 0.000496 4.59 -0.282 -0.148 ] 4.535
[-2.25 4.64 -4.12 -4.01 ] [-2.14 4.3 0.683 -0.268 ] 6.097
[-0.499 3.74 -4.69 -2.47 ] [-0.33 3.72 -0.483 0.408 ] 5.106
[ 1.98 2.38 -6.63 -4.76 ] [ 1.5 2.99 -1.84 -1.53 ] 5.827
[ 1.16 1.11 -8.27 -3.61 ] [ 0.914 1.64 -1.64 -0.862 ] 7.203
[ 1.5 -1.53 -8.55 -1.27 ] [ 1.24 -1.38 -2.56 -0.747 ] 6.018
[ 1.92 -2.4 -8.77 -2.41 ] [ 2.03 -2.28 -3.45 -1.63 ] 5.375
[ 2.75 -1.74 -9.9 -2.19 ] [ 2.23 -1.2 -4.94 -0.683 ] 5.244
[ 3.32 -1.48 -10 -2.94 ] [ 3.54 -1.79 -5.61 -0.753 ] 4.965
[ 2.29 -1.09 -10.7 -3.22 ] [ 2.24 -1.4 -6.61 -1.35 ] 4.538
[ 1.95 1.13 -11.4 -3.15 ] [ 1.37 0.714 -7.24 -0.65 ] 4.929
[ 1.13 3.68 -9.84 -2.15 ] [ 1.79 3.57 -3.84 1.01 ] 6.811
[ 0.843 2.93 -12 -1.97 ] [ 1.19 3.66 -6.33 2.28 ] 7.17
[ 0.559 4.84 -9.96 -1.44 ] [ 0.978 5.35 -5.74 1.76 ] 5.344
[-0.445 6.32 -10.5 -1.15 ] [ 0.147 6.88 -5.65 2.75 ] 6.257
[ 0.702 6.76 -8.87 -1.82 ] [ 0.74 7.05 -4.54 2.5 ] 6.127