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

Merge pull request #255 from m-dobler/fix-jacobian-of-observation

Browse Source 
EKF-SLAM: Fix jacobian of observation
This commit is contained in:
Atsushi Sakai
2019-11-29 22:55:24 +09:00
committed by GitHub
parent 54991efb8f 2fe00f8824
commit c29d425785
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

@@ -479,7 +479,7 @@
" \"\"\"\n",
" sq = math.sqrt(q)\n",
" G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],\n",
" [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])\n",
" [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])\n",
"\n",
" G = G / q\n",
" nLM = calc_n_LM(x)\n",

2
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -177,7 +177,7 @@ def calc_innovation(lm, xEst, PEst, z, LMid):
def jacob_h(q, delta, x, i):
sq = math.sqrt(q)
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
[delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
[delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])
G = G / q
nLM = calc_n_lm(x)