From e880b0efe448e7024f22bd3f70c26fe8ba7ef075 Mon Sep 17 00:00:00 2001 From: Michael Dobler Date: Tue, 24 Mar 2020 12:59:49 +0100 Subject: [PATCH] Fix parameter of jacobian in EKF-SLAM --- SLAM/EKFSLAM/ekf_slam.ipynb | 2 +- SLAM/EKFSLAM/ekf_slam.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/SLAM/EKFSLAM/ekf_slam.ipynb b/SLAM/EKFSLAM/ekf_slam.ipynb index ccf5e5bb..a64c145a 100644 --- a/SLAM/EKFSLAM/ekf_slam.ipynb +++ b/SLAM/EKFSLAM/ekf_slam.ipynb @@ -305,8 +305,8 @@ " :returns: predicted state vector, predicted covariance, jacobian of control vector, transition fx\n", " \"\"\"\n", " S = STATE_SIZE\n", - " xEst[0:S] = motion_model(xEst[0:S], u)\n", " G, Fx = jacob_motion(xEst[0:S], u)\n", + " xEst[0:S] = motion_model(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", diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 3038e40a..6b349d85 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -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)