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

proper visualization

Browse Source
This commit is contained in:
Andreas Tsouchlos 2022-04-21 17:42:06 +02:00
parent 04a9db824c
commit de1b20029d
1 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
examples/gps_fusion.py
View File

@ -16,6 +16,7 @@ def run(kalman_filter_t):
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
@ -45,18 +46,26 @@ def run(kalman_filter_t):
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
#print(P_kalman[:, 0])
def update(disp, t):
disp.move_object("test", [x_kalman[:, 0][t], 0])
disp.set_circle_radius("test", t*0.2)
disp.move_object("measurement", [x_measurement[:, 0][t], 0])
disp.set_circle_radius("measurement", std_dist[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=50)
disp = Displayer(width=6, height=6, frame_interval=100)
disp.set_axis_limits(xlim=[0, 1000], ylim=[-500, 500])
disp.register_object("test", "red")
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)