From 7560cbdca8b88166bf321feee0279c9a334d9eef Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 18 Nov 2018 08:32:42 +0900 Subject: [PATCH] add @ operator instead of dot function --- .../extended_kalman_filter.py | 16 +++--- .../extended_kalman_filter_localization.ipynb | 54 +++++++++++++++++-- 2 files changed, 59 insertions(+), 11 deletions(-) diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index 3004692c..3e28aab4 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -62,7 +62,7 @@ def motion_model(x, u): [0.0, DT], [1.0, 0.0]]) - x = F.dot(x) + B.dot(u) + x = F@x + B@u return x @@ -74,7 +74,7 @@ def observation_model(x): [0, 1, 0, 0] ]) - z = H.dot(x) + z = H@x return z @@ -120,16 +120,16 @@ def ekf_estimation(xEst, PEst, z, u): # Predict xPred = motion_model(xEst, u) jF = jacobF(xPred, u) - PPred = jF.dot(PEst).dot(jF.T) + R + PPred = jF@PEst@jF.T + R # Update jH = jacobH(xPred) zPred = observation_model(xPred) y = z.T - zPred - S = jH.dot(PPred).dot(jH.T) + Q - K = PPred.dot(jH.T).dot(np.linalg.inv(S)) - xEst = xPred + K.dot(y) - PEst = (np.eye(len(xEst)) - K.dot(jH)).dot(PPred) + S = jH@PPred@jH.T + Q + K = PPred@jH.T@np.linalg.inv(S) + xEst = xPred + K@y + PEst = (np.eye(len(xEst)) - K@jH)@PPred return xEst, PEst @@ -153,7 +153,7 @@ def plot_covariance_ellipse(xEst, PEst): angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0]) R = np.array([[math.cos(angle), math.sin(angle)], [-math.sin(angle), math.cos(angle)]]) - fx = R.dot(np.array([[x, y]])) + fx = R@(np.array([x, y])) px = np.array(fx[0, :] + xEst[0, 0]).flatten() py = np.array(fx[1, :] + xEst[1, 0]).flatten() plt.plot(px, py, "--r") diff --git a/Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb b/Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb index 09376a24..7281bd8e 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb +++ b/Localization/extended_kalman_filter/extended_kalman_filter_localization.ipynb @@ -25,7 +25,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "### Filter Design\n", + "### Filter design\n", "\n", "In this simulation, the robot has a state vector includes 4 states at time $t$.\n", "\n", @@ -33,7 +33,7 @@ "\n", "x, y are a 2D x-y position, $\\theta$ is orientation, and v is velocity.\n", "\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", + "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", "　\n", "\n", @@ -49,7 +49,7 @@ "\n", "The input and observation vector includes sensor noise.\n", "\n", - "In the code, observation function generates the input and observation vector [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L34-L50)\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", @@ -58,6 +58,54 @@ "\n" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Motion Model\n", + "\n", + "The robot model is \n", + "\n", + "$$ \\dot{x} = vcos(\\phi)$$\n", + "\n", + "$$ \\dot{y} = vsin((\\phi)$$\n", + "\n", + "$$ \\dot{\\phi} = \\omega$$\n", + "\n", + "\n", + "So, the motion model is\n", + "\n", + "$$\\textbf{x}_{t+1} = F\\textbf{x}+B\\textbf{u}$$\n", + "\n", + "where\n", + "\n", + "$\\begin{equation*}\n", + "F=\n", + "\\begin{bmatrix}\n", + "1 & 0 & 0 & 0\\\\\n", + "0 & 1 & 0 & 0\\\\\n", + "0 & 0 & 1 & 0 \\\\\n", + "0 & 0 & 0 & 0 \\\\\n", + "\\end{bmatrix}\n", + "\\end{equation*}$\n", + "\n", + "$\\begin{equation*}\n", + "B=\n", + "\\begin{bmatrix}\n", + "sin(\\phi)dt & 0\\\\\n", + "cos(\\phi)dt & 0\\\\\n", + "0 & dt\\\\\n", + "1 & 0\\\\\n", + "\\end{bmatrix}\n", + "\\end{equation*}$\n", + "\n", + "$dt$ is a time interval.\n", + "\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", + "\n" + ] + }, { "cell_type": "markdown", "metadata": {},