Merge pull request AtsushiSakai#305 from m-dobler/fix-jacobian

EKF-SLAM: Fix parameter of Jacobian of motion model

Author: AtsushiSakai

SLAM/EKFSLAM/ekf_slam.ipynb

@@ -305,8 +305,8 @@
     "    :returns:    predicted state vector, predicted covariance, jacobian of control vector, transition fx\n",
     "    \"\"\"\n",
     "    S = STATE_SIZE\n",
-    "    xEst[0:S] = motion_model(xEst[0:S], u)\n",
     "    G, Fx = jacob_motion(xEst[0:S], u)\n",
+    "    xEst[0:S] = motion_model(xEst[0:S], u)\n",
     "    # Fx is an an identity matrix of size (STATE_SIZE)\n",
     "    # sigma = G*sigma*G.T + Noise\n",
     "    PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx\n",

SLAM/EKFSLAM/ekf_slam.py

@@ -29,8 +29,8 @@
 def ekf_slam(xEst, PEst, u, z):
     # Predict
     S = STATE_SIZE
-    xEst[0:S] = motion_model(xEst[0:S], u)
     G, Fx = jacob_motion(xEst[0:S], u)
+    xEst[0:S] = motion_model(xEst[0:S], u)
     PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
     initP = np.eye(2)