Fix EKF SLAM covariance prediction Jacobian orientation

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

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

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