Skip to content

Commit 7f07f89

Browse files
rsasaki0109rsasaki0109
committed
New script: 'ekf_with_velocity_correction.py'
1 parent 97ad5df commit 7f07f89

File tree

2 files changed

+232
-41
lines changed

2 files changed

+232
-41
lines changed

‎Localization/extended_kalman_filter/ekf_with_velocity_correction.py

+198
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
"""
2+
3+
Extended kalman filter (EKF) localization with velocity correction sample
4+
5+
author: Atsushi Sakai (@Atsushi_twi)
6+
modified by: Ryohei Sasaki (@rsasaki0109)
7+
8+
"""
9+
import sys
10+
import pathlib
11+
12+
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
13+
14+
import math
15+
import matplotlib.pyplot as plt
16+
import numpy as np
17+
18+
from utils.plot import plot_covariance_ellipse
19+
20+
# Covariance for EKF simulation
21+
Q = np.diag([
22+
0.1, # variance of location on x-axis
23+
0.1, # variance of location on y-axis
24+
np.deg2rad(1.0), # variance of yaw angle
25+
0.4, # variance of velocity
26+
0.1 # variance of scale factor
27+
]) ** 2 # predict state covariance
28+
R = np.diag([0.1, 0.1]) ** 2 # Observation x,y position covariance
29+
30+
# Simulation parameter
31+
INPUT_NOISE = np.diag([0.1, np.deg2rad(5.0)]) ** 2
32+
GPS_NOISE = np.diag([0.05, 0.05]) ** 2
33+
34+
DT = 0.1 # time tick [s]
35+
SIM_TIME = 50.0 # simulation time [s]
36+
37+
show_animation = True
38+
39+
40+
def calc_input():
41+
v = 1.0 # [m/s]
42+
yawrate = 0.1 # [rad/s]
43+
u = np.array([[v], [yawrate]])
44+
return u
45+
46+
47+
def observation(xTrue, xd, u):
48+
xTrue = motion_model(xTrue, u)
49+
50+
# add noise to gps x-y
51+
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
52+
53+
# add noise to input
54+
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
55+
56+
xd = motion_model(xd, ud)
57+
58+
return xTrue, z, xd, ud
59+
60+
61+
def motion_model(x, u):
62+
F = np.array([[1.0, 0, 0, 0, 0],
63+
[0, 1.0, 0, 0, 0],
64+
[0, 0, 1.0, 0, 0],
65+
[0, 0, 0, 0, 0],
66+
[0, 0, 0, 0, 1.0]])
67+
68+
B = np.array([[DT * math.cos(x[2, 0]) * x[4, 0], 0],
69+
[DT * math.sin(x[2, 0]) * x[4, 0], 0],
70+
[0.0, DT],
71+
[1.0, 0.0],
72+
[0.0, 0.0]])
73+
74+
x = F @ x + B @ u
75+
76+
return x
77+
78+
79+
def observation_model(x):
80+
H = np.array([
81+
[1, 0, 0, 0, 0],
82+
[0, 1, 0, 0, 0]
83+
])
84+
z = H @ x
85+
86+
return z
87+
88+
89+
def jacob_f(x, u):
90+
"""
91+
Jacobian of Motion Model
92+
93+
motion model
94+
x_{t+1} = x_t+v*s*dt*cos(yaw)
95+
y_{t+1} = y_t+v*s*dt*sin(yaw)
96+
yaw_{t+1} = yaw_t+omega*dt
97+
v_{t+1} = v{t}
98+
s_{t+1} = s{t}
99+
so
100+
dx/dyaw = -v*dt*sin(yaw)
101+
dx/dv = dt*cos(yaw)
102+
dx/ds = dt*v*cos(yaw)
103+
dy/dyaw = v*dt*cos(yaw)
104+
dy/dv = dt*sin(yaw)
105+
dy/ds = dt*v*sin(yaw)
106+
"""
107+
yaw = x[2, 0]
108+
v = u[0, 0]
109+
s = x[4, 0]
110+
jF = np.array([
111+
[1.0, 0.0, -DT * v * s * math.sin(yaw), DT * s * math.cos(yaw), DT * v * math.cos(yaw)],
112+
[0.0, 1.0, DT * v * s * math.cos(yaw), DT * s * math.sin(yaw), DT * v * math.sin(yaw)],
113+
[0.0, 0.0, 1.0, 0.0, 0.0],
114+
[0.0, 0.0, 0.0, 1.0, 0.0],
115+
[0.0, 0.0, 0.0, 0.0, 1.0]])
116+
return jF
117+
118+
119+
def jacob_h():
120+
jH = np.array([[1, 0, 0, 0, 0],
121+
[0, 1, 0, 0, 0]])
122+
return jH
123+
124+
125+
def ekf_estimation(xEst, PEst, z, u):
126+
# Predict
127+
xPred = motion_model(xEst, u)
128+
jF = jacob_f(xEst, u)
129+
PPred = jF @ PEst @ jF.T + Q
130+
131+
# Update
132+
jH = jacob_h()
133+
zPred = observation_model(xPred)
134+
y = z - zPred
135+
S = jH @ PPred @ jH.T + R
136+
K = PPred @ jH.T @ np.linalg.inv(S)
137+
xEst = xPred + K @ y
138+
PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
139+
return xEst, PEst
140+
141+
142+
def main():
143+
print(__file__ + " start!!")
144+
145+
time = 0.0
146+
147+
# State Vector [x y yaw v s]'
148+
xEst = np.zeros((5, 1))
149+
xEst[4, 0] = 1.0 # Initial scale factor
150+
xTrue = np.zeros((5, 1))
151+
true_scale_factor = 0.9 # True scale factor
152+
xTrue[4, 0] = true_scale_factor
153+
PEst = np.eye(5)
154+
155+
xDR = np.zeros((5, 1)) # Dead reckoning
156+
157+
# history
158+
hxEst = xEst
159+
hxTrue = xTrue
160+
hxDR = xTrue
161+
hz = np.zeros((2, 1))
162+
163+
while SIM_TIME >= time:
164+
time += DT
165+
u = calc_input()
166+
167+
xTrue, z, xDR, ud = observation(xTrue, xDR, u)
168+
169+
xEst, PEst = ekf_estimation(xEst, PEst, z, ud)
170+
171+
# store data history
172+
hxEst = np.hstack((hxEst, xEst))
173+
hxDR = np.hstack((hxDR, xDR))
174+
hxTrue = np.hstack((hxTrue, xTrue))
175+
hz = np.hstack((hz, z))
176+
estimated_scale_factor = hxEst[4, -1]
177+
if show_animation:
178+
plt.cla()
179+
# for stopping simulation with the esc key.
180+
plt.gcf().canvas.mpl_connect('key_release_event',
181+
lambda event: [exit(0) if event.key == 'escape' else None])
182+
plt.plot(hz[0, :], hz[1, :], ".g")
183+
plt.plot(hxTrue[0, :].flatten(),
184+
hxTrue[1, :].flatten(), "-b")
185+
plt.plot(hxDR[0, :].flatten(),
186+
hxDR[1, :].flatten(), "-k")
187+
plt.plot(hxEst[0, :].flatten(),
188+
hxEst[1, :].flatten(), "-r")
189+
plt.text(0.45, 0.85, f"True Velocity Scale Factor: {true_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
190+
plt.text(0.45, 0.95, f"Estimated Velocity Scale Factor: {estimated_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
191+
plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst)
192+
plt.axis("equal")
193+
plt.grid(True)
194+
plt.pause(0.001)
195+
196+
197+
if __name__ == '__main__':
198+
main()

‎Localization/extended_kalman_filter/extended_kalman_filter.py

+34-41
Original file line numberDiff line numberDiff line change
@@ -21,14 +21,13 @@
2121
0.1, # variance of location on x-axis
2222
0.1, # variance of location on y-axis
2323
np.deg2rad(1.0), # variance of yaw angle
24-
0.4, # variance of velocity
25-
0.1 # variance of scale factor
24+
1.0 # variance of velocity
2625
]) ** 2 # predict state covariance
27-
R = np.diag([0.1, 0.1]) ** 2 # Observation x,y position covariance
26+
R = np.diag([1.0, 1.0]) ** 2 # Observation x,y position covariance
2827

2928
# Simulation parameter
30-
INPUT_NOISE = np.diag([0.1, np.deg2rad(5.0)]) ** 2
31-
GPS_NOISE = np.diag([0.05, 0.05]) ** 2
29+
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
30+
GPS_NOISE = np.diag([0.5, 0.5]) ** 2
3231

3332
DT = 0.1 # time tick [s]
3433
SIM_TIME = 50.0 # simulation time [s]
@@ -58,17 +57,15 @@ def observation(xTrue, xd, u):
5857

5958

6059
def motion_model(x, u):
61-
F = np.array([[1.0, 0, 0, 0, 0],
62-
[0, 1.0, 0, 0, 0],
63-
[0, 0, 1.0, 0, 0],
64-
[0, 0, 0, 0, 0],
65-
[0, 0, 0, 0, 1.0]])
66-
67-
B = np.array([[DT * math.cos(x[2, 0]) * x[4, 0], 0],
68-
[DT * math.sin(x[2, 0]) * x[4, 0], 0],
60+
F = np.array([[1.0, 0, 0, 0],
61+
[0, 1.0, 0, 0],
62+
[0, 0, 1.0, 0],
63+
[0, 0, 0, 0]])
64+
65+
B = np.array([[DT * math.cos(x[2, 0]), 0],
66+
[DT * math.sin(x[2, 0]), 0],
6967
[0.0, DT],
70-
[1.0, 0.0],
71-
[0.0, 0.0]])
68+
[1.0, 0.0]])
7269

7370
x = F @ x + B @ u
7471

@@ -77,9 +74,10 @@ def motion_model(x, u):
7774

7875
def observation_model(x):
7976
H = np.array([
80-
[1, 0, 0, 0, 0],
81-
[0, 1, 0, 0, 0]
77+
[1, 0, 0, 0],
78+
[0, 1, 0, 0]
8279
])
80+
8381
z = H @ x
8482

8583
return z
@@ -90,34 +88,34 @@ def jacob_f(x, u):
9088
Jacobian of Motion Model
9189
9290
motion model
93-
x_{t+1} = x_t+v*s*dt*cos(yaw)
94-
y_{t+1} = y_t+v*s*dt*sin(yaw)
91+
x_{t+1} = x_t+v*dt*cos(yaw)
92+
y_{t+1} = y_t+v*dt*sin(yaw)
9593
yaw_{t+1} = yaw_t+omega*dt
9694
v_{t+1} = v{t}
97-
s_{t+1} = s{t}
9895
so
9996
dx/dyaw = -v*dt*sin(yaw)
10097
dx/dv = dt*cos(yaw)
101-
dx/ds = dt*v*cos(yaw)
10298
dy/dyaw = v*dt*cos(yaw)
10399
dy/dv = dt*sin(yaw)
104-
dy/ds = dt*v*sin(yaw)
105100
"""
106101
yaw = x[2, 0]
107102
v = u[0, 0]
108-
s = x[4, 0]
109103
jF = np.array([
110-
[1.0, 0.0, -DT * v * s * math.sin(yaw), DT * s * math.cos(yaw), DT * v * math.cos(yaw)],
111-
[0.0, 1.0, DT * v * s * math.cos(yaw), DT * s * math.sin(yaw), DT * v * math.sin(yaw)],
112-
[0.0, 0.0, 1.0, 0.0, 0.0],
113-
[0.0, 0.0, 0.0, 1.0, 0.0],
114-
[0.0, 0.0, 0.0, 0.0, 1.0]])
104+
[1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
105+
[0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
106+
[0.0, 0.0, 1.0, 0.0],
107+
[0.0, 0.0, 0.0, 1.0]])
108+
115109
return jF
116110

117111

118112
def jacob_h():
119-
jH = np.array([[1, 0, 0, 0, 0],
120-
[0, 1, 0, 0, 0]])
113+
# Jacobian of Observation Model
114+
jH = np.array([
115+
[1, 0, 0, 0],
116+
[0, 1, 0, 0]
117+
])
118+
121119
return jH
122120

123121

@@ -143,15 +141,12 @@ def main():
143141

144142
time = 0.0
145143

146-
# State Vector [x y yaw v s]'
147-
xEst = np.zeros((5, 1))
148-
xEst[4, 0] = 1.0 # Initial scale factor
149-
xTrue = np.zeros((5, 1))
150-
true_scale_factor = 0.9 # True scale factor
151-
xTrue[4, 0] = true_scale_factor
152-
PEst = np.eye(5)
144+
# State Vector [x y yaw v]'
145+
xEst = np.zeros((4, 1))
146+
xTrue = np.zeros((4, 1))
147+
PEst = np.eye(4)
153148

154-
xDR = np.zeros((5, 1)) # Dead reckoning
149+
xDR = np.zeros((4, 1)) # Dead reckoning
155150

156151
# history
157152
hxEst = xEst
@@ -172,7 +167,7 @@ def main():
172167
hxDR = np.hstack((hxDR, xDR))
173168
hxTrue = np.hstack((hxTrue, xTrue))
174169
hz = np.hstack((hz, z))
175-
estimated_scale_factor = hxEst[4, -1]
170+
176171
if show_animation:
177172
plt.cla()
178173
# for stopping simulation with the esc key.
@@ -185,8 +180,6 @@ def main():
185180
hxDR[1, :].flatten(), "-k")
186181
plt.plot(hxEst[0, :].flatten(),
187182
hxEst[1, :].flatten(), "-r")
188-
plt.text(0.45, 0.85, f"True Velocity Scale Factor: {true_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
189-
plt.text(0.45, 0.95, f"Estimated Velocity Scale Factor: {estimated_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
190183
plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst)
191184
plt.axis("equal")
192185
plt.grid(True)

0 commit comments

Comments
 (0)