Created gps_two_sources_animated and renamed std_dist to std_dev in all files

display/dislpay_2d.py

@@ -10,9 +10,9 @@ class Displayer:
         self._fig.set_figheight(height)
         self._frame_interval = frame_interval
 
-        self._object_map = {}
-        self._circle_map = {}
-        self._objects = []
+        self._object_map     = {}
+        self._circle_map     = {}
+        self._objects        = []
 
         self._xlim = [0, 20]
         self._ylim = [0, 20]
@@ -37,8 +37,8 @@ class Displayer:
 
 
     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._object_map[obj_name], = plt.plot([], [], color=obj_color, marker='o', label=obj_name)
+        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])
@@ -54,6 +54,14 @@ class Displayer:
     def set_circle_radius(self, obj_name, r):
         self._circle_map[obj_name].radius = r
 
+    def hide_object(self, obj_name):
+        self._object_map[obj_name].set_alpha(0)
+        self._circle_map[obj_name].set_alpha(0)
+
+    def show_object(self, obj_name):
+        self._object_map[obj_name].set_alpha(1)
+        self._circle_map[obj_name].set_alpha(1)
+
 
     # Start the animation
 
@@ -61,6 +69,7 @@ class Displayer:
     def animate(self, n_steps, anim_callback):
         def anim_callback_wrapper(t):
             anim_callback(self, t)
+            self._ax.legend()
             return self._objects
 
         anim = FuncAnimation(self._fig,

examples/gps_two_sources_animated.py

@@ -10,7 +10,7 @@ def run(kalman_filter_t):
     # Simulation parameter definitions
 
     n_iterations = 500
-    std_dist     = [30, 0.5]
+    std_dev     = [30, 0.5]
     x0_actual    = [50, 3]
     
     x_actual      = np.zeros([n_iterations, 2])
@@ -27,7 +27,7 @@ def run(kalman_filter_t):
     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_dev[0]**2, 0], [0, std_dev[1]**2]] 
 
     x_actual[0] = x0_actual
     x_kalman[0] = x0
@@ -39,7 +39,7 @@ def run(kalman_filter_t):
 
 
     for i in range(1, n_iterations):
-        measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist)
+        measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dev)
 
         f.step(measurement, R, 0)
 
@@ -51,11 +51,9 @@ def run(kalman_filter_t):
 
     # Show results
 
-    #print(P_kalman[:, 0])
-
     def update(disp, t):
         disp.move_object("measurement", [x_measurement[:, 0][t], 0])
-        disp.set_circle_radius("measurement", std_dist[0])
+        disp.set_circle_radius("measurement", std_dev[0])
         disp.move_object("estimate", [x_kalman[:, 0][t], 0])
         disp.set_circle_radius("estimate", P_kalman[:,0][t])
         disp.move_object("ground_truth", [x_actual[:, 0][t], 0])

examples/second_order_system.py

@@ -28,7 +28,7 @@ def run(kalman_filter_t):
     # Simulation parameter definitions
 
     n_iterations = 500
-    std_dist     = [30, 0.5]
+    std_dev     = [30, 0.5]
     x0_actual    = [50, 3]
     
     x_actual      = np.zeros([n_iterations, 2])
@@ -44,7 +44,7 @@ def run(kalman_filter_t):
     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_dev[0]**2, 0], [0, std_dev[1]**2]] 
 
     x_actual[0] = x0_actual
     x_kalman[0] = x0
@@ -56,7 +56,7 @@ def run(kalman_filter_t):
 
 
     for i in range(1, n_iterations):
-        measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist)
+        measurement = np.dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dev)
 
         f.step(measurement, R, 0)
 

examples/second_order_system_animated.py

@@ -0,0 +1,69 @@
+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_dev     = [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])
+    P_kalman      = np.zeros([n_iterations, 2])
+
+
+    # Kalman Filter parameter definition
+
+    x0 = [100, 50]
+    P0 = [[1000, 0], [0, 400]]
+    H  = [[1, 0], [0, 1]]
+    Q  = [[0.2, 0], [0, 2]]
+    F  = [[1, 1], [0, 1]]
+    G  = 0
+    R  = [[std_dev[0]**2, 0], [0, std_dev[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_dev)
+
+        f.step(measurement, R, 0)
+
+        x_actual[i]      = np.dot(F, x_actual[i-1])
+        x_kalman[i]      = f.x
+        x_measurement[i] = measurement
+        P_kalman[i]      = [f.P[0,0], f.P[1,1]]
+
+
+    # Show results
+
+    def update(disp, t):
+        disp.move_object("measurement", [x_measurement[:, 0][t], 0])
+        disp.set_circle_radius("measurement", std_dev[0])
+        disp.move_object("estimate", [x_kalman[:, 0][t], 0])
+        disp.set_circle_radius("estimate", P_kalman[:,0][t])
+        disp.move_object("ground_truth", [x_actual[:, 0][t], 0])
+        disp.set_circle_radius("ground_truth", 0)
+
+    disp = Displayer(width=6, height=6, frame_interval=150)
+
+    disp.set_axis_limits(xlim=[0, 1000], ylim=[-500, 500])
+    disp.register_object("measurement", "red")
+    disp.register_object("estimate", "blue")
+    disp.register_object("ground_truth", "green")
+
+    disp.animate(n_steps=n_iterations, anim_callback=update)

main.py

@@ -1,13 +1,15 @@
 from examples import gps_measurement as gps
 from examples import second_order_system as sos
-from examples import gps_fusion
+from examples import second_order_system_animated as sos_anim
+from examples import gps_two_sources_animated as gps_ts_anim
 from filter.kalman_filter import LinearKalmanFilter
 
 
 def main():
     #gps.run(LinearKalmanFilter)
     #sos.run(LinearKalmanFilter)
-    gps_fusion.run(LinearKalmanFilter)
+    #sos_anim.run(LinearKalmanFilter)
+    gps_ts_anim.run(LinearKalmanFilter)
 
 
 if __name__ == '__main__':