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

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()

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)
+    disp.animate(n_steps=n_iterations, anim_callback=update)

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()