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])