github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04: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.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)