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

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)