angle0 = np.zeros((track.duration, n_couples))
angle1 = np.zeros((track.duration, n_couples))
sdmax = np.zeros((track.duration, n_couples))
sdmin = np.zeros((track.duration, n_couples))
remove_sat = fn.crossings_sat(aps_sat, couples, track.duration)
remove_same_slope = fn.crossings_same_slopes(slopes, couples, track.duration)
remove_not_valid = remove_sat * remove_same_slope
i = 0
for crossing in couples:
# Calculating cross-section
x_cross[:, i], y_cross[:, i] = fn.crossings(slopes, y_intercept, crossing)
# Calculate distance between APs and cross point
dist0[:, i], dist1[:, i] = fn.crossings_dist(track.aps, crossing, x_cross[:, i], y_cross[:, i])
# Find angles from both APs
angle0[:, i] = local_angle_sim[:, crossing[0]]
angle1[:, i] = local_angle_sim[:, crossing[1]]
# Calculate total SD
sdmax[:, i], sdmin[:, i] = fn.add_sd(angle0[:, i], angle1[:, i], dist0[:, i], dist1[:, i])
i += 1
# Calculate position_error(crossing) in order to find optimal weights
weights = fn.find_weights(sdmax)
# Converting predicted angles into slopes
# [0: x, 1: y, 2: direction, 3: distance, 4: global_angle, 5: predicted_local_angle, 6: predicted_global_angle]
# [7: predicted_slope]
for i in range(n_ap):
aps[i].append(1/math.tan(math.radians(aps[i][6])))
# Finding y intercept
# [0: x, 1: y, 2: direction, 3: distance, 4: global_angle, 5: predicted_local_angle, 6: predicted_global_angle]
# [7: predicted_slope, 8: y_intercept]
for i in range(n_ap):
aps[i].append(aps[i][1] - aps[i][7] * aps[i][0])
# Calculating cross-section
# [0: ap_1, 1: ap_2, 2: cross_x, 3: cross_y]
ap_cross = fn.crossings(ap_cross, aps)
# Calculate distance between APs and cross point
# [0: ap_1, 1: ap_2, 2: cross_x, 3: cross_y, 4: dist_1, 5: dist_2]
ap_cross = fn.crossings_dist(ap_cross, aps)
# Find angles from both APs
# [0: ap_1, 1: ap_2, 2: cross_x, 3: cross_y, 4: dist_1, 5: dist_2, 6: angle_1, 7: angle_2]
ap_cross = fn.crossings_angles(ap_cross, aps)
# Calculate total SD
# [0: ap_1, 1: ap_2, 2: cross_x, 3: cross_y, 4: dist_1, 5: dist_2, 6: angle_1, 7: angle_2, 8: SD_max, 9: SD_min]
ap_cross = fn.add_sd(ap_cross)
# Calculate position_error(crossing) in order to find optimal weights
# [0: ap_1, 1: ap_2, 2: cross_x, 3: cross_y, 4: dist_1, 5: dist_2, 6: angle_1, 7: angle_2, 8: SD_max, 9: SD_min]