Update Project 2: Localization.

Fix minor bug in resample test: the weights array had the wrong shape.

Add more motion model tests to identify turning issues.

Show particle filter plot legend to label state estimate vs ground truth.

localization/scripts/make_particle_filter_plot

@@ -49,6 +49,7 @@ def plot_particle_filter():
     plt.plot(estimates[:, 0], estimates[:, 1], c="r", label="Estimated State")
     plt.plot(references[:, 0], references[:, 1], c="g", label="Ground Truth")
     plt.figtext(0.5, 0.9, caption, wrap=True, horizontalalignment="center", fontsize=12)
+    plt.legend()
     plt.gca().set_aspect(aspect=1.0)
     plt.show()
 

localization/test/motion_model.py

@@ -171,6 +171,23 @@ class TestMotionModel(unittest.TestCase):
             err_msg="Linear motion should point in the direction of the current heading",
         )
 
+    def test_compute_changes_turn_respects_current_heading(self):
+        self.num_particles = 1
+        self.particles = np.array([[0.0, 0.0, np.pi / 2]])
+        # Make the model deterministic
+        linear_control = 1.0
+        angular_control = np.pi / 4
+        dt = 1.0
+        changes = self.motion_model.compute_changes(
+            self.particles, np.array([[linear_control, angular_control]]), dt
+        )
+        np.testing.assert_allclose(
+            changes[0, :],
+            [-0.66, 0.037, 3.03],
+            atol=0.01,
+            err_msg="Angular motion should take into account the direction of the current heading",
+        )
+
     def test_compute_changes_steering_angle_threshold(self):
         threshold = 1e-2
         changes = self.motion_model.compute_changes(
@@ -430,6 +447,47 @@ class TestMotionModel(unittest.TestCase):
             msg="The particle dispersion didn't match our expectations given the controls",
         )
 
+    def test_apply_motion_model_turn(self):
+        self.num_particles = 1
+        self.particles = np.zeros((self.num_particles, 3))
+        # Make the model deterministic
+        self.motion_model = KinematicCarMotionModel(
+            1.0, delta_std=0.0, vel_std=0, x_std=0, y_std=0, theta_std=0
+        )
+        linear_control = 1.0
+        angular_control = 0.3
+        dt = 0.01
+        path = []
+        for i in range(10):
+            self.motion_model.apply_motion_model(
+                self.particles, linear_control, angular_control, dt
+            )
+            path.append(self.particles[0].copy())
+        path = np.array(path)
+        # First diff is displacement. Displacement over time is velocity
+        velocities = np.diff(path, axis=0) / dt
+        accelerations = np.diff(velocities, axis=0) / dt
+        # Magnitude of the vector formed by the individual x and y acceleration components
+        linear_velocity = np.linalg.norm(velocities[:, [0, 1]], axis=1)
+        np.testing.assert_allclose(
+            linear_velocity,
+            linear_control,
+            atol=0.001,
+            err_msg="The linear velocity of the trajectory should remain constant",
+        )
+        # angular velocity ~= lin * angcontrol, only for small angular velocities
+        np.testing.assert_allclose(
+            velocities[:, 2],
+            angular_control * linear_control,
+            atol=0.05 * np.abs(angular_control),
+            err_msg="The angular velocity of the trajectory should remain constant",
+        )
+        np.testing.assert_almost_equal(
+            accelerations[:, 2],
+            0,
+            err_msg="There should be no angular acceleration for a constant velocity angular control",
+        )
+
 
 if __name__ == "__main__":
     rosunit.unitrun("localization", "test_motion_model", TestMotionModel)

localization/test/particle_filter.py

@@ -58,6 +58,7 @@ class TestParticleFilter(unittest.TestCase):
             plt.ylabel("y")
             plt.plot(estimates[:, 0], estimates[:, 1], c="r", label="Estimated State")
             plt.plot(references[:, 0], references[:, 1], c="g", label="Ground Truth")
+            plt.legend()
             plt.gca().set_aspect(aspect=1.0)
             plt.show()
 

localization/test/resample.py

@@ -117,14 +117,15 @@ class TestResample(unittest.TestCase):
         # Create a set of particles with uniform weights
         particles = np.arange(n_particles)
         particles = particles[..., np.newaxis]
-        weights = np.random.uniform([n_particles], 1.0 / n_particles)
+        weights = np.random.uniform(0, 1, n_particles)
+        weights /= weights.sum()
 
         rs = LowVarianceSampler(particles, weights)
         rs.resample()
 
         np.testing.assert_equal(
             weights,
-            np.full_like(weights, 1.0 / n_particles),
+            np.full(n_particles, 1.0 / n_particles),
             err_msg="The resampler should set the weights to be uniform",
         )