def tdoa_multilateration_from_multiple_segments_gauss_newton( anchor_locations: list, range_diffs: list, init_soln: list, max_iters: int = 10, eta: float = 1E-3, abs_tol: float = 1E-6): # Gauss-Newton Iterations soln = np.array(init_soln) # Main iterations for j in range(max_iters): # ==== Calculate the LHS and RHS of Normal Equation: J'*J*d = -J'*res, # where J is Jacobian, d is descent direction, res is residual ====== # jTj = np.zeros((2, 2), dtype=np.float) jTres = np.zeros((2, 1), dtype=np.float) for locations, diffs in zip(anchor_locations, range_diffs): if use_ulab: m = locations.shape()[0] else: m = locations.shape[0] denom_1 = np.linalg.norm(soln - locations[0]) for i in range(1, m): denom_2 = np.linalg.norm(soln - locations[i]) res = (diffs[i - 1] - (denom_1 - denom_2)) del_res = np.array([[((soln[0] - locations[i][0]) / denom_2 - (soln[0] - locations[0][0]) / denom_1)], [((soln[1] - locations[i][1]) / denom_2 - (soln[1] - locations[0][1]) / denom_1)]]) if use_ulab: jTj = jTj + np.linalg.dot(del_res, del_res.transpose()) else: jTj = jTj + np.dot(del_res, del_res.transpose()) jTres = jTres + (del_res * res) # ===================== calculation ENDs here ======================================================= # # Take next Step: # Modification according to Levenberg-Marquardt suggestion jTj = jTj + np.diag(np.diag(jTj) * eta) eta = eta / 2.0 # Invert 2x2 matrix denom = jTj[0][0] * jTj[1][1] - jTj[1][0] * jTj[0][1] numer = np.array([[jTj[1][1], -jTj[0][1]], [-jTj[1][0], jTj[0][0]]]) if denom >= 1E-9: inv_jTj = numer / denom if use_ulab: temp = np.linalg.dot(inv_jTj, jTres) else: temp = np.dot(inv_jTj, jTres) del_soln = np.array([temp[0][0], temp[1][0], 0.0]) soln = soln - del_soln if np.linalg.norm(del_soln) <= abs_tol: break else: print("Can't invert the matrix!") break return soln
def tdoa_multilateration_from_single_segement_gauss_newton( locations: np.array, deltas: np.array, initial_soln: np.array, max_iters: int = 10, eta: float = 1E-3, abs_tol: float = 1E-6): """ tdoa_multilateration_gauss_newton solves the problem: argmin_x sum_{i=1}^{N} (d_i - || x - a_0 ||_2 - || x - a_i ||_2)^{2} by Gauss-Newton Method, which finds a local optimum. :param locations: Nx3 array of anchor locations :param deltas: (N-1)x1 array of range-differences :param initial_soln: 3x1 array of initial solution :param max_iters: int :param eta: float :param abs_tol: float :return: 3x1 array of estimated solution """ if use_ulab: m, n = locations.shape() else: m, n = locations.shape soln = np.array(initial_soln) for j in range(max_iters): # ==== Calculate the LHS and RHS of Normal Equation: J'*J*d = -J'*res, # where J is Jacobian, d is descent direction, res is residual ====== # jTj = np.zeros((n - 1, n - 1)) jTres = np.zeros((n - 1, 1)) denom_1 = np.linalg.norm(soln - locations[0]) for i in range(1, m): denom_2 = np.linalg.norm(soln - locations[i]) res = (deltas[i - 1] - (denom_1 - denom_2)) del_res = np.array([[((soln[0] - locations[i, 0]) / denom_2 - (soln[0] - locations[0, 0]) / denom_1)], [((soln[1] - locations[i, 1]) / denom_2 - (soln[1] - locations[0, 1]) / denom_1)]]) if use_ulab: jTj = jTj + np.linalg.dot(del_res, del_res.transpose()) else: jTj = jTj + np.dot(del_res, del_res.transpose()) jTres = jTres + (del_res * res) # ===================== calculation ENDs here ======================================================= # # ==================== Take next Step ========================= # # Modification according to Levenberg-Marquardt suggestion jTj = jTj + np.diag(np.diag(jTj) * eta) eta = eta / 2.0 # Invert 2x2 matrix and update solution denom = jTj[0, 0] * jTj[1, 1] - jTj[1, 0] * jTj[0, 1] numer = np.array([[jTj[1, 1], -jTj[0, 1]], [-jTj[1, 0], jTj[0, 0]]]) if denom >= 1E-9: inv_jTj = numer / denom if use_ulab: temp = np.linalg.dot(inv_jTj, jTres) else: temp = np.dot(inv_jTj, jTres) del_soln = np.array([temp[0, 0], temp[1, 0], 0.0]) soln = soln - del_soln if np.linalg.norm(del_soln) <= abs_tol: break else: print("Can't invert the matrix!") break return soln
def tdoa_multilateration_from_all_segments_gauss_newton( loc_range_diff_info: list, depth: float, max_iters: int = 10, eta: float = 1E-3, abs_tol: float = 1E-6): num_segment = len(loc_range_diff_info) anchor_location_list = [] zone_list = [] anchor_rangediff_list = [] for i in range(num_segment): loc_range_diff = loc_range_diff_info[i] num_anchors = len(loc_range_diff) # create numpy array to hold anchor locations and range-differences anchor_locations = np.zeros((num_anchors, 3), dtype=np.float) range_diffs = np.zeros(num_anchors - 1, dtype=np.float) # extract locations and range-differences from list and store them into respective numpy array for j in range(num_anchors): if j == 0: zone, zone_letter = loc_range_diff[j][0][0], loc_range_diff[j][ 0][1] zone_list.append([zone, zone_letter]) anchor_locations[j] = np.array(loc_range_diff[j][1]) else: anchor_locations[j] = np.array(loc_range_diff[j][0:3]) range_diffs[j - 1] = loc_range_diff[j][3] # Collect anchor locations and range-differences arrays anchor_location_list.append(anchor_locations) anchor_rangediff_list.append(range_diffs) # Check that all zone and zone letters are same bflag_zone = True zone, zone_letter = zone_list[0][0], zone_list[0][1] for zone_info in zone_list: if zone_info != [zone, zone_letter]: bflag_zone = False break if not bflag_zone: raise ValueError("All the anchors are not in same zone!") # =========== Gauss Newton Iterations for Location Estimation ========== # # Initial Solution is taken as Mean location of all the Anchors mean_anchor_location = np.zeros(3, dtype=np.float) num_anchors = 0 for anchor_location in anchor_location_list: if use_ulab: mean_anchor_location = mean_anchor_location + np.sum( anchor_location, axis=0) num_anchors += anchor_location.shape()[0] else: mean_anchor_location = mean_anchor_location + np.sum( anchor_location, axis=0) num_anchors += anchor_location.shape[0] mean_anchor_location = mean_anchor_location / num_anchors soln = np.array(mean_anchor_location) soln[2] = depth # replace 3rd coordinate with sensor depth # Main iterations for j in range(max_iters): # ==== Calculate the LHS and RHS of Normal Equation: J'*J*d = -J'*res, # where J is Jacobian, d is descent direction, res is residual ====== # jTj = np.zeros((2, 2)) jTres = np.zeros((2, 1)) for anchor_locations, range_diffs in zip(anchor_location_list, anchor_rangediff_list): if use_ulab: m = anchor_locations.shape()[0] else: m = anchor_locations.shape[0] denom_1 = np.linalg.norm(soln - anchor_locations[0]) for i in range(1, m): denom_2 = np.linalg.norm(soln - anchor_locations[i]) res = (range_diffs[i - 1] - (denom_1 - denom_2)) del_res = np.array( [[((soln[0] - anchor_locations[i][0]) / denom_2 - (soln[0] - anchor_locations[0][0]) / denom_1)], [((soln[1] - anchor_locations[i][1]) / denom_2 - (soln[1] - anchor_locations[0][1]) / denom_1)]]) if use_ulab: jTj = jTj + np.linalg.dot(del_res, del_res.transpose()) else: jTj = jTj + np.dot(del_res, del_res.transpose()) jTres = jTres + (del_res * res) # ===================== calculation ENDs here ======================================================= # # ============= Take the next step: ====================== # # Modification according to Levenberg-Marquardt suggestion jTj = jTj + np.diag(np.diag(jTj) * eta) eta = eta / 2.0 # Invert 2x2 matrix denom = jTj[0][0] * jTj[1][1] - jTj[1][0] * jTj[0][1] numer = np.array([[jTj[1][1], -jTj[0][1]], [-jTj[1][0], jTj[0][0]]]) if denom >= 1E-9: inv_jTj = numer / denom if use_ulab: temp = np.linalg.dot(inv_jTj, jTres) else: temp = np.dot(inv_jTj, jTres) del_soln = np.array([temp[0][0], temp[1][0], 0.0]) soln = soln - del_soln if np.linalg.norm(del_soln) <= abs_tol: break else: print("Can't invert the matrix!") break # Convert to Lat/Lon try: lat, lon = to_latlon(soln[0], soln[1], zone, zone_letter) estimated_location = [lat, lon, soln[2]] except Exception as e: print("Not a valid estimate: " + str(e)) estimated_location = [] return estimated_location
ab = np.dot(a, b) m,n = ab.shape for i in range(m): for j in range(n): if i == j: print(math.isclose(ab[i][j], 1.0, rel_tol=1E-9, abs_tol=1E-9)) else: print(math.isclose(ab[i][j], 0.0, rel_tol=1E-9, abs_tol=1E-9)) a = np.array([[1, 2, 3, 4], [4, 5, 6, 4], [7, 8.6, 9, 4], [3, 4, 5, 6]]) result = (np.linalg.det(a)) ref_result = 7.199999999999995 print(math.isclose(result, ref_result, rel_tol=1E-9, abs_tol=1E-9)) a = np.array([1, 2, 3]) w, v = np.linalg.eig(np.diag(a)) for i in range(3): print(math.isclose(w[i], a[i], rel_tol=1E-9, abs_tol=1E-9)) for i in range(3): for j in range(3): if i == j: print(math.isclose(v[i][j], 1.0, rel_tol=1E-9, abs_tol=1E-9)) else: print(math.isclose(v[i][j], 0.0, rel_tol=1E-9, abs_tol=1E-9)) a = np.array([[25, 15, -5], [15, 18, 0], [-5, 0, 11]]) result = (np.linalg.cholesky(a)) ref_result = np.array([[5., 0., 0.], [ 3., 3., 0.], [-1., 1., 3.]]) for i in range(3): for j in range(3):