Permalink
Browse files

kalman

  • Loading branch information...
1 parent f64d9ec commit 50b681bc67f223bdfa0b77e8061805ec45165427 huanghuang committed Mar 5, 2012
Showing with 179 additions and 0 deletions.
  1. +179 −0 kalman.py
View
179 kalman.py
@@ -0,0 +1,179 @@
+# Write a function 'filter' that implements a multi-
+# dimensional Kalman Filter for the example given
+
+from math import *
+
+class matrix:
+
+ # implements basic operations of a matrix class
+
+ def __init__(self, value):
+ self.value = value
+ self.dimx = len(value)
+ self.dimy = len(value[0])
+ if value == [[]]:
+ self.dimx = 0
+
+ def zero(self, dimx, dimy):
+ # check if valid dimensions
+ if dimx < 1 or dimy < 1:
+ raise ValueError, "Invalid size of matrix"
+ else:
+ self.dimx = dimx
+ self.dimy = dimy
+ self.value = [[0 for row in range(dimy)] for col in range(dimx)]
+
+ def identity(self, dim):
+ # check if valid dimension
+ if dim < 1:
+ raise ValueError, "Invalid size of matrix"
+ else:
+ self.dimx = dim
+ self.dimy = dim
+ self.value = [[0 for row in range(dim)] for col in range(dim)]
+ for i in range(dim):
+ self.value[i][i] = 1
+
+ def show(self):
+ for i in range(self.dimx):
+ print self.value[i]
+ print ' '
+
+ def __add__(self, other):
+ # check if correct dimensions
+ if self.dimx != other.dimx or self.dimy != other.dimy:
+ raise ValueError, "Matrices must be of equal dimensions to add"
+ else:
+ # add if correct dimensions
+ res = matrix([[]])
+ res.zero(self.dimx, self.dimy)
+ for i in range(self.dimx):
+ for j in range(self.dimy):
+ res.value[i][j] = self.value[i][j] + other.value[i][j]
+ return res
+
+ def __sub__(self, other):
+ # check if correct dimensions
+ if self.dimx != other.dimx or self.dimy != other.dimy:
+ raise ValueError, "Matrices must be of equal dimensions to subtract"
+ else:
+ # subtract if correct dimensions
+ res = matrix([[]])
+ res.zero(self.dimx, self.dimy)
+ for i in range(self.dimx):
+ for j in range(self.dimy):
+ res.value[i][j] = self.value[i][j] - other.value[i][j]
+ return res
+
+ def __mul__(self, other):
+ # check if correct dimensions
+ if self.dimy != other.dimx:
+ raise ValueError, "Matrices must be m*n and n*p to multiply"
+ else:
+ # subtract if correct dimensions
+ res = matrix([[]])
+ res.zero(self.dimx, other.dimy)
+ for i in range(self.dimx):
+ for j in range(other.dimy):
+ for k in range(self.dimy):
+ res.value[i][j] += self.value[i][k] * other.value[k][j]
+ return res
+
+ def transpose(self):
+ # compute transpose
+ res = matrix([[]])
+ res.zero(self.dimy, self.dimx)
+ for i in range(self.dimx):
+ for j in range(self.dimy):
+ res.value[j][i] = self.value[i][j]
+ return res
+
+ # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
+
+ def Cholesky(self, ztol=1.0e-5):
+ # Computes the upper triangular Cholesky factorization of
+ # a positive definite matrix.
+ res = matrix([[]])
+ res.zero(self.dimx, self.dimx)
+
+ for i in range(self.dimx):
+ S = sum([(res.value[k][i])**2 for k in range(i)])
+ d = self.value[i][i] - S
+ if abs(d) < ztol:
+ res.value[i][i] = 0.0
+ else:
+ if d < 0.0:
+ raise ValueError, "Matrix not positive-definite"
+ res.value[i][i] = sqrt(d)
+ for j in range(i+1, self.dimx):
+ S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
+ if abs(S) < ztol:
+ S = 0.0
+ res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
+ return res
+
+ def CholeskyInverse(self):
+ # Computes inverse of matrix given its Cholesky upper Triangular
+ # decomposition of matrix.
+ res = matrix([[]])
+ res.zero(self.dimx, self.dimx)
+
+ # Backward step for inverse.
+ for j in reversed(range(self.dimx)):
+ tjj = self.value[j][j]
+ S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
+ res.value[j][j] = 1.0/tjj**2 - S/tjj
+ for i in reversed(range(j)):
+ res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
+ return res
+
+ def inverse(self):
+ aux = self.Cholesky()
+ res = aux.CholeskyInverse()
+ return res
+
+ def __repr__(self):
+ return repr(self.value)
+
+
+########################################
+
+# Implement the filter function below
+
+def filter(x, P):
+ for n in range(len(measurements)):
+
+ # measurement update
+ y = matrix([[measurements[n]]]) - H*x
+ s = H * P * H.transpose() + R
+ K = P * H.transpose() * s.inverse()
+ x = x + K*y
+ P = (I-K*H) * P
+
+
+ # prediction
+
+ x = F * x + u
+ P = F * P * F.transpose()
+
+ print 'x= '
+ x.show()
+ print 'P= '
+ P.show()
+
+
+
+########################################
+
+measurements = [1, 2, 3]
+
+x = matrix([[0.], [0.]]) # initial state (location and velocity)
+P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
+u = matrix([[0.], [0.]]) # external motion
+F = matrix([[1., 1.], [0, 1.]]) # next state function
+H = matrix([[1., 0.]]) # measurement function
+R = matrix([[1.]]) # measurement uncertainty
+I = matrix([[1., 0.], [0., 1.]]) # identity matrix
+
+filter(x, P)
+

0 comments on commit 50b681b

Please sign in to comment.