# 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]
# [10: pos_error]
for i in range(len(ap_cross)):
ap_cross[i].append(math.sqrt((ap_cross[i][2] - mobile[0]) ** 2 +
(ap_cross[i][3] - mobile[1]) ** 2))
# Save parameters
pos_error.append(ap_cross[i][-1])
sd_max.append(ap_cross[i][8])
sd_min.append(ap_cross[i][9])
sd_tot.append(math.sqrt(ap_cross[i][8]**2 + ap_cross[i][9]**2))
print k
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)
# remove not valid weights
weights_valid = weights * remove_not_valid
# Calculate grid
pos = fn.estimate_xy(x_cross, y_cross, weights, remove_not_valid)
# Remove points from outside, still doesn't work
pos = fn.remove_outside(pos, boundaries)
y_intercept = track.aps[track.valid_ants, 1] * np.ones(slopes.shape) - slopes * track.aps[track.valid_ants, 0]
couples = itertools.combinations(track.valid_ants, 2)
for crossing in couples:
# Calculating cross-points
x_cross, y_cross = fn.crossings(slopes, y_intercept, crossing)
# Calculate distance between exp.aps and cross point
dist0, dist1 = fn.crossings_dist(track.aps, crossing, x_cross, y_cross)
# Find angles from both exp.aps
angle0 = doa.ap_timed_pred[:, crossing[0]]
angle1 = doa.ap_timed_pred[:, crossing[1]]
# Calculate total SD
sdmax, sdmin = fn.add_sd(angle0, angle1, dist0, dist1)
weights = fn.find_weights(sdmax)
# Calculate position_error(crossing) in order to find optimal weights
# [x, y]
pos = fn.estimate_xy(x_cross, y_cross, weights)
# Change NaN to last known position
pos = fn.remove_nan(pos)
# Remove points from outside
# pos = fn.remove_outside(pos)
# Holt's filtering algorithm
holt = np.zeros(pos.shape)