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)
d_fun = grad(fun)
check_grads(fun, 0.2)
check_grads(d_fun, 0.3)
def test_arctan():
fun = lambda x : 3.0 * np.arctan(x)
check_grads(fun)(0.2)
