def perform_robust_multilateration(loc_range_diff_info, depth):
num_segment = len(loc_range_diff_info)
estimated_locations = []
# estimate location from each segment
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
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
zone = None
zone_letter = None
for j in range(num_anchors):
if j == 0:
zone, zone_letter = loc_range_diff[j][0][0], loc_range_diff[j][
0][1]
locations[j] = np.array(loc_range_diff[j][1])
else:
locations[j] = np.array(loc_range_diff[j][0:3])
range_diffs[j - 1] = loc_range_diff[j][3]
# Call Gauss Newton Method for location estimation
initial_soln = np.mean(locations, axis=0)
# replace 3 coordinate with sensor depth
initial_soln[2] = depth
estimated_location = tdoa_multilateration_from_single_segement_gauss_newton(
locations, range_diffs, initial_soln)
# estimated_location = TDOALocalization.perform_tdoa_multilateration_closed_form(locations, range_diffs, depth)
if use_ulab:
bflag_estimated = estimated_location.size() > 0
else:
bflag_estimated = estimated_location.size > 0
# Convert to Lat/Lon
if bflag_estimated > 0:
lat, lon = to_latlon(estimated_location[0], estimated_location[1],
zone, zone_letter)
estimated_locations.append([lat, lon, estimated_location[2]])
return estimated_locations
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
def least_median_square_estimate(loc_range_diff_info: list, depth: float,
max_iters: int) -> list:
num_segments = len(loc_range_diff_info)
lead_anchor_locations_list = []
asst_anchor_locations_list = []
range_diffs_list = []
zone_info_list = []
lut = []
for i in range(num_segments):
loc_range_diff = loc_range_diff_info[i]
num_anchors = len(loc_range_diff)
lut += [i] * (num_anchors - 1)
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_info_list.append([zone, zone_letter])
lead_anchor_locations_list.append(loc_range_diff[j][1])
else:
asst_anchor_locations_list.append(loc_range_diff[j][0:3])
range_diffs_list.append(loc_range_diff[j][3])
# Check if all anchors belong to same zone
bflag_zone = True
zone, zone_letter = zone_info_list[0][0], zone_info_list[0][1]
for zone_info in zone_info_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!")
num_asst_anchors = len(lut)
# Generate subsets of assistant anchors with each subset containing 3 elements
subsets = list(random_combinations(range(num_asst_anchors), 3, max_iters))
# For each subset, calculate the unknown location using multilateration
# and then calculate median of square residues = (range_diff - (SA - SB))**2 ,
# where A and B represent the lead and the assistant anchor, respectively, and
# S represent estimated unknown location
min_median = np.inf
robust_location = None
iter_count = 0
for subset in subsets:
# Pack together the assistant and the corresponding lead anchor's location
anchor_locations = []
range_diffs = []
for index in subset:
lead_anchor = lead_anchor_locations_list[lut[index]]
asst_anchor = asst_anchor_locations_list[index]
anchors = np.array([lead_anchor, asst_anchor])
range_diff = np.array([range_diffs_list[index]])
anchor_locations.append(anchors)
range_diffs.append(range_diff)
# Calculate initial soln as mean of Anchor Positions
mean_anchor_location = np.zeros(3, dtype=np.float)
num_anchors = 0
for anchor in anchor_locations:
mean_anchor_location = mean_anchor_location + np.sum(anchor,
axis=0)
if use_ulab:
num_anchors += anchor.shape()[0]
else:
num_anchors += anchor.shape[0]
init_soln = mean_anchor_location / num_anchors
init_soln[2] = depth
# Estimate location from selected subset of anchors using Gauss-Newton method
sensor_location = tdoa_multilateration_from_multiple_segments_gauss_newton(
anchor_locations, range_diffs, init_soln)
# For each estimated location calculate the square of residues, i.e., (observed_range_diff - calculated_range_diff)**2
sq_residues = []
for index in subset:
lead_anchor = np.array(lead_anchor_locations_list[lut[index]])
asst_anchor = np.array(asst_anchor_locations_list[index])
range_diff = np.array([range_diffs_list[index]])
lead_to_sensor = np.linalg.norm((lead_anchor - sensor_location))
asst_to_sensor = np.linalg.norm((asst_anchor - sensor_location))
estimated_range_diff = lead_to_sensor - asst_to_sensor
sq_residues.append((range_diff - estimated_range_diff)**2)
# Calculate Median of Square-Residues
current_median = np.median(np.array(sq_residues))
# Select the subset which results into lowest median of errors
if current_median <= min_median:
min_median = current_median
robust_location = sensor_location
if iter_count >= max_iters:
break
iter_count += 1
# Convert the location into Lat/Lon system
try:
lat, lon = to_latlon(robust_location[0], robust_location[1], zone,
zone_letter)
estimated_location = [lat, lon, robust_location[2]]
except Exception as e:
print("Not a valid estimate: " + str(e))
estimated_location = []
return estimated_location
timestamp = t0 + timedelta(seconds=master_to_sensor)
data_packet.source_address = 0
data_packet.destination_address = 255
data_packet.packet_type = MessagePacket.PACKETTYPE_BROADCAST
data_packet.packet_payload = msg
data_packet.timestamp = (timestamp.year, timestamp.month,
timestamp.day, timestamp.hour,
timestamp.minute, timestamp.second,
timestamp.microsecond)
# data_packet.timestamp = (seconds, millis, micros)
sensor.handle_incoming_data_packet(data_packet)
# Generate remaining Beacon Signals
for j in range(0, len(Beacon_Segments[i])):
lat, lon = to_latlon(Beacon_Segments[i][j][0],
Beacon_Segments[i][j][1],
Beacon_Segments[i][j][3],
Beacon_Segments[i][j][4])
anchor_depth = Beacon_Segments[i][j][2]
lat = struct.pack('f', lat)
lon = struct.pack('f', lon)
anchor_depth = struct.pack('f', anchor_depth)
if j == 0: # Start of Beacon Segment
time_delay = random.random() # in seconds
t0 = t0 + timedelta(seconds=time_delay) # add some random delay
master_to_sensor = distance(
Beacon_Segments[i][0][0:3],
sensor_loc[0:3]) / sound_speed # in seconds
timestamp = t0 + timedelta(seconds=master_to_sensor)
m = len(Beacon_Segments[i])
n = i
msg = b'ULMB' + struct.pack('B', n) + struct.pack(