github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 10:48:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #258 from m-dobler/master

Browse Source 
EKF-SLAM: Fix prediction step of motion
This commit is contained in:
Atsushi Sakai
2019-12-07 19:41:32 +09:00
committed by GitHub
parent b80b7c9833 99c784948d
commit 410304a529
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
SLAM/EKFSLAM/ekf_slam.ipynb
View File

@@ -309,7 +309,7 @@
" G, Fx = jacob_motion(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",
" PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx\n",
" return xEst, PEst, G, Fx"
]
},
@@ -584,7 +584,7 @@
" [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],\n",
" [0.0, 0.0, 0.0]])\n",
"\n",
" G = np.eye(STATE_SIZE) + Fx.T * jF * Fx\n",
" G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx\n",
" if calc_n_LM(x) > 0:\n",
" print(Fx.shape)\n",
" return G, Fx,\n",

4
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -31,7 +31,7 @@ def ekf_slam(xEst, PEst, u, z):
S = STATE_SIZE
xEst[0:S] = motion_model(xEst[0:S], u)
G, Fx = jacob_motion(xEst[0:S], u)
PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx
PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
initP = np.eye(2)
# Update
@@ -119,7 +119,7 @@ def jacob_motion(x, u):
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]])
G = np.eye(STATE_SIZE) + Fx.T * jF * Fx
G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
return G, Fx,