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