Permalink
Browse files

implement Kalman Filter (WIP), tests, add COPYING file

  • Loading branch information...
1 parent 0c80af1 commit 1d68d3e4bf4f1cd8c534d80003d1e53fe16bb0c6 @strohel committed Sep 19, 2010
View
339 COPYING

Large diffs are not rendered by default.

Oops, something went wrong.
View
@@ -1,5 +1,6 @@
"""TODO: documentation"""
-__all__ = ['pdfs']
+__all__ = ['pdfs', 'kalman']
import pdfs
+import kalman
View
@@ -0,0 +1,53 @@
+"""Kalman filter"""
+
+import numpy as np
+from numpy import dot
+from numpy.linalg import inv
+
+import bayepy as bp
+
+class Kalman:
+ """Kalman filter"""
+
+ def __init__(self, A, B, C, D, Q, R, state_pdf):
+ # TODO: check A, B ... for type, dimension and shape
+ self.A = np.asarray(A)
+ self.B = np.asarray(B)
+ self.C = np.asarray(C)
+ self.D = np.asarray(D)
+ self.Q = np.asarray(Q)
+ self.R = np.asarray(R)
+
+ if False:
+ print
+ dict = {"A":A, "B":B, "C":C, "D":D, "Q":Q, "R":R}
+ for key in dict:
+ print key + ":"
+ print repr(dict[key])
+
+ if not isinstance(state_pdf, bp.pdfs.Pdf):
+ raise TypeException("apos_pdf must be (a subclass of) bayepy.pdfs.Pdf")
+
+ self.P = state_pdf
+ self.S = bp.pdfs.GaussPdf()
+
+
+ def bayes(self, yt, ut):
+ """Aproximate Bayes rule"""
+ yt = np.asarray(yt)
+ ut = np.asarray(ut)
+
+ # predict
+ self.P.mu = dot(self.A, self.P.mu) + dot(self.B, ut) # a priori estimate
+ self.P.R = dot(dot(self.A, self.P.R), self.A.T) + self.Q # a priori variance
+
+ # data update
+ self.S.mu = dot(self.C, self.P.mu) + dot(self.D, ut)
+ self.S.R = dot(dot(self.C, self.P.R), self.C.T) + self.R
+
+ K = dot(dot(self.P.R, self.C.T), inv(self.S.R))
+
+ self.P.mu = self.P.mu + dot(K, (yt - self.S.mu)) # a posteriori estimate
+ self.P.R = self.P.R - dot(dot(K, self.C), self.P.R) # a posteriori variance
+
+ return self.P.mu
View
@@ -27,23 +27,23 @@ class GaussPdf(Pdf):
.. math: f(x|\mu,b) \propto \exp(-(x-\mu)'R^{-1}(x-\mu))
"""
- def __init__(self, mean, variance):
+ def __init__(self, mean=np.array([1]), variance=np.array([[1]])):
"""Initialise Gaussian pdf with mean value mu and variance R
mu % mean values
R % variance
"""
- if not isinstance(mean, np.ndarray) or not isinstance(variance, np.ndarray):
- raise TypeError("both mean and variance must be numpy arrays");
+ mean = np.asarray(mean)
+ variance = np.asarray(variance)
if mean.ndim != 1:
- raise ValueError("mean must be one-dimensional")
+ raise ValueError("mean must be one-dimensional (" + str(mean.ndim) + " encountered)")
if variance.ndim != 2:
raise ValueError("variance must be 2-dimensional")
if variance.shape[0] != variance.shape[1]:
raise ValueError("variance must be rectangular")
if mean.shape[0] != variance.shape[0]:
raise ValueError("mean and variance must have equal shape")
- if np.any(variance != variance.transpose()):
+ if np.any(variance != variance.T):
raise ValueError("variance must be symmetric (complex variance not supported)")
# TODO: variance must be positive definite
self.mu = mean
View
@@ -1,3 +1,4 @@
"""BayePy's tests"""
from test_pdfs import *
+from test_kalman import *
@@ -0,0 +1,38 @@
+"""Tests for bayepy.kalman"""
+
+import unittest as ut
+import os.path
+import time
+
+import numpy as np
+from scipy.io import loadmat
+
+import bayepy as bp
+
+class TestKalman(ut.TestCase):
+
+ def setUp(self):
+ pass
+
+ def test_bayes(self):
+ file = os.path.join(os.path.dirname(__file__), "test_kalman_data.mat")
+ d = loadmat(file, struct_as_record=True, mat_dtype=True)
+ mu0 = np.reshape(d['mu0'], -1) # otherwise we would get 2D array of shape (1xN)
+
+ gauss = bp.pdfs.GaussPdf(mu0, d['P0'])
+ kalman = bp.kalman.Kalman(d['A'], np.squeeze(d['B'].T), d['C'], d['D'], d['Q'], d['R'], gauss)
+
+ y = np.squeeze(d['y']) # to prevent 2D array (1xN)
+ u = np.squeeze(d['u']) # ditto
+ N = y.shape[0]
+ Mu_py = np.zeros((mu0.shape[0], N))
+
+ #print "y, u, N, mu0, Mu_py:", y, u, N, mu0, Mu_py
+
+ start = np.array([time.time(), time.clock()])
+ for t in xrange(1, N): # the 1 start offset is intentional
+ temp = kalman.bayes(y[t], u[t])
+ Mu_py[:,t] = 0
+ spent = np.array([time.time(), time.clock()]) - start
+
+ print "time spent [real, cpu]:", spent
Binary file not shown.
@@ -1,8 +1,9 @@
-"""This file tests bayepy.pdfs"""
+"""Tests for bayepy.pdfs"""
-import numpy as np
import unittest as ut
+import numpy as np
+
import bayepy as bp
class TestPdf(ut.TestCase):
@@ -34,10 +35,6 @@ def setUp(self):
def test_invalid_initialisation(self):
constructor = bp.pdfs.GaussPdf
- # invalid parameter types
- self.assertRaises(TypeError, constructor, [1, 2, 3], self.variance)
- self.assertRaises(TypeError, constructor, self.mean, [[1, 2], [3, 4]])
-
# invalid mean and variance shape
self.assertRaises(ValueError, constructor, np.array([[1], [2]]), self.variance)
self.assertRaises(ValueError, constructor, self.mean, np.array([1., 2., 3.,]))

0 comments on commit 1d68d3e

Please sign in to comment.