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