def arc_distance(theta_1, phi_1,
theta_2, phi_2):
"""
Calculates the pairwise arc distance between all points in vector a and b.
"""
temp = np.sin((theta_2-theta_1)/2)**2+np.cos(theta_1)*np.cos(theta_2)*np.sin((phi_2-phi_1)/2)**2
distance_matrix = 2 * (np.arctan2(np.sqrt(temp),np.sqrt(1-temp)))
return distance_matrix
def match(tester,dataset):
codex = np.array(dataset)
t = np.array(tester)
for i in range(codex.shape()[0]):
codex[i,:] = codex[i,:] - t
data = np.sum(codex * codex, axis=1)
data = np.sqrt(data)
if codex.shape()[0] == 1:
return data,1
ind = np.argmin(data)
return data[ind][0],ind + 1
c, d = np.fft.ifft(a, b)
# c should be equal to y
cmp_result = []
for p, q in zip(list(y), list(c)):
cmp_result.append(math.isclose(p, q, rel_tol=1e-09, abs_tol=1e-09))
print(cmp_result)
z = np.zeros(len(x))
a, b = np.fft.fft(y, z)
c, d = np.fft.ifft(a, b)
# c should be equal to y
cmp_result = []
for p, q in zip(list(y), list(c)):
cmp_result.append(math.isclose(p, q, rel_tol=1e-09, abs_tol=1e-09))
print(cmp_result)
g = np.fft.spectrogram(y)
h = np.sqrt(a * a + b * b)
cmp_result = []
for p, q in zip(list(g), list(h)):
cmp_result.append(math.isclose(p, q, rel_tol=1e-09, abs_tol=1e-09))
print(cmp_result)
else:
a = np.fft.fft(y)
c = np.fft.ifft(a)
# c should be equal to y
cmp_result = []
for p, q in zip(list(y), list(c.real)):
cmp_result.append(math.isclose(p, q, rel_tol=1e-09, abs_tol=1e-09))
print(cmp_result)
print(math.isclose(result, ref_result, rel_tol=1E-6, abs_tol=1E-6))
result = (np.arctan(np.tan(np.pi / 2)))
print(math.isclose(result, ref_result, rel_tol=1E-6, abs_tol=1E-6))
result = (np.cosh(np.arccosh(np.pi / 2)))
print(math.isclose(result, ref_result, rel_tol=1E-6, abs_tol=1E-6))
result = np.sinh((np.arcsinh(np.pi / 2)))
print(math.isclose(result, ref_result, rel_tol=1E-6, abs_tol=1E-6))
print(np.degrees(np.pi))
print(np.radians(np.degrees(np.pi)))
print(np.floor(np.pi))
print(np.ceil(np.pi))
print(np.sqrt(np.pi))
print(np.exp(1))
print(np.log(np.exp(1)))
if use_ulab:
print(np.log2(2**1))
else:
print(np.log2(2**1))
print(np.log10(10**1))
print(np.exp(1) - np.expm1(1))
x = np.array([-1, +1, +1, -1])
y = np.array([-1, -1, +1, +1])
result = (np.arctan2(y, x) * 180 / np.pi)
ref_result = np.array([-135.0, -45.0, 45.0, 135.0], dtype=np.float)
def perform_tdoa_multilateration_closed_form(anchor_locations, range_diffs,
sensor_depth):
"""
Calculates the unknown location given the anchor locations and TDoA info
:param anchor_locations: an array of dims [n,3]
:param sensor_depth: float
:param range_diffs: an array of dims [n-1,1]
:return: an array of dim [3,1]
"""
soln = np.array([])
if use_ulab:
bflag_proceed = anchor_locations.size() > 0 and range_diffs.size() > 0
else:
bflag_proceed = anchor_locations.size > 0 and range_diffs.size > 0
if bflag_proceed:
if use_ulab:
n = range_diffs.size()
else:
n = range_diffs.size
mat_m = np.zeros((n, 2), dtype=np.float)
for i in range(n):
mat_m[i, :] = np.array(
[(anchor_locations[0, 0] - anchor_locations[i + 1, 0]),
(anchor_locations[0, 1] - anchor_locations[i + 1, 1])]) * 2.0
vec_c = range_diffs * 2.0
vec_d = np.zeros(n, dtype=np.float)
for i in range(n):
vec_d[i] = np.linalg.norm(anchor_locations[i + 1]) ** 2 - np.linalg.norm(anchor_locations[0]) ** 2 \
- range_diffs[i] ** 2 + 2.0 * (anchor_locations[0, 2] - anchor_locations[i + 1, 2]) * sensor_depth
# Invert the matrix and find the solution if feasible
if use_ulab:
mTm = np.linalg.dot(mat_m.transpose(), mat_m)
else:
mTm = np.dot(mat_m.transpose(), mat_m)
# Invert the matrix mTm
denom = mTm[0, 0] * mTm[1, 1] - mTm[1, 0] * mTm[0, 1]
numer = np.array([[mTm[1, 1], -mTm[0, 1]], [-mTm[1, 0], mTm[0, 0]]])
if denom >= 1E-9:
mTm_inv = numer / denom
if use_ulab:
mat_m_inv = np.linalg.dot(mTm_inv, mat_m.transpose())
else:
mat_m_inv = np.matmul(mTm_inv, mat_m.transpose())
# Solve the quadratic equations
if use_ulab:
vec_a = -np.linalg.dot(mat_m_inv, vec_c)
vec_b = -np.linalg.dot(mat_m_inv, vec_d)
alpha = np.linalg.dot(vec_a, vec_a) - 1.0
beta = 2.0 * np.linalg.dot(vec_a,
(vec_b - anchor_locations[0][0:2]))
gamma = np.linalg.dot(
(vec_b - anchor_locations[0][0:2]),
(vec_b - anchor_locations[0][0:2])) + (
sensor_depth - anchor_locations[0][2])**2
else:
vec_a = -np.dot(mat_m_inv, vec_c)
vec_b = -np.dot(mat_m_inv, vec_d)
alpha = np.dot(vec_a, vec_a) - 1.0
beta = 2.0 * np.dot(vec_a, (vec_b - anchor_locations[0][0:2]))
gamma = np.dot((vec_b - anchor_locations[0][0:2]),
(vec_b - anchor_locations[0][0:2])) + (
sensor_depth - anchor_locations[0][2])**2
delta = beta**2 - 4.0 * alpha * gamma
if delta < 0.0:
pass
elif math.isclose(delta, 0.0, abs_tol=1E-5) and beta < 0.0:
root = -beta / (2.0 * alpha)
soln = vec_a * root + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif math.isclose(alpha, 0.0, abs_tol=1E-5) and beta < 0.0:
root = -gamma / beta
soln = vec_a * root + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif delta > 0.0 and not math.isclose(alpha, 0.0, abs_tol=1E-5):
# print("delta > 0 and alpha != 0")
sqrt_delta = np.sqrt(delta)
root1 = (-beta - sqrt_delta) / (2 * alpha)
root2 = (-beta + sqrt_delta) / (2 * alpha)
# print("Root1={:0.4f} and Root2={:0.4f}".format(root1, root2))
if root2 < 0.0 < root1:
soln = vec_a * root1 + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif root1 < 0.0 < root2:
soln = vec_a * root2 + vec_b
soln = np.array([soln[0], soln[1], sensor_depth])
elif root1 > 0.0 and root2 > 0.0:
pass
else: # Both roots are negative
pass
else:
pass
else:
pass
return soln