fix(mobile): retain reverse motion in vehicle odometry

src/roboticstoolbox/mobile/Vehicle.py

@@ -804,6 +804,9 @@ def step(
 
         .. note:: Vehicle control input limits are applied.
 
+        The distance component of the odometry is signed: reverse motion gives
+        a negative distance.
+
         .. seealso:: :func:`control`, ``update``, :func:`run`
         """
         # determine vehicle control
@@ -814,6 +817,8 @@ def step(
 
         # update state
         xd = self._dt * self.deriv(self._x, u)  # delta state
+        theta = self._x[2]
+        distance = xd[0] * cos(theta) + xd[1] * sin(theta)
 
         # update state vector
         self._x += xd
@@ -822,7 +827,7 @@ def step(
         # print('VEH', u, self.x)
 
         # odometry comes from change in state vector
-        odo = np.r_[np.linalg.norm(xd[0:2]), xd[2]]
+        odo = np.r_[distance, xd[2]]
 
         if self._V is not None:
             odo += self.random.multivariate_normal((0, 0), self._V)

tests/test_vehicle_estimators.py

@@ -22,6 +22,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 import numpy.testing as nt
+import pytest
 
 from roboticstoolbox import (
     EKF,
@@ -32,6 +33,7 @@
     RandomPath,
     RangeBearingSensor,
     Unicycle,
+    VehicleBase,
 )
 
 T = TypeVar("T")
@@ -55,6 +57,28 @@ def numerical_jacobian(
     return np.column_stack(cols)
 
 
+@pytest.mark.parametrize(
+    "model, control",
+    [
+        (Bicycle, (-1.0, 0.2)),
+        (Unicycle, (-1.0, 0.2)),
+        (DiffSteer, (-1.2, -0.8)),
+        (DiffSteer, (0.2, -0.6)),
+    ],
+)
+@pytest.mark.parametrize("direction", [-1, 1])
+def test_step_odometry_reconstructs_forward_and_reverse_motion(
+    model: type[VehicleBase], control: tuple[float, float], direction: int
+) -> None:
+    vehicle = model(x0=[2.0, -3.0, 1.2], dt=0.1)
+    initial = vehicle.x.copy()
+
+    odometry = vehicle.step(np.array(control) * direction, animate=False)
+
+    nt.assert_allclose(vehicle.f(initial, odometry), vehicle.x, atol=1e-14)
+    assert np.sign(odometry[0]) == -direction
+
+
 class TestBicycle(unittest.TestCase):
     def test_deriv(self) -> None:
         bike = Bicycle(L=2)