diff --git a/examples/gps_measurement.py b/examples/gps_measurement.py index b512d8f..9ae1ee8 100644 --- a/examples/gps_measurement.py +++ b/examples/gps_measurement.py @@ -55,8 +55,8 @@ The reset of the parameters are empirically determined def run(kalman_filter_t): # Read data - x0_actual = [48.993552704, 8.371038159] - x_measurement = read_csv_lat_long('examples/res/29_03_2022_Unterreut_8_Karlsruhe.csv') + x0_ground_truth = [48.993552704, 8.371038159] + x_measurement = read_csv_lat_long('examples/res/29_03_2022_Unterreut_8_Karlsruhe.csv') # Simulation parameter definitions @@ -69,8 +69,8 @@ def run(kalman_filter_t): print("Computed stdandard deviation:") print(std_dev) - x_actual = np.zeros([n_iterations, 2]) - x_kalman = np.zeros([n_iterations, 2]) + x_ground_truth = np.zeros([n_iterations, 2]) + x_kalman = np.zeros([n_iterations, 2]) @@ -84,7 +84,7 @@ def run(kalman_filter_t): G = 0 R = [[std_dev[0]**2, 0], [0, std_dev[1]**2]] - x_actual[0] = x0_actual + x_ground_truth[0] = x0_ground_truth x_kalman[0] = x0 @@ -96,8 +96,8 @@ def run(kalman_filter_t): for i in range(1, n_iterations): f.step(x_measurement[i], R, 0) - x_actual[i] = np.dot(F, x_actual[i-1]) - x_kalman[i] = f.x + x_ground_truth[i] = np.dot(F, x_ground_truth[i-1]) + x_kalman[i] = f.x # Show results @@ -106,19 +106,19 @@ def run(kalman_filter_t): fig1 = plt.figure("Measured vs Filtered vs Actual") ax1 = fig1.add_axes([0.05,0.05,0.4,0.9]) - ax1.plot(t, x_actual[:, 0], label="Actual Latitude") + ax1.plot(t, x_ground_truth[:, 0], label="Actual Latitude") ax1.plot(t, x_kalman[:, 0], label="Kalman Filter Latitude") ax1.plot(t, x_measurement[:, 0], label="Measured Latitude") ax1.legend() ax2 = fig1.add_axes([0.55,0.05,0.4,0.9]) - ax2.plot(t, x_actual[:, 1], label="Actual Longitude") + ax2.plot(t, x_ground_truth[:, 1], label="Actual Longitude") ax2.plot(t, x_kalman[:, 1], label="Kalman Filter Longitude") ax2.plot(t, x_measurement[:, 1], label="Measured Longitude") ax2.legend() - x_error_kalman = lat_long_to_km(x_actual, x_kalman) - x_error_measurement = lat_long_to_km(x_actual, x_measurement) + x_error_kalman = lat_long_to_km(x_ground_truth, x_kalman) + x_error_measurement = lat_long_to_km(x_ground_truth, x_measurement) fig2 = plt.figure("Error in Position") ax3 = fig2.add_axes([0.1,0.1,0.8,0.8]) diff --git a/examples/gps_two_sources_animated.py b/examples/gps_two_sources_animated.py index e7be2e9..1893b40 100644 --- a/examples/gps_two_sources_animated.py +++ b/examples/gps_two_sources_animated.py @@ -9,10 +9,11 @@ from display.dislpay_2d import Displayer def run(kalman_filter_t): # Simulation parameter definitions - n_iterations = 500 - std_dev1 = 2 - std_dev2 = 30 - x_actual = 50 + n_iterations = 500 + std_dev1 = 0.2 + std_dev2 = 10 + x_offset2 = 6 + x_ground_truth = 50 x_kalman = np.zeros(n_iterations) x_measurement1 = np.zeros(n_iterations) @@ -40,8 +41,8 @@ def run(kalman_filter_t): for i in range(1, n_iterations): - measurement1 = np.random.normal(x_actual, std_dev1) - measurement2 = np.random.normal(x_actual, std_dev2) + measurement1 = np.random.normal(x_ground_truth, std_dev1) + measurement2 = np.random.normal(x_ground_truth + x_offset2, std_dev2) if i < source1_iter_end: f.step(measurement1, std_dev1, 0) @@ -64,17 +65,17 @@ def run(kalman_filter_t): 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.move_object("filtered", [x_kalman[t], 0]) + disp.set_circle_radius("filtered", np.sqrt(P_kalman[t])) + disp.move_object("ground_truth", [x_ground_truth, 0]) disp.set_circle_radius("ground_truth", 0) - disp = Displayer(width=6, height=6, frame_interval=150) + disp = Displayer(width=8, height=8, frame_interval=150) - disp.set_axis_limits(xlim=[0, 100], ylim=[-40, 60]) + disp.set_axis_limits(xlim=[35, 75], ylim=[-15, 25]) disp.register_object("measurement1", "purple") disp.register_object("measurement2", "pink") - disp.register_object("estimate", "blue") + disp.register_object("filtered", "blue") disp.register_object("ground_truth", "green") disp.animate(n_steps=n_iterations, anim_callback=update) diff --git a/examples/second_order_system.py b/examples/second_order_system.py index 3d505ba..97b7a5c 100644 --- a/examples/second_order_system.py +++ b/examples/second_order_system.py @@ -27,13 +27,13 @@ The reset of the parameters are empirically determined def run(kalman_filter_t): # Simulation parameter definitions - n_iterations = 500 - std_dev = [30, 0.5] - x0_actual = [50, 3] + n_iterations = 500 + std_dev = [30, 0.5] + x0_ground_truth = [50, 3] - x_actual = np.zeros([n_iterations, 2]) - x_kalman = np.zeros([n_iterations, 2]) - x_measurement = np.zeros([n_iterations, 2]) + x_ground_truth = np.zeros([n_iterations, 2]) + x_kalman = np.zeros([n_iterations, 2]) + x_measurement = np.zeros([n_iterations, 2]) # Kalman Filter parameter definition @@ -46,7 +46,7 @@ def run(kalman_filter_t): G = 0 R = [[std_dev[0]**2, 0], [0, std_dev[1]**2]] - x_actual[0] = x0_actual + x_ground_truth[0] = x0_ground_truth x_kalman[0] = x0 @@ -56,20 +56,20 @@ 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) + measurement = np.dot(F,x_ground_truth[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 + x_ground_truth[i] = np.dot(F, x_ground_truth[i-1]) + x_kalman[i] = f.x + x_measurement[i] = measurement # Show results t = np.linspace(0, n_iterations, n_iterations) - plt.plot(t, x_actual[:, 0]) + plt.plot(t, x_ground_truth[:, 0]) plt.plot(t, x_kalman[:, 0]) plt.plot(t, x_measurement[:, 0]) plt.show() \ No newline at end of file diff --git a/examples/second_order_system_animated.py b/examples/second_order_system_animated.py index f4bd49d..a7ac80b 100644 --- a/examples/second_order_system_animated.py +++ b/examples/second_order_system_animated.py @@ -9,14 +9,14 @@ 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] + n_iterations = 500 + std_dev = [30, 0.5] + x0_ground_truth = [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]) + x_ground_truth = 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 @@ -29,7 +29,7 @@ def run(kalman_filter_t): G = 0 R = [[std_dev[0]**2, 0], [0, std_dev[1]**2]] - x_actual[0] = x0_actual + x_ground_truth[0] = x0_ground_truth x_kalman[0] = x0 @@ -39,14 +39,14 @@ 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) + measurement = np.dot(F,x_ground_truth[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]] + x_ground_truth[i] = np.dot(F, x_ground_truth[i-1]) + x_kalman[i] = f.x + x_measurement[i] = measurement + P_kalman[i] = [f.P[0,0], f.P[1,1]] # Show results @@ -54,16 +54,16 @@ def run(kalman_filter_t): 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.move_object("filtered", [x_kalman[:, 0][t], 0]) + disp.set_circle_radius("filtered", P_kalman[:,0][t]) + disp.move_object("ground_truth", [x_ground_truth[:, 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("filtered", "blue") disp.register_object("ground_truth", "green") disp.animate(n_steps=n_iterations, anim_callback=update)