def predictor_init(self) -> None: """Initialize the LPV models used for interval prediction.""" position_i = self.interval.position target_lane = self.road.network.get_lane(self.target_lane_index) longi_i, lat_i = interval_absolute_to_local(position_i, target_lane) v_i = self.interval.speed psi_i = self.interval.heading - self.lane.heading_at(longi_i.mean()) # Longitudinal predictor if not self.longitudinal_lpv: front_interval = self.get_front_interval() # LPV specification if front_interval: f_longi_i, _ = interval_absolute_to_local( front_interval.position, target_lane) f_pos = f_longi_i[0] f_vel = front_interval.speed[0] else: f_pos, f_vel = 0, 0 x0 = [longi_i[0], f_pos, v_i[0], f_vel] center = [ -self.DISTANCE_WANTED - self.target_speed * self.TIME_WANTED, 0, self.target_speed, self.target_speed ] noise = 1 b = np.eye(4) d = np.array([[1], [0], [0], [0]]) omega_i = np.array([[-1], [1]]) * noise u = [[self.target_speed], [self.target_speed], [0], [0]] a0, da = self.longitudinal_matrix_polytope() self.longitudinal_lpv = LPV(x0, a0, da, b, d, omega_i, u, center=center) # Lateral predictor if not self.lateral_lpv: # LPV specification x0 = [lat_i[0], psi_i[0]] center = [0, 0] noise = 0.5 b = np.identity(2) d = np.array([[1], [0]]) omega_i = np.array([[-1], [1]]) * noise u = [[0], [0]] a0, da = self.lateral_matrix_polytope() self.lateral_lpv = LPV(x0, a0, da, b, d, omega_i, u, center=center)
def predictor_init(self): """ Initialize the LPV models used for interval prediction """ position_i = self.interval.position target_lane = self.road.network.get_lane(self.target_lane_index) longi_i, lat_i = interval_absolute_to_local(position_i, target_lane) v_i = self.interval.velocity psi_i = self.interval.heading - self.lane.heading_at(longi_i.mean()) # Longitudinal predictor if not self.longitudinal_lpv: front_interval = self.get_front_interval() # Parameters interval params_i = self.theta_a_i.copy() # TODO: for now, we assume Kx is known params_i[:, 2] = params_i.mean(axis=0)[2] # Structure and features a, phi = self.get_longitudinal_features() # Polytope a_theta = lambda params: a + np.tensordot(phi, params, axes=[0, 0]) a0, da = polytope(a_theta, params_i) # LPV specification if front_interval: f_longi_i, f_lat_i = interval_absolute_to_local( front_interval.position, target_lane) f_pos = f_longi_i[0] f_vel = front_interval.velocity[0] else: f_pos, f_vel = 0, 0 x0 = [longi_i[0], f_pos, v_i[0], f_vel] center = [ -self.DISTANCE_WANTED - self.target_velocity * self.TIME_WANTED, 0, self.target_velocity, self.target_velocity ] d = [self.target_velocity, self.target_velocity, 0, 0] self.longitudinal_lpv = LPV(x0, a0, da, d, center) # Lateral predictor if not self.lateral_lpv: # Parameters interval params_i = self.theta_b_i.copy() # Matrix polytope a, phi = self.get_lateral_features() a_theta = lambda params: a + np.tensordot( phi, params, axes=[0, 0]) a0, da = polytope(a_theta, params_i) # LPV specification x0 = [lat_i[0], psi_i[0]] center = [0, 0] d = [0, 0] self.lateral_lpv = LPV(x0, a0, da, d, center)
def predictor_init(self): """ Initialize the LPV models used for interval prediction """ position_i = self.interval.position target_lane = self.road.network.get_lane(self.target_lane_index) longi_i, lat_i = interval_absolute_to_local(position_i, target_lane) v_i = self.interval.velocity psi_i = self.interval.heading - self.lane.heading_at(longi_i.mean()) # Longitudinal predictor if not self.longitudinal_lpv: front_interval = self.get_front_interval() # LPV specification if front_interval: f_longi_i, f_lat_i = interval_absolute_to_local( front_interval.position, target_lane) f_pos = f_longi_i[0] f_vel = front_interval.velocity[0] else: f_pos, f_vel = 0, 0 x0 = [longi_i[0], f_pos, v_i[0], f_vel] center = [ -self.DISTANCE_WANTED - self.target_velocity * self.TIME_WANTED, 0, self.target_velocity, self.target_velocity ] noise = 1 b = np.array([1, 0, 0, 0])[:, np.newaxis] d_i = np.array([[-1], [1]]) * noise c = [self.target_velocity, self.target_velocity, 0, 0] a0, da = self.longitudinal_matrix_polytope() self.longitudinal_lpv = LPV(x0, a0, da, b, d_i, c, center) # Lateral predictor if not self.lateral_lpv: # LPV specification x0 = [lat_i[0], psi_i[0]] center = [0, 0] noise = 0.5 b = np.identity(2) d_i = np.array([[-1, 0], [1, 0]]) * noise c = [0, 0] a0, da = self.lateral_matrix_polytope() self.lateral_lpv = LPV(x0, a0, da, b, d_i, c, center)
def predictor_step(self, dt: float) -> None: """ Step the interval predictor dynamics :param dt: timestep [s] """ # Create longitudinal and lateral LPVs self.predictor_init() # Detect lane change and update intervals of local coordinates with the new frame if self.target_lane_index != self.previous_target_lane_index: position_i = self.interval.position target_lane = self.road.network.get_lane(self.target_lane_index) previous_target_lane = self.road.network.get_lane( self.previous_target_lane_index) longi_i, lat_i = interval_absolute_to_local( position_i, target_lane) psi_i = self.interval.heading + \ target_lane.heading_at(longi_i.mean()) - previous_target_lane.heading_at(longi_i.mean()) x_i_local_unrotated = np.transpose([lat_i, psi_i]) new_x_i_t = self.lateral_lpv.change_coordinates( x_i_local_unrotated, back=False, interval=True) delta = new_x_i_t.mean(axis=0) - self.lateral_lpv.x_i_t.mean( axis=0) self.lateral_lpv.x_i_t += delta x_i_local_unrotated = self.longitudinal_lpv.change_coordinates( self.longitudinal_lpv.x_i_t, back=True, interval=True) x_i_local_unrotated[:, 0] = longi_i new_x_i_t = self.longitudinal_lpv.change_coordinates( x_i_local_unrotated, back=False, interval=True) self.longitudinal_lpv.x_i_t += new_x_i_t.mean( axis=0) - self.longitudinal_lpv.x_i_t.mean(axis=0) self.previous_target_lane_index = self.target_lane_index # Step self.longitudinal_lpv.step(dt) self.lateral_lpv.step(dt) # Backward coordinates change x_i_long = self.longitudinal_lpv.change_coordinates( self.longitudinal_lpv.x_i_t, back=True, interval=True) x_i_lat = self.lateral_lpv.change_coordinates(self.lateral_lpv.x_i_t, back=True, interval=True) # Conversion from rectified to true coordinates target_lane = self.road.network.get_lane(self.target_lane_index) position_i = interval_local_to_absolute(x_i_long[:, 0], x_i_lat[:, 0], target_lane) self.interval.position = position_i self.interval.speed = x_i_long[:, 2] self.interval.heading = x_i_lat[:, 1]
def observer_step(self, dt: float) -> None: """ Step the interval observer dynamics :param dt: timestep [s] """ # Input state intervals position_i = self.interval.position v_i = self.interval.speed psi_i = self.interval.heading # Features interval front_interval = self.get_front_interval() # Acceleration features phi_a_i = np.zeros((2, 3)) phi_a_i[:, 0] = [0, 0] if front_interval: phi_a_i[:, 1] = interval_negative_part( intervals_diff(front_interval.speed, v_i)) # Lane distance interval lane_psi = self.lane.heading_at(self.lane.local_coordinates(self.position)[0]) lane_direction = [np.cos(lane_psi), np.sin(lane_psi)] diff_i = intervals_diff(front_interval.position, position_i) d_i = vector_interval_section(diff_i, lane_direction) d_safe_i = self.DISTANCE_WANTED + self.TIME_WANTED * v_i phi_a_i[:, 2] = interval_negative_part(intervals_diff(d_i, d_safe_i)) # Steering features phi_b_i = None lanes = self.get_followed_lanes() for lane_index in lanes: lane = self.road.network.get_lane(lane_index) longitudinal_pursuit = lane.local_coordinates(self.position)[0] + self.speed * self.TAU_PURSUIT lane_psi = lane.heading_at(longitudinal_pursuit) _, lateral_i = interval_absolute_to_local(position_i, lane) lateral_i = -np.flip(lateral_i) i_v_i = 1/np.flip(v_i, 0) phi_b_i_lane = np.transpose(np.array([ [0, 0], intervals_product(lateral_i, i_v_i)])) # Union of candidate feature intervals if phi_b_i is None: phi_b_i = phi_b_i_lane else: phi_b_i[0] = np.minimum(phi_b_i[0], phi_b_i_lane[0]) phi_b_i[1] = np.maximum(phi_b_i[1], phi_b_i_lane[1]) # Commands interval a_i = intervals_product(self.theta_a_i, phi_a_i) b_i = intervals_product(self.theta_b_i, phi_b_i) # Speeds interval keep_stability = False if keep_stability: dv_i = integrator_interval(v_i - self.target_speed, self.theta_a_i[:, 0]) else: dv_i = intervals_product(self.theta_a_i[:, 0], self.target_speed - np.flip(v_i, 0)) dv_i += a_i dv_i = np.clip(dv_i, -self.ACC_MAX, self.ACC_MAX) keep_stability = True if keep_stability: delta_psi = list(map(utils.wrap_to_pi, psi_i - lane_psi)) d_psi_i = integrator_interval(delta_psi, self.theta_b_i[:, 0]) else: d_psi_i = intervals_product(self.theta_b_i[:, 0], lane_psi - np.flip(psi_i, 0)) d_psi_i += b_i # Position interval cos_i = [-1 if psi_i[0] <= np.pi <= psi_i[1] else min(map(np.cos, psi_i)), 1 if psi_i[0] <= 0 <= psi_i[1] else max(map(np.cos, psi_i))] sin_i = [-1 if psi_i[0] <= -np.pi/2 <= psi_i[1] else min(map(np.sin, psi_i)), 1 if psi_i[0] <= np.pi/2 <= psi_i[1] else max(map(np.sin, psi_i))] dx_i = intervals_product(v_i, cos_i) dy_i = intervals_product(v_i, sin_i) # Interval dynamics integration self.interval.speed += dv_i * dt self.interval.heading += d_psi_i * dt self.interval.position[:, 0] += dx_i * dt self.interval.position[:, 1] += dy_i * dt # Add noise noise = 0.3 self.interval.position[:, 0] += noise * dt * np.array([-1, 1]) self.interval.position[:, 1] += noise * dt * np.array([-1, 1]) self.interval.heading += noise * dt * np.array([-1, 1])