def advance_particle(self, particle):
old_state = [particle.x, particle.y, particle.theta]
velocity = self.current_velocity
steering = self.current_steering
# Add controls noise:
if self.add_controls_noise:
velocity += np.random.normal(
loc=0.0, scale=self.velocity_noise_stdev)
steering += np.random.normal(
loc=0.0, scale=self.steering_noise_stdev)
steering = normalize_radians(steering)
# Dynamics update:
prev_timestamp = 0.0
if self.current_ts > 0:
prev_timestamp = self.dataset.timestamps[self.current_ts - 1]
delta_t = self.dataset.timestamps[self.current_ts] - prev_timestamp
assert delta_t > 0
new_state = draw_dr.dynamics(
self.dataset, old_state, velocity, steering, delta_t)
# Add dynamics noise:
if self.add_dynamics_noise:
new_state[0] += np.random.normal(
loc=0.0, scale=self.x_noise_stdev)
new_state[1] += np.random.normal(
loc=0.0, scale=self.y_noise_stdev)
new_state[2] += np.random.normal(
loc=0.0, scale=self.theta_noise_stdev)
new_state[2] = normalize_radians(new_state[2])
# Extract obstacles.
obstacles = particle.obstacles
if (len(obstacles) == 0 and
self.dataset.ts2sensor[self.current_ts] == 'laser'):
laser_x, laser_y, laser_theta = lasers.car_loc_to_laser_loc(
particle.x, particle.y, particle.theta,
self.dataset.a, self.dataset.b)
obstacles = ransac.extract_obstacles(
laser_x, laser_y, laser_theta,
self.laser_angles, self.laser_max_range,
np.array(self.dataset.ts2laser[self.current_ts]))
# print "old obstacles:", particle.obstacles
# print "new obstacles:", obstacles
return LocPFParticle(
new_state[0], new_state[1], new_state[2], obstacles)
def dynamics(dataset, old_state, encoder_velocity, steering_angle, delta_t):
"""
Apply dynamics model and return new state.
The model is taken from guivant_2006, equations 5 and 6.
The model is non-linear and noise-free.
state is assumed to be a column vector of [x, y, theta].
steering_angle is in radians.
"""
a, b, h, L = dataset.a, dataset.b, dataset.h, dataset.L
# print
# print "controls:", encoder_velocity, steering_angle
# print "old_state:", old_state
x, y, theta = old_state
# Translate velocity from encoder to center of back axle:
velocity = encoder_velocity / (1 - np.tan(steering_angle) * h / L)
# Compute new xdot, ydot, thetadot:
xdot = (
velocity * np.cos(theta) -
(velocity / L) *
(a * np.sin(theta) + b * np.cos(theta)) *
np.tan(steering_angle))
ydot = (
velocity * np.sin(theta) +
(velocity / L) *
(a * np.cos(theta) - b * np.sin(theta)) *
np.tan(steering_angle))
thetadot = (velocity / L) * np.tan(steering_angle)
# Compute new x, y, theta:
new_x = x + delta_t * xdot
new_y = y + delta_t * ydot
new_theta = theta + delta_t * thetadot
new_theta = normalize_radians(new_theta)
new_state = [new_x, new_y, new_theta]
# print "new_state:", new_state
return new_state
