From 04a9db824cf87e7db88689ad4efa54599e2ff8b1 Mon Sep 17 00:00:00 2001
From: Andreas Tsouchlos <andreas.tsouchlos@yardeye.com>
Date: Thu, 21 Apr 2022 17:17:01 +0200
Subject: [PATCH] gps_fusion.py now shows second order system results with the
 Displayer (position, not yet uncertainty)

---
 display/dislpay_2d.py  | 35 +++++++++++--------------
 examples/gps_fusion.py | 58 +++++++++++++++++++++++++++++++++++-------
 main.py                |  5 ++--
 3 files changed, 67 insertions(+), 31 deletions(-)

diff --git a/display/dislpay_2d.py b/display/dislpay_2d.py
index 36b7fa4..2a2bd2c 100644
--- a/display/dislpay_2d.py
+++ b/display/dislpay_2d.py
@@ -3,16 +3,13 @@ from matplotlib.animation import FuncAnimation
 import numpy as np
 
 
-#
-# Displayer class
-#
-
 class Displayer:
-    def __init__(self, width, height):
+    def __init__(self, width, height, frame_interval=50):
         self._fig, self._ax = plt.subplots()
         self._fig.set_figwidth(width)
         self._fig.set_figheight(height)
-    
+        self._frame_interval = frame_interval
+
         self._object_map = {}
         self._circle_map = {}
         self._objects = []
@@ -31,14 +28,12 @@ class Displayer:
         return self._objects
 
     
-    #
     # Functions for initalization
-    #
 
 
-    def set_axis_limits(self, min, max):
-        self._xlim = (min, max)
-        self._ylim = (min, max)
+    def set_axis_limits(self, xlim, ylim):
+        self._xlim = xlim
+        self._ylim = ylim
 
 
     def register_object(self, obj_name, obj_color='r'):
@@ -49,22 +44,18 @@ class Displayer:
         self._objects.append(self._circle_map[obj_name])
 
 
-    #
     # Functions to be used in the animation callback
-    #
 
 
-    def move_object(self, obj_name, x, y):
-        self._object_map[obj_name].set_data([x, y])
-        self._circle_map[obj_name].center = (x, y)
+    def move_object(self, obj_name, position):
+        self._object_map[obj_name].set_data(position)
+        self._circle_map[obj_name].center = position
 
     def set_circle_radius(self, obj_name, r):
         self._circle_map[obj_name].radius = r
 
 
-    #
     # Start the animation
-    #
 
 
     def animate(self, n_steps, anim_callback):
@@ -72,6 +63,10 @@ class Displayer:
             anim_callback(self, t)
             return self._objects
 
-        anim = FuncAnimation(self._fig, anim_callback_wrapper, frames=np.linspace(0, n_steps-1, n_steps),
-                            init_func=self.__init_animation, blit=True)
+        anim = FuncAnimation(self._fig,
+                            anim_callback_wrapper,
+                            n_steps,
+                            interval=self._frame_interval,
+                            init_func=self.__init_animation,
+                            blit=True)
         plt.show()
\ No newline at end of file
diff --git a/examples/gps_fusion.py b/examples/gps_fusion.py
index 993b2bc..770c7fb 100644
--- a/examples/gps_fusion.py
+++ b/examples/gps_fusion.py
@@ -6,17 +6,57 @@ from matplotlib.animation import FuncAnimation
 from display.dislpay_2d import Displayer
 
 
-def update(disp, t):
-    disp.move_object("test", t, t)
-    disp.move_object("test2", 20-t, t)
-    disp.set_circle_radius("test", t*0.2)
+def run(kalman_filter_t):
+    # Simulation parameter definitions
+
+    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])
 
 
-def run():
-    disp = Displayer(width=6, height=6)
+    # Kalman Filter parameter definition
 
-    disp.set_axis_limits(min=0, max=20)
+    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]] 
+
+    x_actual[0] = x0_actual
+    x_kalman[0] = x0
+
+    
+    # Simulation
+
+    f = kalman_filter_t(x0, P0, H, Q, F, G)
+
+
+    for i in range(1, n_iterations):
+        measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist)
+
+        f.step(measurement, R, 0)
+
+        x_actual[i]      = np.dot(F, x_actual[i-1])
+        x_kalman[i]      = f.x
+        x_measurement[i] = measurement
+
+
+    # Show results
+
+
+    def update(disp, t):
+        disp.move_object("test", [x_kalman[:, 0][t], 0])
+        disp.set_circle_radius("test", t*0.2)
+
+    disp = Displayer(width=6, height=6, frame_interval=50)
+
+    disp.set_axis_limits(xlim=[0, 1000], ylim=[-500, 500])
     disp.register_object("test", "red")
-    disp.register_object("test2", "black")
 
-    disp.animate(n_steps = 20, anim_callback=update)
\ No newline at end of file
+    disp.animate(n_steps=n_iterations, anim_callback=update)
\ No newline at end of file
diff --git a/main.py b/main.py
index 773b78b..a0b8cf4 100644
--- a/main.py
+++ b/main.py
@@ -1,13 +1,14 @@
 from examples import gps_measurement as gps
 from examples import second_order_system as sos
 from examples import gps_fusion
+from filter.kalman_filter import LinearKalmanFilter
 
 
 def main():
     #gps.run(LinearKalmanFilter)
     #sos.run(LinearKalmanFilter)
-    gps_fusion.run()
-    
+    gps_fusion.run(LinearKalmanFilter)
+
 
 if __name__ == '__main__':
     main()