From ba6e7d4ce7634515ec64965b7744539322435daf Mon Sep 17 00:00:00 2001 From: Andreas Tsouchlos Date: Mon, 25 Apr 2022 10:09:58 +0200 Subject: [PATCH] Created gps_two_sources_animated and renamed std_dist to std_dev in all files --- examples/gps_two_sources_animated.py | 69 ++++++++++++++++++++++++ examples/second_order_system.py | 6 +-- examples/second_order_system_animated.py | 8 +-- main.py | 4 +- 4 files changed, 79 insertions(+), 8 deletions(-) create mode 100644 examples/gps_two_sources_animated.py diff --git a/examples/gps_two_sources_animated.py b/examples/gps_two_sources_animated.py new file mode 100644 index 0000000..994ce6e --- /dev/null +++ b/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) diff --git a/examples/second_order_system.py b/examples/second_order_system.py index 14410b2..cab0fc1 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_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) diff --git a/examples/second_order_system_animated.py b/examples/second_order_system_animated.py index 26bce2d..994ce6e 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_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]) diff --git a/main.py b/main.py index 4f8f3e8..9d2c364 100644 --- a/main.py +++ b/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__':