Skip to content

Commit ac90c14

Browse files
committed
finish notebook
1 parent 4e7fbc3 commit ac90c14

File tree

3 files changed

+116
-26
lines changed

3 files changed

+116
-26
lines changed

Localization/extended_kalman_filter/extended_kalman_filter.py

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

1313
# Estimation parameter of EKF
14-
Q = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
15-
R = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
14+
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
15+
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
1616

1717
# Simulation parameter
18-
Qsim = np.diag([0.5, 0.5])**2
19-
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
18+
Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
19+
Rsim = np.diag([0.5, 0.5])**2
2020

2121
DT = 0.1 # time tick [s]
2222
SIM_TIME = 50.0 # simulation time [s]
@@ -36,13 +36,13 @@ def observation(xTrue, xd, u):
3636
xTrue = motion_model(xTrue, u)
3737

3838
# add noise to gps x-y
39-
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
40-
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
41-
z = np.array([[zx, zy]])
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
4242

4343
# add noise to input
44-
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
45-
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
44+
ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
45+
ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
4646
ud = np.array([[ud1, ud2]]).T
4747

4848
xd = motion_model(xd, ud)
@@ -120,13 +120,13 @@ def ekf_estimation(xEst, PEst, z, u):
120120
# Predict
121121
xPred = motion_model(xEst, u)
122122
jF = jacobF(xPred, u)
123-
PPred = jF@PEst@jF.T + R
123+
PPred = jF@PEst@jF.T + Q
124124

125125
# Update
126126
jH = jacobH(xPred)
127127
zPred = observation_model(xPred)
128-
y = z.T - zPred
129-
S = jH@PPred@jH.T + Q
128+
y = z - zPred
129+
S = jH@PPred@jH.T + R
130130
K = PPred@jH.T@np.linalg.inv(S)
131131
xEst = xPred + K@y
132132
PEst = (np.eye(len(xEst)) - K@jH)@PPred
@@ -175,7 +175,7 @@ def main():
175175
hxEst = xEst
176176
hxTrue = xTrue
177177
hxDR = xTrue
178-
hz = np.zeros((1, 2))
178+
hz = np.zeros((2, 1))
179179

180180
while SIM_TIME >= time:
181181
time += DT
@@ -189,7 +189,7 @@ def main():
189189
hxEst = np.hstack((hxEst, xEst))
190190
hxDR = np.hstack((hxDR, xDR))
191191
hxTrue = np.hstack((hxTrue, xTrue))
192-
hz = np.vstack((hz, z))
192+
hz = np.hstack((hz, z))
193193

194194
if show_animation:
195195
plt.cla()

Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb

Lines changed: 92 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,12 @@
3535
"\n",
3636
"In the code, \"xEst\" means the state vector. [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L168)\n",
3737
"\n",
38+
"And, $P_t$ is covariace matrix of the state,\n",
39+
"\n",
40+
"$Q$ is covariance matrix of process noise, \n",
41+
"\n",
42+
"$R$ is covariance matrix of observation noise at time $t$ \n",
43+
"\n",
3844
" \n",
3945
"\n",
4046
"The robot has a speed sensor and a gyro sensor.\n",
@@ -50,11 +56,6 @@
5056
"The input and observation vector includes sensor noise.\n",
5157
"\n",
5258
"In the code, \"observation\" function generates the input and observation vector with noise [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L34-L50)\n",
53-
"\n",
54-
"\n",
55-
"\n",
56-
"\n",
57-
"\n",
5859
"\n"
5960
]
6061
},
@@ -75,7 +76,7 @@
7576
"\n",
7677
"So, the motion model is\n",
7778
"\n",
78-
"$$\\textbf{x}_{t+1} = F\\textbf{x}+B\\textbf{u}$$\n",
79+
"$$\\textbf{x}_{t+1} = F\\textbf{x}_t+B\\textbf{u}_t$$\n",
7980
"\n",
8081
"where\n",
8182
"\n",
@@ -103,24 +104,103 @@
103104
"\n",
104105
"This is implemented at [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L53-L67)\n",
105106
"\n",
107+
"Its Javaobian matrix is\n",
108+
"\n",
109+
"$\\begin{equation*}\n",
110+
"J_F=\n",
111+
"\\begin{bmatrix}\n",
112+
"\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} & \\frac{dx}{dv}\\\\\n",
113+
"\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} & \\frac{dy}{dv}\\\\\n",
114+
"\\frac{d\\phi}{dx}& \\frac{d\\phi}{dy} & \\frac{d\\phi}{d\\phi} & \\frac{d\\phi}{dv}\\\\\n",
115+
"\\frac{dv}{dx}& \\frac{dv}{dy} & \\frac{dv}{d\\phi} & \\frac{dv}{dv}\\\\\n",
116+
"\\end{bmatrix}\n",
117+
"=\n",
118+
"\\begin{bmatrix}\n",
119+
"1& 0 & -v sin(\\phi)dt & cos(\\phi)dt\\\\\n",
120+
"0 & 1 & v cos(\\phi)dt & sin(\\phi) dt\\\\\n",
121+
"0 & 0 & 1 & 0\\\\\n",
122+
"0 & 0 & 0 & 1\\\\\n",
123+
"\\end{bmatrix}\n",
124+
"\\end{equation*}$\n"
125+
]
126+
},
127+
{
128+
"cell_type": "markdown",
129+
"metadata": {},
130+
"source": [
131+
"# Observation Model\n",
132+
"\n",
133+
"The robot can get x-y position infomation from GPS.\n",
134+
"\n",
135+
"So GPS Observation model is\n",
136+
"\n",
137+
"$$\\textbf{z}_{t} = H\\textbf{x}_t$$\n",
138+
"\n",
139+
"where\n",
140+
"\n",
141+
"$\\begin{equation*}\n",
142+
"B=\n",
143+
"\\begin{bmatrix}\n",
144+
"1 & 0 & 0& 0\\\\\n",
145+
"0 & 1 & 0& 0\\\\\n",
146+
"\\end{bmatrix}\n",
147+
"\\end{equation*}$\n",
148+
"\n",
149+
"Its Jacobian matrix is\n",
150+
"\n",
151+
"$\\begin{equation*}\n",
152+
"J_H=\n",
153+
"\\begin{bmatrix}\n",
154+
"\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} & \\frac{dx}{dv}\\\\\n",
155+
"\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} & \\frac{dy}{dv}\\\\\n",
156+
"\\end{bmatrix}\n",
157+
"=\n",
158+
"\\begin{bmatrix}\n",
159+
"1& 0 & 0 & 0\\\\\n",
160+
"0 & 1 & 0 & 0\\\\\n",
161+
"\\end{bmatrix}\n",
162+
"\\end{equation*}$\n",
106163
"\n"
107164
]
108165
},
109166
{
110167
"cell_type": "markdown",
111168
"metadata": {},
112169
"source": [
113-
"### Ref\n",
170+
"# Extented Kalman Filter\n",
114171
"\n",
115-
"- [PROBABILISTIC\\-ROBOTICS\\.ORG](http://www.probabilistic-robotics.org/)"
172+
"Localization process using Extendted Kalman Filter:EKF is\n",
173+
"\n",
174+
"=== Predict ===\n",
175+
"\n",
176+
"$x_{Pred} = Fx_t+Bu_t$\n",
177+
"\n",
178+
"$P_{Pred} = J_FP_t J_F^T + Q$\n",
179+
"\n",
180+
"=== Update ===\n",
181+
"\n",
182+
"$z_{Pred} = Hx_{Pred}$ \n",
183+
"\n",
184+
"$y = z - z_{Pred}$\n",
185+
"\n",
186+
"$ S = J_H P_{Pred}.J_H^T + R$\n",
187+
"\n",
188+
"$ K = P_{Pred}.J_H^T S^{-1}$\n",
189+
"\n",
190+
"\n",
191+
"$ x_{t+1} = x_{Pred} + Ky$\n",
192+
"\n",
193+
" $P_{t+1} = ( I - K J_H) P_{Pred} $"
116194
]
117195
},
118196
{
119-
"cell_type": "code",
120-
"execution_count": null,
197+
"cell_type": "markdown",
121198
"metadata": {},
122-
"outputs": [],
123-
"source": []
199+
"source": [
200+
"### Ref\n",
201+
"\n",
202+
"- [PROBABILISTIC\\-ROBOTICS\\.ORG](http://www.probabilistic-robotics.org/)"
203+
]
124204
}
125205
],
126206
"metadata": {

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,11 @@
1010
import numpy as np
1111
import matplotlib.pyplot as plt
1212

13+
import sys
14+
sys.path.append("../../")
15+
from matplotrecorder import matplotrecorder
16+
17+
1318
show_animation = True
1419

1520

@@ -202,6 +207,7 @@ def main():
202207
plt.axis("equal")
203208
plt.grid(True)
204209
plt.pause(0.0001)
210+
matplotrecorder.save_frame()
205211

206212
# check goal
207213
if math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2) <= config.robot_radius:
@@ -213,6 +219,10 @@ def main():
213219
plt.plot(traj[:, 0], traj[:, 1], "-r")
214220
plt.pause(0.0001)
215221

222+
for i in range(10):
223+
matplotrecorder.save_frame()
224+
matplotrecorder.save_movie("animation.gif", 0.1)
225+
216226
plt.show()
217227

218228

0 commit comments

Comments
 (0)