mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
finish notebook
This commit is contained in:
Atsushi Sakai
@@ -11,12 +11,12 @@ import math
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Estimation parameter of EKF
|
||||
Q = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
|
||||
R = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
|
||||
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
|
||||
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
|
||||
|
||||
# Simulation parameter
|
||||
Qsim = np.diag([0.5, 0.5])**2
|
||||
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
|
||||
Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
|
||||
Rsim = np.diag([0.5, 0.5])**2
|
||||
|
||||
DT = 0.1 # time tick [s]
|
||||
SIM_TIME = 50.0 # simulation time [s]
|
||||
@@ -36,13 +36,13 @@ def observation(xTrue, xd, u):
|
||||
xTrue = motion_model(xTrue, u)
|
||||
|
||||
# add noise to gps x-y
|
||||
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
|
||||
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
|
||||
z = np.array([[zx, zy]])
|
||||
zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0]
|
||||
zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1]
|
||||
z = np.array([[zx, zy]]).T
|
||||
|
||||
# add noise to input
|
||||
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
|
||||
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
|
||||
ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
|
||||
ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
|
||||
ud = np.array([[ud1, ud2]]).T
|
||||
|
||||
xd = motion_model(xd, ud)
|
||||
@@ -120,13 +120,13 @@ def ekf_estimation(xEst, PEst, z, u):
|
||||
# Predict
|
||||
xPred = motion_model(xEst, u)
|
||||
jF = jacobF(xPred, u)
|
||||
PPred = jF@PEst@jF.T + R
|
||||
PPred = jF@PEst@jF.T + Q
|
||||
|
||||
# Update
|
||||
jH = jacobH(xPred)
|
||||
zPred = observation_model(xPred)
|
||||
y = z.T - zPred
|
||||
S = jH@PPred@jH.T + Q
|
||||
y = z - zPred
|
||||
S = jH@PPred@jH.T + R
|
||||
K = PPred@jH.T@np.linalg.inv(S)
|
||||
xEst = xPred + K@y
|
||||
PEst = (np.eye(len(xEst)) - K@jH)@PPred
|
||||
@@ -175,7 +175,7 @@ def main():
|
||||
hxEst = xEst
|
||||
hxTrue = xTrue
|
||||
hxDR = xTrue
|
||||
hz = np.zeros((1, 2))
|
||||
hz = np.zeros((2, 1))
|
||||
|
||||
while SIM_TIME >= time:
|
||||
time += DT
|
||||
@@ -189,7 +189,7 @@ def main():
|
||||
hxEst = np.hstack((hxEst, xEst))
|
||||
hxDR = np.hstack((hxDR, xDR))
|
||||
hxTrue = np.hstack((hxTrue, xTrue))
|
||||
hz = np.vstack((hz, z))
|
||||
hz = np.hstack((hz, z))
|
||||
|
||||
if show_animation:
|
||||
plt.cla()
|
||||
|
||||
@@ -35,6 +35,12 @@
|
||||
"\n",
|
||||
"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",
|
||||
"\n",
|
||||
"And, $P_t$ is covariace matrix of the state,\n",
|
||||
"\n",
|
||||
"$Q$ is covariance matrix of process noise, \n",
|
||||
"\n",
|
||||
"$R$ is covariance matrix of observation noise at time $t$ \n",
|
||||
"\n",
|
||||
" \n",
|
||||
"\n",
|
||||
"The robot has a speed sensor and a gyro sensor.\n",
|
||||
@@ -50,11 +56,6 @@
|
||||
"The input and observation vector includes sensor noise.\n",
|
||||
"\n",
|
||||
"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",
|
||||
"\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
@@ -75,7 +76,7 @@
|
||||
"\n",
|
||||
"So, the motion model is\n",
|
||||
"\n",
|
||||
"$$\\textbf{x}_{t+1} = F\\textbf{x}+B\\textbf{u}$$\n",
|
||||
"$$\\textbf{x}_{t+1} = F\\textbf{x}_t+B\\textbf{u}_t$$\n",
|
||||
"\n",
|
||||
"where\n",
|
||||
"\n",
|
||||
@@ -103,9 +104,95 @@
|
||||
"\n",
|
||||
"This is implemented at [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L53-L67)\n",
|
||||
"\n",
|
||||
"Its Javaobian matrix is\n",
|
||||
"\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"J_F=\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} & \\frac{dx}{dv}\\\\\n",
|
||||
"\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} & \\frac{dy}{dv}\\\\\n",
|
||||
"\\frac{d\\phi}{dx}& \\frac{d\\phi}{dy} & \\frac{d\\phi}{d\\phi} & \\frac{d\\phi}{dv}\\\\\n",
|
||||
"\\frac{dv}{dx}& \\frac{dv}{dy} & \\frac{dv}{d\\phi} & \\frac{dv}{dv}\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"=\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"1& 0 & -v sin(\\phi)dt & cos(\\phi)dt\\\\\n",
|
||||
"0 & 1 & v cos(\\phi)dt & sin(\\phi) dt\\\\\n",
|
||||
"0 & 0 & 1 & 0\\\\\n",
|
||||
"0 & 0 & 0 & 1\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Observation Model\n",
|
||||
"\n",
|
||||
"The robot can get x-y position infomation from GPS.\n",
|
||||
"\n",
|
||||
"So GPS Observation model is\n",
|
||||
"\n",
|
||||
"$$\\textbf{z}_{t} = H\\textbf{x}_t$$\n",
|
||||
"\n",
|
||||
"where\n",
|
||||
"\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"B=\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"1 & 0 & 0& 0\\\\\n",
|
||||
"0 & 1 & 0& 0\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$\n",
|
||||
"\n",
|
||||
"Its Jacobian matrix is\n",
|
||||
"\n",
|
||||
"$\\begin{equation*}\n",
|
||||
"J_H=\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} & \\frac{dx}{dv}\\\\\n",
|
||||
"\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} & \\frac{dy}{dv}\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"=\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
"1& 0 & 0 & 0\\\\\n",
|
||||
"0 & 1 & 0 & 0\\\\\n",
|
||||
"\\end{bmatrix}\n",
|
||||
"\\end{equation*}$\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Extented Kalman Filter\n",
|
||||
"\n",
|
||||
"Localization process using Extendted Kalman Filter:EKF is\n",
|
||||
"\n",
|
||||
"=== Predict ===\n",
|
||||
"\n",
|
||||
"$x_{Pred} = Fx_t+Bu_t$\n",
|
||||
"\n",
|
||||
"$P_{Pred} = J_FP_t J_F^T + Q$\n",
|
||||
"\n",
|
||||
"=== Update ===\n",
|
||||
"\n",
|
||||
"$z_{Pred} = Hx_{Pred}$ \n",
|
||||
"\n",
|
||||
"$y = z - z_{Pred}$\n",
|
||||
"\n",
|
||||
"$ S = J_H P_{Pred}.J_H^T + R$\n",
|
||||
"\n",
|
||||
"$ K = P_{Pred}.J_H^T S^{-1}$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"$ x_{t+1} = x_{Pred} + Ky$\n",
|
||||
"\n",
|
||||
" $P_{t+1} = ( I - K J_H) P_{Pred} $"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
@@ -114,13 +201,6 @@
|
||||
"\n",
|
||||
"- [PROBABILISTIC\\-ROBOTICS\\.ORG](http://www.probabilistic-robotics.org/)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
||||
@@ -10,6 +10,11 @@ import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
import sys
|
||||
sys.path.append("../../")
|
||||
from matplotrecorder import matplotrecorder
|
||||
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
@@ -202,6 +207,7 @@ def main():
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
plt.pause(0.0001)
|
||||
matplotrecorder.save_frame()
|
||||
|
||||
# check goal
|
||||
if math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2) <= config.robot_radius:
|
||||
@@ -213,6 +219,10 @@ def main():
|
||||
plt.plot(traj[:, 0], traj[:, 1], "-r")
|
||||
plt.pause(0.0001)
|
||||
|
||||
for i in range(10):
|
||||
matplotrecorder.save_frame()
|
||||
matplotrecorder.save_movie("animation.gif", 0.1)
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user