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):