def eigen(matx: np.matrix, eps=1e-3):
"""
функція знаходження власного числа і вектора методом скалярних добутків
"""
tr_matrix = matx.transpose()
y = np.zeros(matx.shape[0])
z = np.zeros(matx.shape[0])
y[0] = START_Y
z[0] = y[0]
eigenvalue = 0
for j in range(ITERATION_LIMIT):
if y.shape[0] == 1:
y = y.transpose()
if z.shape[0] == 1:
z = z.transpose()
next_y = np.array(matx.dot(y))
next_z = np.array(tr_matrix.dot(z))
tmp1 = sum1(next_y * next_z)
tmp2 = sum1(y * next_z)
tmp_res = tmp1 / tmp2
if j == 0:
eigenvalue = tmp_res
elif abs(eigenvalue - tmp_res) < eps:
break
else:
eigenvalue = tmp_res
y = next_y
z = next_z
eigenvector = y / norm(y)
return eigenvalue, eigenvector
def nipals1(x: np.matrix):
max_it = 3
# max_it = matrix_rank(x)
# print("rank(x) = " + str(max_it))
p = np.zeros(shape=(x.shape[1], max_it))
t = np.zeros(shape=(x.shape[0], max_it))
for j in range(0, max_it):
# a = norm(x, axis=0)
# idx = a.argmax()
t_j = x[:, j]
# # verify if the column is a zero vector
# zero_col = np.zeros(shape=t_j.shape)
# if norm(t_j - zero_col) <= epsilon:
# j += 1
# max_it += 1
# continue
t_j1 = t_j - t_j
while norm(t_j1 - t_j) > epsilon:
p_j = x.transpose().dot(t_j)
p_j /= norm(p_j)
t_j1 = t_j
t_j = x.dot(p_j)
p[:, j] = p_j
t[:, j] = t_j
j += 1
x = x - np.dot(t_j[:, None], p_j[None, :])
return t, p
def ComputeSVD(A: np.matrix):
"""Given a matrix A use the eigen value decomposition to compute a SVD
decomposition. It returns a tuple (U,Sigma,V) of np.matrix objects."""
k = np.linalg.matrix_rank(A)
B = A.transpose() @ A
w, V = eig(B)
#Eigenwerte und Vektoren neu sortieren
idx = np.argsort(w)
w = w[idx]
V = V[:, idx]
#S berechnen
S = np.zeros(A.shape)
for i in range(S.shape[0]):
for j in range(S.shape[1]):
if i == j:
S[i][j] = sqrt(w[i])
#U berechnen
U = np.zeros((k + 1, k + 1))
for i in range(k):
U[:, i] = (1 / S[i][i] * A * V[:, i]).flat
U = np.linalg.qr(U)[0]
return np.asmatrix(U), np.asmatrix(S), np.asmatrix(V)
def loss_function_and_gradients(X: np.matrix, theta: np.matrix, y: np.matrix):
m = y.shape[0]
hx = np.exp(-X @ theta)
J = np.sum(np.log(1 / (1 + hx)) * (-y) - (1 - y) * np.log(hx /
(1 + hx))) / m
gradient = (X.transpose() @ (1 / (1 + hx) - y)) / m
return J, gradient
def svd(a: np.matrix):
if a.shape[0] == a.shape[1]:
s, u = linalg.eig(a)
s.sort()
s = np.delete(np.diagflat(np.append(s[::-1], [0])), (len(s), ), axis=0)
v = u.transpose()
elif a.shape[0] < a.shape[1]:
m = a * a.transpose()
s, u = linalg.eig(m)
s, u = sorting(s, u)
s = np.sqrt(s)
s = np.delete(np.diagflat(np.append(s, [0])), (len(s), ), axis=0)
v = np.linalg.pinv(s) * np.matrix(u).transpose() * a
else:
m = a.transpose() * a
s, v = linalg.eig()
s.sort()
s = np.delete(np.diagflat(np.append(s[::-1], [0])), (len(s), ), axis=0)
u = a * np.matrix(v).transpose() * np.linalg.pinv(s)
return u, s, v
def __plot_principal_components__(features: np.ndarray, s: np.matrix = None):
plt.scatter(features[:, :1], features[:, 1:])
if s is not None:
s = s.transpose()
s_list = s.tolist()
x = np.linspace(-2, 2, 100)
m1 = s_list[0][1] / s_list[0][0]
y1 = m1 * x
m2 = s_list[1][1] / s_list[1][0]
y2 = m2 * x
plt.plot(x, y1, c='blue')
plt.plot(x, y2, c='red')
plt.show()
def variance(weights: np.matrix, sigma: np.array):
# returns the variance (NOT standard deviation) given weights and sigma
return (weights * sigma * weights.transpose())[0, 0]
def transformMatrix(m: matrix, byUser: bool):
if byUser:
return m
else:
return m.transpose()
def optimized_scenario(initial_vehicles: List[VehicleInfo],
epsilon: float,
it_max: int,
my: int,
a_ref: ndarray,
scenario: Scenario,
planning_problem: PlanningProblem,
W: np.matrix = None):
# Require
# x_0 initial state vector
# epsilon threshold
# it_max iteration limit
# my binary search iteration limit
# W weighted matrix
# a_ref reference area profile
# Ensure
# critical scenario S
# kappa_new <- 0
# kappa_old <- -inf
# it <- 0
# x_{0,curr} <- x_o
# while abs(kappa_new - kappa_old) >= epsilon and it < it_max do
# success <- true
# while abs(kappa_new - kappa_old) >= epsilon and success = true do
# kappa_old <- kappa_new
# x_{0,old} <- x_{0,curr}
# x_{0,curr}, S, success <- quadProg(solve(quadratic optimization problem))
# kappa_new <- kappa(S, a_ref, W)
# end while
# x_{0,s)^v <- binarySearch(x_{0,old}, x_{0,curr}, my)
# S <- updateScenario(S, x_{0,s}^v)
# kappa_new <- kappa(S, a_ref, W)
# it <- it + 1
# end while
p: int = len(initial_vehicles)
r: int = len(MyState.variables)
total_steps: int = len(a_ref)
if not W:
W = eye(p)
kappa_new: float = 0
kappa_old: float = -np.inf
it: int = 0
current_initial_vehicles = initial_vehicles
while abs(kappa_new - kappa_old) >= epsilon and it < it_max:
success: bool = True
while abs(kappa_new - kappa_old) >= epsilon and success:
kappa_old = kappa_new
old_initial_vehicles: List[VehicleInfo] = current_initial_vehicles
current_generated_vehicles, _ = GenerationHelp.generate_states(
scenario, current_initial_vehicles[0].state,
total_steps) # FIXME Only ego vehicle considered
# x_{0,curr}, S, success <- quadProg(solve(quadratic optimization problem))
B: matrix = calculate_B_as_matrix(old_initial_vehicles,
current_initial_vehicles,
scenario, total_steps)
W_tilde: matrix = np.asmatrix(B.transpose() * W * B)
delta_a0: matrix = np.asmatrix(
calculate_area_profile(
flatten_dict_values(current_generated_vehicles)) - a_ref)
c: ndarray = delta_a0.transpose() * (W * B + W.transpose() * B)
delta_x: Variable = Variable((1, r))
objective: Minimize = Minimize(
cvxpy.norm(quad_form(delta_x, W_tilde) + c * delta_x))
# Instead of the "real" goal region constraint use a minimum velocity needed for getting to the goal region
# within the given amount of steps
# FIXME Only ego vehicle considered
current_initial_position: Point = Point(
current_initial_vehicles[0].state.state.position)
# FIXME Only first goal region entry recognized
goal_region: Polygon = planning_problem.goal.state_list[
0].position.shapely_object
distance_to_goal_region: float = current_initial_position.distance(
goal_region)
min_velocity: float = distance_to_goal_region / (scenario.dt *
total_steps)
print("min_velocity: " + str(min_velocity))
# FIXME Really use delta_a0?
# FIXME If min_velocity is greater than zero the optimization fails
constraints = [
delta_x >= 0, delta_a0 + B * delta_x >= 0,
delta_x[0] >= min_velocity
]
problem = Problem(objective, constraints)
assert is_dccp(problem)
print("Problem solve result: " +
str(problem.solve(method='dccp', solver='ECOS')))
print("Current optimization result: " + str(delta_x.value))
# FIXME Change current initial vehicles like this?
for i in range(len(current_initial_vehicles)):
for j in range(len(MyState.variables)):
# FIXME Only ego vehicle considered
current_initial_vehicles[i].state.set_variable(
j, delta_x.value[i][j])
kappa_new = kappa(delta_a0, a_ref, W) # FIXME Really use delta_a0?
print("Difference between new and old costs: " +
str(abs(kappa_new - kappa_old)))
binary_search(my, old_initial_vehicles, current_initial_vehicles,
scenario,
total_steps) # FIXME What to do with this value?
# initial_vehicles = ? # FIXME What to do here?
update_scenario_vehicles(scenario, planning_problem, initial_vehicles)
kappa_new = kappa(calculate_area_profile(initial_vehicles), a_ref, W)
it += 1