First draft of filter implementation

.gitignore

@@ -0,0 +1 @@
+kalman_filter/__pycache__

kalman_filter/__main__.py

@@ -1,7 +1,29 @@
-from .filter import *
+from .kalman_filter import *
+
+
+def print_state(x, P, K):
+    print("------------")
+    print("x: " + str(x))
+    print("P: " + str(P))
+    print("K: " + str(K))
+
 
 def main():
-    print("Hello World!")
+#    f = KalmanFilter(x0=[100, 20], P0=[[50, 0],[0, 10]], H=np.identity(2), Q=[[0, 0], [0,0]], F=[[1, 1], [0,1]], G=np.zeros([2, 2]))
+    f = KalmanFilter(x0=10, P0=5, H=1, Q=0, F=1, G=0)
+
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    f.update(z=6, R=5)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
 
 if __name__ == '__main__':
     main()

kalman_filter/kalman_filter.py

@@ -0,0 +1,49 @@
+import numpy as np
+from numpy import dot
+
+
+def inv_gen(A):
+    if np.isscalar(A):
+        return 1 / A
+    else:
+        return np.linalg.inv(A)
+
+class KalmanFilter:
+    def __init__(self, x0, P0, H, Q, F, G):
+        self.x = x0
+        self.P = P0
+        self.H = H
+        self.Q = Q
+        self.F = F
+        self.G = G
+        self.K = 1
+
+    def update(self, z, R):
+        temp = dot(self.H, dot(self.P,self.H)) + R
+
+
+        inv = inv_gen(temp)
+
+        self.K = dot(self.P, dot(np.transpose(self.H), inv))
+        K = self.K
+
+        innovation = z - dot(self.H, self.x)
+        self.x = self.x + dot(K, innovation)
+
+        KH = dot(K, self.H)
+        
+
+        K_complementary = 0
+
+        if np.isscalar(temp):
+            K_complementary = 1 - KH
+        else:
+            K_complementary = np.identity(np.shape(KH)[0]) - KH
+
+
+        self.P = dot(K_complementary, dot(self.P, np.transpose(K_complementary))) + dot(K, dot(R, np.transpose(K)))
+
+    def predict(self, u):
+        self.x = dot(self.F, self.x) + dot(self.G,u)
+        self.P = dot(self.F, dot(self.P, inv_gen(self.F))) + self.Q
+