Tidied up code

kalman_filter/__main__.py

@@ -2,28 +2,30 @@ from .kalman_filter import *
 import matplotlib.pyplot as plt
 
 
-def print_state(x, P):
-    print("------------")
-    print("x: " + str(x))
-    print("P: " + str(P))
-
 """
+Second order LTI-system:
 x_{n+1} = x_n + dt*v_n
 v_{n+1} = v_n
 
+F = [[1, 1],
+     [0, 1]]
 
+
+No known control inputs:
+G = 0
+
+
+Measured values are the states:
+H = [[1, 0],
+     [0, 1]]
+
+
+The reset of the parameters are empirically determined
 """
 
 
 def main():
-    n_iterations = 500
-
-    x_actual = np.zeros([n_iterations, 2])
-    x_kalman = np.zeros([n_iterations, 2])
-    x_measurement = np.zeros([n_iterations, 2])
-
-    std_dist = [30, 0.5]
-    x0_actual = [50, 3]
+    # Kalman Filter parameter definition
 
     x0 = [100, 50]
     P0 = [[10, 0], [0, 5]]
@@ -32,28 +34,39 @@ def main():
     F  = [[1, 1], [0, 1]]
     G  = 0
     R  = [[std_dist[0]**2, 0], [0, std_dist[1]**2]] 
-#    R  = [[std_dist[0], 0], [0, std_dist[1]]] 
-    
-    f = KalmanFilter(x0, P0, H, Q, F, G)
 
 
+    # Simulation parameter definitions / initialization
+
+    n_iterations = 500
+    std_dist     = [30, 0.5]
+    x0_actual    = [50, 3]
+    
+    x_actual      = np.zeros([n_iterations, 2])
+    x_kalman      = np.zeros([n_iterations, 2])
+    x_measurement = np.zeros([n_iterations, 2])
+
     x_actual[0] = x0_actual
     x_kalman[0] = x0
 
+    
+    # Simulation
+
+    f = KalmanFilter(x0, P0, H, Q, F, G)
+
 
     for i in range(1, n_iterations):
         measurement = dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist)
 
-        f.predict(0)
-        f.update(measurement, R)
+        f.step(measurement, R, 0)
 
-        print_state(f.x, f.P)
-        
-        x_actual[i] = dot(F, x_actual[i-1])
-        x_kalman[i] = f.x
+        x_actual[i]      = dot(F, x_actual[i-1])
+        x_kalman[i]      = f.x
         x_measurement[i] = measurement
 
 
+    # Show results
+
     t = np.linspace(0, n_iterations, n_iterations)
     
     plt.plot(t, x_actual[:, 0])

kalman_filter/kalman_filter.py

@@ -2,46 +2,71 @@ import numpy as np
 from numpy import dot
 
 
+#
+# 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 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.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 = dot(self.H, dot(self.P,self.H)) + R
-
-
         inv = inv_gen(temp)
-
         K = dot(self.P, dot(np.transpose(self.H), inv))
 
+
+        # Update state
+
         innovation = z - dot(self.H, self.x)
         self.x = self.x + dot(K, innovation)
 
+
+        # Update uncertainty
+
         KH = dot(K, self.H)
-        
+        K_complementary = identity_gen(KH) - KH
 
-        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)))
+        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
 
+    def step(self, z, R, u):
+        self.predict(u)
+        self.update(z, R)
+