Reorganize directory structure

filter/kalman_filter.py

@@ -0,0 +1,71 @@
+import numpy as np
+
+
+#
+# Utility Functions
+#
+
+
+#
+# These functions are necessary in order to be able to work with
+# scalars as well as vectors
+# (np.identity() and np.linalg.inv() do not work with scalars)
+#
+
+def inv_gen(A):
+    if np.isscalar(A):
+        return 1 / A
+    else:
+        return np.linalg.inv(A)
+
+def identity_gen(A):
+    if np.isscalar(A):
+        return 1
+    else:
+        return np.identity(np.shape(A)[0])
+
+
+#
+# Kalman Filter
+#
+
+
+class LinearKalmanFilter:
+    def __init__(self, x0, P0, H, Q, F, G):
+        self.x = x0 # Initial state
+        self.P = P0 # Initial uncertainty
+        self.H = H  # Observation matrix
+        self.Q = Q  # Process noise matrix
+        self.F = F  # State transition matrix
+        self.G = G  # Input transition matrix
+    
+    def update(self, z, R):
+        # Calculate Kalman Gain
+
+        temp = np.dot(self.H, np.dot(self.P,self.H)) + R
+        inv = inv_gen(temp)
+        K = np.dot(self.P, np.dot(np.transpose(self.H), inv))
+
+
+        # Update state
+
+        innovation = z - np.dot(self.H, self.x)
+        self.x = self.x + np.dot(K, innovation)
+
+
+        # Update uncertainty
+
+        KH = np.dot(K, self.H)
+        K_complementary = identity_gen(KH) - KH
+
+        self.P = np.dot(K_complementary, np.dot(self.P, np.transpose(K_complementary))) \
+                    + np.dot(K, np.dot(R, np.transpose(K)))
+
+    def predict(self, u):
+        self.x = np.dot(self.F, self.x) + np.dot(self.G,u)
+        self.P = np.dot(self.F, np.dot(self.P, inv_gen(self.F))) + self.Q
+
+    def step(self, z, R, u):
+        self.predict(u)
+        self.update(z, R)
+