prelim_pos = []
weights = []
for crossing in couples:
# Calculating cross-points
prelim_pos.append(fn_exp.crossings(slopes[j, :], y_intercept[j, :], crossing))
# Calculate distance between exp.aps and cross point
dist0, dist1 = fn_exp.crossings_dist(track.aps, crossing, prelim_pos[-1])
# Find angles from both exp.aps
angle0 = global_angle[j, crossing[0]]
angle1 = global_angle[j, crossing[1]]
# Calculate total SD
sdmax, sdmin = fn_exp.add_sd(angle0, angle1, dist0, dist1)
weights.append(fn.find_weights(sdmax))
# Calculate position_error(crossing) in order to find optimal weights
# [x, y]
prelim_pos = np.array(prelim_pos)
weights = np.array(weights)
prelim_pos_x = prelim_pos[:, 0]
prelim_pos_y = prelim_pos[:, 1]
pos[j] = fn_exp.estimate_xy(prelim_pos_x, prelim_pos_y, weights)
print pos[:30, :]
# # Change NaN to last known position
# pos = fn.remove_nan(pos)
# Remove points from outside
# pos = fn.remove_outside(pos)
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)
# Holt's filtering algorithm
holt = np.zeros(pos.shape)
holt[0, :] = pos[0, :]
holt_trend = np.zeros(pos.shape)
