From 95eeec3d6f74ff467e4c3bcab6c9f323dd9efbc3 Mon Sep 17 00:00:00 2001 From: yunyv <62236062+yunyv@users.noreply.github.com> Date: Mon, 26 Oct 2026 04:06:34 -0600 Subject: [PATCH 1/3] Fix covariance update equation in ekf_slam functionFix EKF SLAM covariance prediction Jacobian orientation --- SLAM/EKFSLAM/ekf_slam.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 5c4417d7c3..1d54afa6b3 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -31,7 +31,7 @@ def ekf_slam(xEst, PEst, u, z): # Predict G, Fx = jacob_motion(xEst, u) xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u) - PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx + PEst = G @ PEst @ G.T + Fx.T @ Cx @ Fx initP = np.eye(2) # Update From 6a3eb4f893e40fe6c5acdf9086e66c5ccd7db7c2 Mon Sep 17 00:00:00 2001 From: yunyv <62236062+yunyv@users.noreply.github.com> Date: Mon, 26 Oct 2026 04:07:10 -0600 Subject: [PATCH 2/3] Test EKF SLAM prediction against numerical motion Jacobian --- tests/test_ekf_slam.py | 44 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/tests/test_ekf_slam.py b/tests/test_ekf_slam.py index 4181bb64ba..aaae523361 100644 --- a/tests/test_ekf_slam.py +++ b/tests/test_ekf_slam.py @@ -1,4 +1,6 @@ import conftest +import numpy as np +import pytest from SLAM.EKFSLAM import ekf_slam as m @@ -7,5 +9,47 @@ def test_1(): m.main() +@pytest.mark.parametrize("landmark_count", [0, 1, 2]) +@pytest.mark.parametrize("yaw", [0.0, 0.7, -1.2]) +@pytest.mark.parametrize("velocity", [0.0, 2.0]) +def test_prediction_covariance_matches_motion_linearization( + landmark_count, yaw, velocity): + state_size = m.STATE_SIZE + m.LM_SIZE * landmark_count + state = np.arange(state_size, dtype=float).reshape(-1, 1) + state[2, 0] = yaw + control = np.array([[velocity], [0.3]]) + # Include pose/map correlations, not just a diagonal pose covariance. + factor = np.arange(1, state_size ** 2 + 1, dtype=float).reshape( + state_size, state_size) / state_size + covariance = factor @ factor.T + np.eye(state_size) + + def predict_state(value): + predicted = value.copy() + predicted[:m.STATE_SIZE] = m.motion_model( + value[:m.STATE_SIZE], control) + return predicted + + # Differentiate the actual state transition independently of jacob_motion. + step = 1e-5 + jacobian = np.empty((state_size, state_size)) + for column in range(state_size): + perturbation = np.zeros_like(state) + perturbation[column, 0] = step + jacobian[:, column] = ( + (predict_state(state + perturbation) + - predict_state(state - perturbation)) / (2.0 * step) + ).ravel() + expected_covariance = jacobian @ covariance @ jacobian.T + expected_covariance[:m.STATE_SIZE, :m.STATE_SIZE] += m.Cx + + predicted_state, predicted_covariance = m.ekf_slam( + state.copy(), covariance.copy(), control, np.empty((0, 3))) + + np.testing.assert_allclose(predicted_state, predict_state(state)) + np.testing.assert_allclose(predicted_covariance, expected_covariance, + rtol=1e-9, atol=1e-9) + np.testing.assert_allclose(predicted_covariance[3:, 3:], covariance[3:, 3:]) + + if __name__ == '__main__': conftest.run_this_test(__file__) From 9e34ee8e2c2bb5e3ea800a7697035fc6b37542c4 Mon Sep 17 00:00:00 2001 From: yunyv <62236062+yunyv@users.noreply.github.com> Date: Mon, 26 Oct 2026 04:07:42 -0600 Subject: [PATCH 3/3] Align EKF SLAM documentation with covariance prediction --- docs/modules/4_slam/ekf_slam/ekf_slam_main.rst | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst index 05bd636ef5..869bafe788 100644 --- a/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst +++ b/docs/modules/4_slam/ekf_slam/ekf_slam_main.rst @@ -161,14 +161,15 @@ The implementation of the motion model prediction code is shown in uses (or doesn’t use) the process noise ``Rsim`` to the find the ground truth and dead reckoning estimates of the pose. -**Predict covariance:** Add the state covariance to the the current -uncertainty of the EKF. At each time step, the uncertainty in the system -grows by the covariance of the pose, :math:`Cx`. +**Predict covariance:** Propagate the covariance through the motion model +using its state Jacobian :math:`G`, then add process noise to the pose. +Here :math:`F_x` selects the pose from the full pose-and-landmark state. -:math:`P = G^TPG + Cx` +:math:`P = G P G^T + F_x^T Cx F_x` -Notice this uncertainty is only growing with respect to the pose, not -the landmarks. +This propagates heading uncertainty into position uncertainty and updates +the pose-landmark cross-covariances. The landmark-landmark covariance is +unchanged because the landmarks are stationary. .. code:: ipython3 @@ -183,9 +184,8 @@ the landmarks. """ G, Fx = jacob_motion(xEst, u) xEst[0:STATE_SIZE] = motion_model(xEst[0:STATE_SIZE], u) - # Fx is an an identity matrix of size (STATE_SIZE) - # sigma = G*sigma*G.T + Noise - PEst = G.T @ PEst @ G + Fx.T @ Cx @ Fx + # Fx selects the pose from the full state + PEst = G @ PEst @ G.T + Fx.T @ Cx @ Fx return xEst, PEst .. code:: ipython3