def test_function_overloading(): a = pe.pseudo_Obs(17, 2.9, 'e1') b = pe.pseudo_Obs(4, 0.8, 'e1') fs = [ lambda x: x[0] + x[1], lambda x: x[1] + x[0], lambda x: x[0] - x[1], lambda x: x[1] - x[0], lambda x: x[0] * x[1], lambda x: x[1] * x[0], lambda x: x[0] / x[1], lambda x: x[1] / x[0], lambda x: np.exp(x[0]), lambda x: np.sin(x[0]), lambda x: np.cos(x[0]), lambda x: np.tan(x[0]), lambda x: np.log(x[0]), lambda x: np.sqrt(np.abs(x[0])), lambda x: np.sinh(x[0]), lambda x: np.cosh(x[0]), lambda x: np.tanh(x[0]) ] for i, f in enumerate(fs): t1 = f([a, b]) t2 = pe.derived_observable(f, [a, b]) c = t2 - t1 assert c.is_zero() assert np.log(np.exp(b)) == b assert np.exp(np.log(b)) == b assert np.sqrt(b**2) == b assert np.sqrt(b)**2 == b np.arcsin(1 / b) np.arccos(1 / b) np.arctan(1 / b) np.arctanh(1 / b) np.sinc(1 / b)
def burst_soln(t, n): if n % 2 == 0: x = (np.sqrt(1 + t**2) / n) * ( (-1)**(n / 2)) * np.sin(n * np.arctan(t)) elif (n + 1) % 2 == 0: x = (np.sqrt(1 + t**2) / n) * ((-1)**( (n - 1) / 2)) * np.cos(n * np.arctan(t)) return x
def dburst_soln(t, n): if n % 2 == 0: x = (1 / (np.sqrt(1 + t**2) * n)) * ((-1)**(n / 2)) * ( t * np.sin(n * np.arctan(t)) + n * np.cos(n * np.arctan(t))) elif (n + 1) % 2 == 0: x = (1 / (np.sqrt(1 + t**2) * n)) * ((-1)**((n - 1) / 2)) * ( t * np.cos(n * np.arctan(t)) - n * np.sin(n * np.arctan(t))) return x
def EKF_predict(self): move_x = self.move_x move_z = self.move_z delta_yaw = self.delta_yaw EKF_lane_last = self.EKF_lane_last EKF_lane_last_col1 = EKF_lane_last[0:2] EKF_lane_last_col2 = EKF_lane_last[2:4] # predict in cartesian coord move_matrix = np.array([move_x, move_z]) rotation_martix = build_rotation_matrix_2D(delta_yaw) EKF_lane_now_esti_col1 = coords_ro_move_2D(EKF_lane_last_col1, move_matrix, rotation_martix, mode='move first') EKF_lane_now_esti_col2 = coords_ro_move_2D(EKF_lane_last_col2, move_matrix, rotation_martix, mode='move first') EKF_lane_now_esti = np.hstack((EKF_lane_now_esti_col1, EKF_lane_now_esti_col2)) # change to rho theta space theta = np.arctan((EKF_lane_now_esti[0] - EKF_lane_now_esti[2]) / ( EKF_lane_now_esti[3] - EKF_lane_now_esti[1])) rho = EKF_lane_now_esti[0] * np.cos(theta) + EKF_lane_now_esti[1] * np.sin(theta) # offset to lane self.x = np.array([theta, rho]) Qj = grad(self.control_inputs_to_rho) # predict noise u = np.array([self.v, self.acc, self.yaw]) rho_noise = np.sum(np.abs(Qj(u) * self.control_noise)) theta_nois = self.control_noise[2] self.P += np.array([[theta_nois, 0], [0, rho_noise]])
def coal_var(template, data, noise): g_tilde = G_tilde(template, data, noise) g = np.fft.ifft(g_tilde) tc_index = np.argmax(np.multiply(g, np.conj(g))) tc = 0 phic = np.arctan(g[tc_index].imag / g[tc_index].real) return phic, tc
def rho_theta(line): if line[3] - line[1] != 0: theta = np.arctan((line[0] - line[2]) / (line[3] - line[1])) else: theta = np.pi / 2 rho = line[0] * np.cos(theta) + line[1] * np.sin(theta) return rho, theta
def calc_boundary_phases(k, params): l = params['l'] L_0 = params['L_0'] C_0 = params['C_0'] C_i = params['C_i'] C_o = params['C_o'] Z_0 = np.sqrt(L_0 / C_0) velocity = 1 / np.sqrt(L_0 * C_0) omega = velocity * k if not np.isclose(C_i / (2 * l * C_0), 0.0): phi_i = np.arctan(1 / np.abs(Z_0 * omega * C_i)) else: phi_i = np.pi / 2 if not np.isclose(C_o / (2 * l * C_0), 0.0): phi_o = np.arctan(1 / np.abs(Z_0 * omega * C_o)) else: phi_o = np.pi / 2 return phi_i, phi_o
def to_lane_dist(lanes_col1, lanes_col2): # use middle point to calcu dist_y? sample_num = lanes_col1.shape[0] dist_y = np.min(abs(np.hstack((lanes_col1[:, 1].reshape(sample_num, 1), lanes_col2[:, 1].reshape(sample_num, 1)))), axis=1) # nearest y in each lane theta = np.arctan((lanes_col1[:, 0] - lanes_col2[:, 0]) / (lanes_col2[:, 1] - lanes_col1[:, 1])) rho = abs(lanes_col1[:, 0] * np.cos(theta) + lanes_col1[:, 1] * np.sin(theta)) # offset to lane dist = dist_y + rho return dist
def _evaluate(self, x, out, *args, **kwargs): f1 = x[:, 0] f2 = x[:, 1] g1 = -(anp.square(x[:, 0]) + anp.square(x[:, 1]) - 1.0 - 0.1 * anp.cos(16.0 * anp.arctan(x[:, 0] / x[:, 1]))) g2 = 2 * (anp.square(x[:, 0] - 0.5) + anp.square(x[:, 1] - 0.5)) - 1 out["F"] = anp.column_stack([f1, f2]) out["G"] = anp.column_stack([g1, g2])
def cart2sph(self, n): temp = n[1]/n[0] if n[0]==0: if n[1]<0: phi = -np.pi/2 else: phi = np.pi/2 else: if n[0]>0: phi = np.arctan(temp) elif n[1]<0: phi = np.arctan(temp) - np.pi else: phi = np.arctan(temp) + np.pi # phi = np.arctan() #arctan(y/x) theta = np.arccos(n[2]) #arccos(z) return [phi, theta]
def draw_ellipse(mu, cov, conf=.95): chiscale = chi2.isf(1-conf,2) v, w = np.linalg.eigh(cov) v = 2. * np.sqrt(chiscale) * np.sqrt(v) u = w[0] / np.linalg.norm(w[0]) # Plot an ellipse to show the Gaussian component angle = np.arctan(u[1] / u[0]) angle = 180. * angle / np.pi # convert to degrees ell = mpl.patches.Ellipse(mu, v[0], v[1], 180. + angle) return ell
def visit_Function(self, node): f = node.value if f == EXP: return np.exp(self.visit(node.expr)) if (f == LOG) or (f == LN): return np.log(self.visit(node.expr)) if f == LOG10: return np.log10(self.visit(node.expr)) if f == SQRT: return np.sqrt(self.visit(node.expr)) if f == ABS: return np.abs(self.visit(node.expr)) if f == SIGN: return np.sign(self.visit(node.expr)) if f == SIN: return np.sin(self.visit(node.expr)) if f == COS: return np.cos(self.visit(node.expr)) if f == TAN: return np.tan(self.visit(node.expr)) if f == ASIN: return np.arcsin(self.visit(node.expr)) if f == ACOS: return np.arccos(self.visit(node.expr)) if f == ATAN: return np.arctan(self.visit(node.expr)) if f == MAX: raise NotImplementedError(MAX) if f == MIN: raise NotImplementedError(MIN) if f == NORMCDF: raise NotImplementedError(NORMCDF) if f == NORMPDF: raise NotImplementedError(NORMPDF) if f == ERF: return erf(self.visit(node.expr))
def EKF_update(self, tracked_lane): theta = np.arctan((tracked_lane[0] - tracked_lane[2]) / (tracked_lane[3] - tracked_lane[1])) rho = tracked_lane[0] * np.cos(theta) + tracked_lane[1] * np.sin(theta) # offset to lane z = np.array([theta, rho]) P = self.P # update y = z - self.x S = P + self.measure_noise K = np.dot(P, np.linalg.inv(S)) self.x += np.dot(K, y) self.P = np.dot((self.I - K), P)
def plot_results(ax, X, Y, means, covariances, index, title): for i, (mean, covar, color) in enumerate(zip( means, covariances, color_iter)): v, w = linalg.eigh(np.diag(np.full([2], covar))) v = 2. * np.sqrt(2.) * np.sqrt(v) u = w[0] / linalg.norm(w[0]) if not np.any(Y == i): continue ax.scatter(X[Y == i, 0], X[Y == i, 1], 2., color=color) angle = np.arctan(u[1] / u[0]) angle = 180. * angle / np.pi # convert to degrees ell = mpl.patches.Ellipse(mean, v[0], v[1], 180. + angle, color=color) #ell.set_clip_box(splot.bbox) ell.set_alpha(0.5) ax.add_artist(ell)
def draw_ellipse(mu, cov, num_sd=1): # as I understand it - # diagonalise the cov matrix # use eigenvalues for radii # use eigenvector rotation from x axis for rotation x = mu[0] #x-position of the center y = mu[1] #y-position of the center cov = cov * num_sd # show within num_sd standard deviations lam, V = np.linalg.eig(cov) # eigenvalues and vectors t_rot = np.arctan(V[1, 0] / V[0, 0]) a, b = lam[0], lam[1] t = np.linspace(0, 2 * np.pi, 100) Ell = np.array([a * np.cos(t), b * np.sin(t)]) #u,v removed to keep the same center location R_rot = np.array([[cos(t_rot), -sin(t_rot)], [sin(t_rot), cos(t_rot)]]) #2-D rotation matrix Ell_rot = np.zeros((2, Ell.shape[1])) for i in range(Ell.shape[1]): Ell_rot[:, i] = np.dot(R_rot, Ell[:, i]) return x + Ell_rot[0, :], y + Ell_rot[1, :]
def compute_control_inputs(fc, xk, yk, thetak): n = xk.shape[0] - 1 # import ipdb; ipdb.set_trace() dx = xk[1:] - xk[0:-1] dy = yk[1:] - yk[0:-1] dk = np.array([dx, dy, np.zeros(n)]).T dtheta = (thetak[1:] - thetak[0:-1]) / (1.0 / fc) # vel qk = np.array([np.cos(thetak[:-1]), np.sin(thetak[:-1]), np.zeros(n)]).T proj_q_d = np.sum(qk * dk, axis=1) sign_v = sign(proj_q_d) vk = np.linalg.norm(dk, axis=1) * sign_v / (1.0 / fc) # steering angle phi_k = np.zeros(vk.shape) mask = np.absolute(vk) > 1e-5 phi_k[mask] = np.arctan(wheelbase * dtheta[mask] / vk[mask]) u = np.array((vk, phi_k)) return u
def control_inputs_to_rho(self, u): v, acc, yaw = u[0], u[1], u[2] x1, z1, x2, z2 = self.EKF_lane_last dt_in_game = self.dt_in_game if yaw != 0: # simple ver. only using the yaw to estimate, in world coord: x,y,z refer to left, up, front horizontal_turn_radius = (v * dt_in_game + acc * dt_in_game ** 2 / 2) / yaw move_z = horizontal_turn_radius * np.sin(yaw) move_x = horizontal_turn_radius * (1 - np.cos(yaw)) else: move_z = (v * dt_in_game + acc * dt_in_game ** 2 / 2) move_x = 0 rotation_martix = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) move_matrix = np.array([move_x, move_z]) points_col = np.array([[x1, z1], [x2, z2]]) points_col_new = np.dot(points_col - move_matrix, np.linalg.inv(rotation_martix)) theta = np.arctan((points_col_new[0, 0] - points_col_new[1, 0]) / (points_col_new[1, 1] - points_col_new[0, 1])) rho = points_col_new[0, 0] * np.cos(theta) + points_col_new[0, 1] * np.sin(theta) # offset to lane return rho
def run(self): if self.initialized == 0: if self.lanes_now is not None: # the very first lane data received self.initialized = 1 # state: first data ever, no older data available else: # still waiting for data return self.image, None elif self.initialized == 1: # the second time it runs if self.lanes_last is not None: self.initialized = 2 # state: at least for 2 times function runs, older data available selected_lane_now = self.generate_order() EKF_lane = None # shoe info on image if selected_lane_now is not None: # calculate info: theta = np.arctan((selected_lane_now[0] - selected_lane_now[2]) / (selected_lane_now[3] - selected_lane_now[1])) rho = selected_lane_now[0] * np.cos(theta) + selected_lane_now[1] * np.sin(theta) # offset to lane # print: print_lane = self.change_coord2 - (selected_lane_now * 15).astype(int) new_img = cv2.line(self.image.copy(), (print_lane[0], print_lane[1]), (print_lane[2], print_lane[3]), (255, 0, 0), 10) font = cv2.FONT_HERSHEY_SIMPLEX text = 'angle: {} offset: {}'.format(round(theta,2), round(rho,2)) cv2.putText(new_img, text, (10, 50), font, 1, (255, 150,150), 1, cv2.LINE_AA) # draw EKF_lane theta, rho = self.x EKF_lane = self.build_line_from_theta_rho(theta, rho) print_lane = self.change_coord2 - (EKF_lane * 15).astype(int) new_img = cv2.line(new_img, (print_lane[0], print_lane[1]), (print_lane[2], print_lane[3]), (0, 255, 0), 10) text = 'angle: {} offset: {}'.format(round(theta, 2), round(rho, 2)) cv2.putText(new_img, text, (10, 100), font, 1, (150, 255, 150), 1, cv2.LINE_AA) else: new_img = self.image ## update last values: self.selected_lane_last = selected_lane_now self.EKF_lane_last = EKF_lane if self.lanes_now is not None: # update using real new value self.lanes_last = self.lanes_now else: # update using inertial value move_x = self.move_x move_z = self.move_z delta_yaw = self.delta_yaw if self.lanes_last is not None: lanes_last = self.lanes_last lanes_last_col1 = lanes_last[:, 0:2] lanes_last_col2 = lanes_last[:, 2:4] move_matrix = np.array([move_x, move_z]) rotation_matrix = build_rotation_matrix_2D(delta_yaw) lanes_now_esti_col1 = coords_ro_move_2D(lanes_last_col1, move_matrix, rotation_matrix, mode='move first') lanes_now_esti_col2 = coords_ro_move_2D(lanes_last_col2, move_matrix, rotation_matrix, mode='move first') self.lanes_last = np.hstack((lanes_now_esti_col1, lanes_now_esti_col2)) #if self.selected_lane_last is not None: # selected_lane_last = self.selected_lane_last # selected_lane_last_col1 = selected_lane_last[0:2] # selected_lane_last_col2 = selected_lane_last[2:4] # selected_lanes_now_esti_col1 = move_roatation_2d(selected_lane_last_col1, move_x, move_z, delta_yaw, mode='move first') # selected_lanes_now_esti_col2 = move_roatation_2d(selected_lane_last_col2, move_x, move_z, delta_yaw, mode='move first') # self.selected_lane_last = np.hstack((selected_lanes_now_esti_col1, selected_lanes_now_esti_col2)) return new_img, EKF_lane
def EKF_init(self, lanes_now): theta = np.arctan((lanes_now[0] - lanes_now[2]) / (lanes_now[3] - lanes_now[1])) rho = lanes_now[0] * np.cos(theta) + lanes_now[1] * np.sin(theta) self.x = np.array([theta, rho]) self.P = self.measure_noise
def convert_R_0_1(x): return ag.arctan(x) / np.pi + 0.5
def test_arctan(): fun = lambda x: 3.0 * np.arctan(x) check_grads(fun)(0.2)
def test_arctan(): fun = lambda x : 3.0 * np.arctan(x) d_fun = grad(fun) check_grads(fun, 0.2) check_grads(d_fun, 0.3)
def arctan(a: Numeric): return anp.arctan(a)
def populate_coordinates(self): # Populates a variable called self.coordinates with the coordinates of the airfoil. name = self.name.lower().strip() # If it's a NACA 4-series airfoil, try to generate it if "naca" in name: nacanumber = name.split("naca")[1] if nacanumber.isdigit(): if len(nacanumber) == 4: # Parse max_camber = int(nacanumber[0]) * 0.01 camber_loc = int(nacanumber[1]) * 0.1 thickness = int(nacanumber[2:]) * 0.01 # Set number of points per side n_points_per_side = 100 # Referencing https://en.wikipedia.org/wiki/NACA_airfoil#Equation_for_a_cambered_4-digit_NACA_airfoil # from here on out # Make uncambered coordinates x_t = cosspace(n_points=n_points_per_side) # Generate some cosine-spaced points y_t = 5 * thickness * ( + 0.2969 * np.power(x_t, 0.5) - 0.1260 * x_t - 0.3516 * np.power(x_t, 2) + 0.2843 * np.power(x_t, 3) - 0.1015 * np.power(x_t, 4) #0.1015 is original, #0.1036 for sharp TE ) if camber_loc == 0: camber_loc = 0.5 # prevents divide by zero errors for things like naca0012's. # Get camber y_c_piece1 = max_camber / camber_loc ** 2 * ( 2 * camber_loc * x_t[x_t <= camber_loc] - x_t[x_t <= camber_loc] ** 2 ) y_c_piece2 = max_camber / (1 - camber_loc) ** 2 * ( (1 - 2 * camber_loc) + 2 * camber_loc * x_t[x_t > camber_loc] - x_t[x_t > camber_loc] ** 2 ) y_c = np.hstack((y_c_piece1, y_c_piece2)) # Get camber slope dycdx_piece1 = 2 * max_camber / camber_loc ** 2 * ( camber_loc - x_t[x_t <= camber_loc] ) dycdx_piece2 = 2 * max_camber / (1 - camber_loc) ** 2 * ( camber_loc - x_t[x_t > camber_loc] ) dycdx = np.hstack((dycdx_piece1, dycdx_piece2)) theta = np.arctan(dycdx) # Combine everything x_U = x_t - y_t * np.sin(theta) x_L = x_t + y_t * np.sin(theta) y_U = y_c + y_t * np.cos(theta) y_L = y_c - y_t * np.cos(theta) # Flip upper surface so it's back to front x_U, y_U = np.flip(x_U), np.flip(y_U) # Trim 1 point from lower surface so there's no overlap x_L, y_L = x_L[1:], y_L[1:] x = np.hstack((x_U, x_L)) y = np.hstack((y_U, y_L)) coordinates = np.column_stack((x, y)) self.coordinates = coordinates return else: print("Unfortunately, only 4-series NACA airfoils can be generated at this time.") # Try to read from airfoil database try: import importlib.resources from . import airfoils raw_text = importlib.resources.read_text(airfoils, name + '.dat') trimmed_text = raw_text[raw_text.find('\n'):] coordinates1D = np.fromstring(trimmed_text, sep='\n') # returns the coordinates in a 1D array assert len( coordinates1D) % 2 == 0, 'File was found in airfoil database, but it could not be read correctly!' # Should be even coordinates = np.reshape(coordinates1D, (-1, 2)) self.coordinates = coordinates return except FileNotFoundError: print("File was not found in airfoil database!")
def phi_z_0_0(Cphi, Dphi, nc, nd, psi_dot, psi): term1 = (Cphi / psi_dot * (np.sqrt(nc) / (nc - 1)) * np.arctan( (1 - np.sqrt(nc)) * np.tan(psi) / (1 + np.sqrt(nc) * np.tan(psi)**2))) term2 = (Dphi / psi_dot * (np.sqrt(nd) / (nd - 1)) * np.arctan( (1 - np.sqrt(nd)) * np.tan(psi) / (1 + np.sqrt(nd) * np.tan(psi)**2))) return term1 + term2
def test_arctan(): fun = lambda x : 3.0 * np.arctan(x) check_grads(fun)(0.2)