Skip to content
Snippets Groups Projects
Commit 29e0bab8 authored by Brian Hou's avatar Brian Hou
Browse files

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.
parent ffcfcee7
No related branches found
No related tags found
No related merge requests found
Pipeline #413771 failed with stage
in 5 minutes and 12 seconds
...@@ -49,6 +49,7 @@ def plot_particle_filter(): ...@@ -49,6 +49,7 @@ def plot_particle_filter():
plt.plot(estimates[:, 0], estimates[:, 1], c="r", label="Estimated State") plt.plot(estimates[:, 0], estimates[:, 1], c="r", label="Estimated State")
plt.plot(references[:, 0], references[:, 1], c="g", label="Ground Truth") 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.figtext(0.5, 0.9, caption, wrap=True, horizontalalignment="center", fontsize=12)
plt.legend()
plt.gca().set_aspect(aspect=1.0) plt.gca().set_aspect(aspect=1.0)
plt.show() plt.show()
......
...@@ -171,6 +171,23 @@ class TestMotionModel(unittest.TestCase): ...@@ -171,6 +171,23 @@ class TestMotionModel(unittest.TestCase):
err_msg="Linear motion should point in the direction of the current heading", 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): def test_compute_changes_steering_angle_threshold(self):
threshold = 1e-2 threshold = 1e-2
changes = self.motion_model.compute_changes( changes = self.motion_model.compute_changes(
...@@ -430,6 +447,47 @@ class TestMotionModel(unittest.TestCase): ...@@ -430,6 +447,47 @@ class TestMotionModel(unittest.TestCase):
msg="The particle dispersion didn't match our expectations given the controls", 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__": if __name__ == "__main__":
rosunit.unitrun("localization", "test_motion_model", TestMotionModel) rosunit.unitrun("localization", "test_motion_model", TestMotionModel)
...@@ -58,6 +58,7 @@ class TestParticleFilter(unittest.TestCase): ...@@ -58,6 +58,7 @@ class TestParticleFilter(unittest.TestCase):
plt.ylabel("y") plt.ylabel("y")
plt.plot(estimates[:, 0], estimates[:, 1], c="r", label="Estimated State") plt.plot(estimates[:, 0], estimates[:, 1], c="r", label="Estimated State")
plt.plot(references[:, 0], references[:, 1], c="g", label="Ground Truth") plt.plot(references[:, 0], references[:, 1], c="g", label="Ground Truth")
plt.legend()
plt.gca().set_aspect(aspect=1.0) plt.gca().set_aspect(aspect=1.0)
plt.show() plt.show()
......
...@@ -117,14 +117,15 @@ class TestResample(unittest.TestCase): ...@@ -117,14 +117,15 @@ class TestResample(unittest.TestCase):
# Create a set of particles with uniform weights # Create a set of particles with uniform weights
particles = np.arange(n_particles) particles = np.arange(n_particles)
particles = particles[..., np.newaxis] 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 = LowVarianceSampler(particles, weights)
rs.resample() rs.resample()
np.testing.assert_equal( np.testing.assert_equal(
weights, 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", err_msg="The resampler should set the weights to be uniform",
) )
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment