Skip to content

Commit a6aae71

Browse files
committed
code clean up for EKF and UKF
1 parent 56e0b7a commit a6aae71

File tree

2 files changed

+33
-28
lines changed

2 files changed

+33
-28
lines changed

Localization/extended_kalman_filter/extended_kalman_filter.py

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -6,17 +6,22 @@
66
77
"""
88

9-
import numpy as np
109
import math
10+
import numpy as np
1111
import matplotlib.pyplot as plt
1212

13-
# Estimation parameter of EKF
14-
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
13+
# Covariance for EKF simulation
14+
Q = np.diag([
15+
0.1, # variance of location on x-axis
16+
0.1, # variance of location on y-axis
17+
np.deg2rad(1.0), # variance of yaw angle
18+
1.0 # variance of velocity
19+
])**2 # predict state covariance
1520
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
1621

1722
# Simulation parameter
18-
Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
19-
Rsim = np.diag([0.5, 0.5])**2
23+
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
24+
GPS_NOISE = np.diag([0.5, 0.5])**2
2025

2126
DT = 0.1 # time tick [s]
2227
SIM_TIME = 50.0 # simulation time [s]
@@ -27,7 +32,7 @@
2732
def calc_input():
2833
v = 1.0 # [m/s]
2934
yawrate = 0.1 # [rad/s]
30-
u = np.array([[v, yawrate]]).T
35+
u = np.array([[v], [yawrate]])
3136
return u
3237

3338

@@ -36,14 +41,10 @@ def observation(xTrue, xd, u):
3641
xTrue = motion_model(xTrue, u)
3742

3843
# add noise to gps x-y
39-
zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0]
40-
zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1]
41-
z = np.array([[zx, zy]]).T
44+
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
4245

4346
# add noise to input
44-
ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
45-
ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
46-
ud = np.array([[ud1, ud2]]).T
47+
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
4748

4849
xd = motion_model(xd, ud)
4950

@@ -62,19 +63,18 @@ def motion_model(x, u):
6263
[0.0, DT],
6364
[1.0, 0.0]])
6465

65-
x = F@x + B@u
66+
x = F @ x + B @ u
6667

6768
return x
6869

6970

7071
def observation_model(x):
71-
# Observation Model
7272
H = np.array([
7373
[1, 0, 0, 0],
7474
[0, 1, 0, 0]
7575
])
7676

77-
z = H@x
77+
z = H @ x
7878

7979
return z
8080

Localization/unscented_kalman_filter/unscented_kalman_filter.py

Lines changed: 18 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -11,13 +11,18 @@
1111
import math
1212
import matplotlib.pyplot as plt
1313

14-
# Estimation parameter of UKF
15-
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
16-
R = np.diag([1.0, np.deg2rad(40.0)])**2
14+
# Covariance for UKF simulation
15+
Q = np.diag([
16+
0.1, # variance of location on x-axis
17+
0.1, # variance of location on y-axis
18+
np.deg2rad(1.0), # variance of yaw angle
19+
1.0 # variance of velocity
20+
])**2 # predict state covariance
21+
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
1722

1823
# Simulation parameter
19-
Qsim = np.diag([0.5, 0.5])**2
20-
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
24+
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
25+
GPS_NOISE = np.diag([0.5, 0.5])**2
2126

2227
DT = 0.1 # time tick [s]
2328
SIM_TIME = 50.0 # simulation time [s]
@@ -42,14 +47,10 @@ def observation(xTrue, xd, u):
4247
xTrue = motion_model(xTrue, u)
4348

4449
# add noise to gps x-y
45-
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
46-
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
47-
z = np.array([[zx, zy]]).T
50+
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
4851

4952
# add noise to input
50-
ud1 = u[0] + np.random.randn() * Rsim[0, 0]
51-
ud2 = u[1] + np.random.randn() * Rsim[1, 1]
52-
ud = np.array([ud1, ud2])
53+
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
5354

5455
xd = motion_model(xd, ud)
5556

@@ -100,15 +101,19 @@ def generate_sigma_points(xEst, PEst, gamma):
100101

101102

102103
def predict_sigma_motion(sigma, u):
103-
# Sigma Points prediction with motion model
104+
"""
105+
Sigma Points prediction with motion model
106+
"""
104107
for i in range(sigma.shape[1]):
105108
sigma[:, i:i + 1] = motion_model(sigma[:, i:i + 1], u)
106109

107110
return sigma
108111

109112

110113
def predict_sigma_observation(sigma):
111-
# Sigma Points prediction with observation model
114+
"""
115+
Sigma Points prediction with observation model
116+
"""
112117
for i in range(sigma.shape[1]):
113118
sigma[0:2, i] = observation_model(sigma[:, i])
114119

0 commit comments

Comments
 (0)