an.tsouchlos/kalman_filter
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Changed parameters in gps_two_sources_animated.py; Renamed variables

Browse Source
This commit is contained in:
Andreas Tsouchlos 2022-04-25 12:56:46 +02:00
parent 55e180ba46
commit 18aa441f82
4 changed files with 53 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
examples/gps_measurement.py
View File

@ -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])

25
examples/gps_two_sources_animated.py
View File

@ -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)

24
examples/second_order_system.py
View File

@ -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()

34
examples/second_order_system_animated.py
View File

@ -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)