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 jacobian of observation

Browse Source
This commit is contained in:
Michael Dobler
2019-11-28 23:35:55 +01:00
parent 54991efb8f
commit 2fe00f8824
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

@@ -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)