add @ operator instead of dot function

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")

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": {},