From daa1a355ea6cd6664aa640322e5285caa92d48f7 Mon Sep 17 00:00:00 2001
From: Andreas Tsouchlos <an.tsouchlos@gmail.com>
Date: Fri, 8 Apr 2022 02:46:55 +0200
Subject: [PATCH] Implemented second order LTI system example

---
 kalman_filter/__main__.py      | 68 ++++++++++++++++++++++++++--------
 kalman_filter/kalman_filter.py |  4 +-
 2 files changed, 53 insertions(+), 19 deletions(-)

diff --git a/kalman_filter/__main__.py b/kalman_filter/__main__.py
index d88db32..a68b4e3 100644
--- a/kalman_filter/__main__.py
+++ b/kalman_filter/__main__.py
@@ -1,29 +1,65 @@
 from .kalman_filter import *
+import matplotlib.pyplot as plt
 
 
-def print_state(x, P, K):
+def print_state(x, P):
     print("------------")
     print("x: " + str(x))
     print("P: " + str(P))
-    print("K: " + str(K))
+
+"""
+x_{n+1} = x_n + dt*v_n
+v_{n+1} = v_n
+
+
+"""
 
 
 def main():
-#    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)
+    n_iterations = 500
 
-    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)
+    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]
+
+    x0 = [100, 50]
+    P0 = [[10, 0], [0, 5]]
+    H  = [[1, 0], [0, 1]]
+    Q  = [[0.2, 0], [0, 2]]
+    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)
+
+
+    x_actual[0] = x0_actual
+    x_kalman[0] = x0
+
+
+    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)
+
+        print_state(f.x, f.P)
+        
+        x_actual[i] = dot(F, x_actual[i-1])
+        x_kalman[i] = f.x
+        x_measurement[i] = measurement
+
+
+    t = np.linspace(0, n_iterations, n_iterations)
+    
+    plt.plot(t, x_actual[:, 0])
+    plt.plot(t, x_kalman[:, 0])
+    plt.plot(t, x_measurement[:, 0])
+    plt.show()
 
 if __name__ == '__main__':
     main()
diff --git a/kalman_filter/kalman_filter.py b/kalman_filter/kalman_filter.py
index 0be6a8c..b9b7f1b 100644
--- a/kalman_filter/kalman_filter.py
+++ b/kalman_filter/kalman_filter.py
@@ -16,7 +16,6 @@ class KalmanFilter:
         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
@@ -24,8 +23,7 @@ class KalmanFilter:
 
         inv = inv_gen(temp)
 
-        self.K = dot(self.P, dot(np.transpose(self.H), inv))
-        K = self.K
+        K = dot(self.P, dot(np.transpose(self.H), inv))
 
         innovation = z - dot(self.H, self.x)
         self.x = self.x + dot(K, innovation)