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)