From 99c784948d9cfc8145ae208d5301c6bd1144f401 Mon Sep 17 00:00:00 2001 From: Michael Dobler Date: Fri, 6 Dec 2019 11:35:29 +0100 Subject: [PATCH] Use matrix multiplication instead of elementwise --- SLAM/EKFSLAM/ekf_slam.ipynb | 4 ++-- SLAM/EKFSLAM/ekf_slam.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/SLAM/EKFSLAM/ekf_slam.ipynb b/SLAM/EKFSLAM/ekf_slam.ipynb index f8009395..ccf5e5bb 100644 --- a/SLAM/EKFSLAM/ekf_slam.ipynb +++ b/SLAM/EKFSLAM/ekf_slam.ipynb @@ -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", diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 27e50354..22ccd8d6 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -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,