Created gps_two_sources_animated and renamed std_dist to std_dev in all files

examples/gps_two_sources_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)

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

@@ -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)
 
@@ -53,7 +53,7 @@ def run(kalman_filter_t):
 
     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])

main.py

@@ -1,13 +1,15 @@
 from examples import gps_measurement as gps
 from examples import second_order_system as sos
 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)
-    sos_anim.run(LinearKalmanFilter)
+    #sos_anim.run(LinearKalmanFilter)
+    gps_ts_anim.run(LinearKalmanFilter)
 
 
 if __name__ == '__main__':