# udacity373_code / unit2 / u2-32_kalman_matrices.py

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177``` ```# 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 z = measurements[n] y = matrix([[z]]) - 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) ```
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.