github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-12 22:08:33 -05:00
Code Issues Packages Projects Releases Wiki Activity

Fix parameter of jacobian in EKF-SLAM

Browse Source
This commit is contained in:
Michael Dobler
2020-03-24 12:59:49 +01:00
parent 4093b642e0
commit e880b0efe4
2 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
SLAM/EKFSLAM/ekf_slam.ipynb
View File

@@ -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",

2
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -29,8 +29,8 @@ show_animation = True
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)