Commit c5bc3bfe 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
Pipeline #413734 failed with stage
in 5 minutes and 13 seconds
......@@ -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()
......
......@@ -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.46, 0.84, 1],
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,49 @@ 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 = 10.0
angular_control = 0.1
dt = 0.01
path = []
for i in range(20):
self.motion_model.apply_motion_model(
self.particles, linear_control, angular_control, dt
)
path.append(self.particles[0].copy())
path = np.array(path)
velocities = np.diff(path, axis=0)
accelerations = np.diff(path, n=2, axis=0)
jerk = np.diff(path, n=3, axis=0)
# 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,
0.1,
atol=0.001,
err_msg="The linear velocity of the trajectory should remain constant",
)
np.testing.assert_allclose(
velocities[:, 2],
0.01,
atol=0.01,
err_msg="The angular velocity of the trajectory should remain constant",
)
np.testing.assert_equal(
accelerations[:, 2],
0,
err_msg="There should be no angular acceleration for a constant velocity angular control",
)
np.testing.assert_allclose(
jerk, 0, atol=0.01, err_msg="There should be no jerk in the motion"
)
if __name__ == "__main__":
rosunit.unitrun("localization", "test_motion_model", TestMotionModel)
......@@ -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()
......
......@@ -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",
)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment