Fix jacobian of observation

Author: m-dobler

SLAM/EKFSLAM/ekf_slam.ipynb

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

SLAM/EKFSLAM/ekf_slam.py

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