next up previous contents
Next: Graphics with gnuplot Up: Getting started and examples Previous: Tests of statistical utilities   Contents

Tests of tests kalman filter on a random walk

The file test_kalman.py

. 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


Huaiyu Zhu
2002-03-23