From 5f0274fbffa70ffdbbfa3454aa67bf92df8fa0c5 Mon Sep 17 00:00:00 2001 From: Andreas Tsouchlos Date: Mon, 25 Apr 2022 10:57:04 +0200 Subject: [PATCH] Implmented example showing two gps measurements --- examples/gps_two_sources_animated.py | 63 ++++++++++++++---------- examples/second_order_system.py | 2 +- examples/second_order_system_animated.py | 2 +- 3 files changed, 39 insertions(+), 28 deletions(-) diff --git a/examples/gps_two_sources_animated.py b/examples/gps_two_sources_animated.py index 994ce6e..e7be2e9 100644 --- a/examples/gps_two_sources_animated.py +++ b/examples/gps_two_sources_animated.py @@ -10,26 +10,27 @@ def run(kalman_filter_t): # Simulation parameter definitions n_iterations = 500 - std_dev = [30, 0.5] - x0_actual = [50, 3] + std_dev1 = 2 + std_dev2 = 30 + x_actual = 50 - 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]) + x_kalman = np.zeros(n_iterations) + x_measurement1 = np.zeros(n_iterations) + x_measurement2 = np.zeros(n_iterations) + P_kalman = np.zeros(n_iterations) + + source1_iter_end = 30 # 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]] + x0 = 100 + P0 = 50 + H = 1 + Q = 0.1 + F = 1 G = 0 - R = [[std_dev[0]**2, 0], [0, std_dev[1]**2]] - x_actual[0] = x0_actual x_kalman[0] = x0 @@ -39,30 +40,40 @@ 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_dev) + measurement1 = np.random.normal(x_actual, std_dev1) + measurement2 = np.random.normal(x_actual, std_dev2) - f.step(measurement, R, 0) + if i < source1_iter_end: + f.step(measurement1, std_dev1, 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]] + f.step(measurement2, std_dev2, 0) + + x_kalman[i] = f.x + x_measurement1[i] = measurement1 + x_measurement2[i] = measurement2 + P_kalman[i] = f.P # 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]) + if t >= source1_iter_end: + disp.hide_object("measurement1") + + disp.move_object("measurement1", [x_measurement1[t], 0]) + disp.set_circle_radius("measurement1", std_dev1) + disp.move_object("measurement2", [x_measurement2[t], 0]) + disp.set_circle_radius("measurement2", std_dev2) + disp.move_object("estimate", [x_kalman[t], 0]) + disp.set_circle_radius("estimate", np.sqrt(P_kalman[t])) + disp.move_object("ground_truth", [x_actual, 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.set_axis_limits(xlim=[0, 100], ylim=[-40, 60]) + disp.register_object("measurement1", "purple") + disp.register_object("measurement2", "pink") disp.register_object("estimate", "blue") disp.register_object("ground_truth", "green") diff --git a/examples/second_order_system.py b/examples/second_order_system.py index cab0fc1..3d505ba 100644 --- a/examples/second_order_system.py +++ b/examples/second_order_system.py @@ -28,7 +28,7 @@ def run(kalman_filter_t): # Simulation parameter definitions n_iterations = 500 - std_dev = [30, 0.5] + std_dev = [30, 0.5] x0_actual = [50, 3] x_actual = np.zeros([n_iterations, 2]) diff --git a/examples/second_order_system_animated.py b/examples/second_order_system_animated.py index 994ce6e..f4bd49d 100644 --- a/examples/second_order_system_animated.py +++ b/examples/second_order_system_animated.py @@ -10,7 +10,7 @@ def run(kalman_filter_t): # Simulation parameter definitions n_iterations = 500 - std_dev = [30, 0.5] + std_dev = [30, 0.5] x0_actual = [50, 3] x_actual = np.zeros([n_iterations, 2])