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