gps_fusion.py now shows second order system results with the Displayer (position, not yet uncertainty)

.gitignore

@@ -1,2 +1,6 @@
-kalman_filter/__pycache__
-kalman_filter/*.pyc
+filter/__pycache__
+filter/*.pyc
+examples/__pycache__
+examples/*.pyc
+display/__pycache__
+display/*.pyc

README.md

@@ -15,7 +15,5 @@ $ pip install numpy matplotlib geopy
 From the root directory of this repository run
 
 ```bash
-$ python -m kalman_filter
-```
-
-Which example is run can be changed in `kalman_filter/__main__.py`
+$ python main.py
+```

display/dislpay_2d.py

@@ -0,0 +1,72 @@
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+import numpy as np
+
+
+class Displayer:
+    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 = []
+
+        self._xlim = [0, 20]
+        self._ylim = [0, 20]
+
+
+    def __init_animation(self):
+        self._ax.set_xlim(self._xlim)
+        self._ax.set_ylim(self._ylim)
+
+        for key in self._circle_map:
+            self._ax.add_patch(self._circle_map[key])
+
+        return self._objects
+
+    
+    # Functions for initalization
+
+
+    def set_axis_limits(self, xlim, ylim):
+        self._xlim = xlim
+        self._ylim = ylim
+
+
+    def register_object(self, obj_name, obj_color='r'):
+        self._object_map[obj_name], = plt.plot([], [], color=obj_color, marker='o')
+        self._circle_map[obj_name] = plt.Circle((0, 0), 1, color=obj_color, fill=False)
+
+        self._objects.append(self._object_map[obj_name])
+        self._objects.append(self._circle_map[obj_name])
+
+
+    # Functions to be used in the animation callback
+
+
+    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):
+        def anim_callback_wrapper(t):
+            anim_callback(self, t)
+            return self._objects
+
+        anim = FuncAnimation(self._fig,
+                            anim_callback_wrapper,
+                            n_steps,
+                            interval=self._frame_interval,
+                            init_func=self.__init_animation,
+                            blit=True)
+        plt.show()

examples/gps_fusion.py

@@ -0,0 +1,62 @@
+import matplotlib.pyplot as plt
+import numpy as np
+from time import sleep
+from matplotlib.animation import FuncAnimation
+
+from display.dislpay_2d import Displayer
+
+
+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])
+
+
+    # Kalman Filter parameter definition
+
+    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.animate(n_steps=n_iterations, anim_callback=update)

examples/gps_measurement.py

@@ -1,6 +1,6 @@
-from .kalman_filter import *
 import matplotlib.pyplot as plt
 import csv
+import numpy as np
 import geopy.distance
 
 
@@ -52,11 +52,11 @@ The reset of the parameters are empirically determined
 """
 
 
-def simulate():
+def run(kalman_filter_t):
     # Read data
     
     x0_actual     = [48.993552704, 8.371038159]
-    x_measurement = read_csv_lat_long('res/29_03_2022_Unterreut_8_Karlsruhe.csv')
+    x_measurement = read_csv_lat_long('examples/res/29_03_2022_Unterreut_8_Karlsruhe.csv')
 
     
     # Simulation parameter definitions
@@ -90,7 +90,7 @@ def simulate():
     
     # Simulation
 
-    f = KalmanFilter(x0, P0, H, Q, F, G)
+    f = kalman_filter_t(x0, P0, H, Q, F, G)
 
 
     for i in range(1, n_iterations):
@@ -126,4 +126,4 @@ def simulate():
     ax3.plot(t, x_error_measurement*1000, label="Error in measured position in m")
     ax3.legend()
     
-    plt.show()
+    plt.show()

examples/second_order_system.py

@@ -1,5 +1,5 @@
-from .kalman_filter import *
 import matplotlib.pyplot as plt
+import numpy as np
 
 
 """
@@ -24,7 +24,7 @@ The reset of the parameters are empirically determined
 """
 
 
-def simulate():
+def run(kalman_filter_t):
     # Simulation parameter definitions
 
     n_iterations = 500
@@ -52,7 +52,7 @@ def simulate():
     
     # Simulation
 
-    f = KalmanFilter(x0, P0, H, Q, F, G)
+    f = kalman_filter_t(x0, P0, H, Q, F, G)
 
 
     for i in range(1, n_iterations):
@@ -72,5 +72,4 @@ def simulate():
     plt.plot(t, x_actual[:, 0])
     plt.plot(t, x_kalman[:, 0])
     plt.plot(t, x_measurement[:, 0])
-    plt.show()
-
+    plt.show()

filter/kalman_filter.py

@@ -30,7 +30,7 @@ def identity_gen(A):
 #
 
 
-class KalmanFilter:
+class LinearKalmanFilter:
     def __init__(self, x0, P0, H, Q, F, G):
         self.x = x0 # Initial state
         self.P = P0 # Initial uncertainty

kalman_filter/__main__.py

@@ -1,10 +0,0 @@
-from . import gps_measurement as gps
-from . import second_order_system as sos
-
-
-def main():
-    gps.simulate()
-#    sos.simulate()
-
-if __name__ == '__main__':
-    main()

main.py

@@ -0,0 +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(LinearKalmanFilter)
+
+
+if __name__ == '__main__':
+    main()