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

Implmented example showing two gps measurements

Browse Source
This commit is contained in:
Andreas Tsouchlos 2022-04-25 10:57:04 +02:00
parent ba6e7d4ce7
commit 5f0274fbff
3 changed files with 39 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
examples/gps_two_sources_animated.py
View File

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

2
examples/second_order_system.py
View File

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

2
examples/second_order_system_animated.py
View File

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