def test_SpherePath_calculate_position(self): ecef_points = global_Point3d(ROUTE_LATS, ROUTE_LONS) ecef_path = SpherePath(ecef_points, TURN_DISTANCES) pos_0 = ecef_path.calculate_position(0, 0.0) assert_almost_equal(distance_radians(pos_0, ecef_points[0]), 0.0) pos_1 = ecef_path.calculate_position(1, 0.0) assert_almost_equal(distance_radians(pos_1, ecef_points[1]), 0.0) arc2 = Arc3d(ecef_points[1], ecef_points[2]) arc3 = Arc3d(ecef_points[2], ecef_points[3]) turn_arc2 = SphereTurnArc(arc2, arc3, TURN_DISTANCES[2]) turn2_midpoint = turn_arc2.position(0.5 * abs(turn_arc2.angle)) pos_2 = ecef_path.calculate_position(2, 0.000001) assert_almost_equal(distance_radians(pos_2, turn2_midpoint), 0.0) pos_1_99999 = ecef_path.calculate_position(1, 0.99999) assert_almost_equal(distance_radians(pos_1_99999, pos_2), 0.0, decimal=6) pos_13 = ecef_path.calculate_position(13, 0.0) assert_almost_equal(distance_radians(pos_13, ecef_points[-1]), 0.0)
def test_derive_horizontal_path(self): test_data_home = env.get('TEST_DATA_HOME') self.assertTrue(test_data_home) ecef_points = global_Point3d(ROUTE_LATS, ROUTE_LONS) ecef_path = derive_horizontal_path(ecef_points, ACROSS_TRACK_TOLERANCE) self.assertEqual(len(ecef_path), 12) assert_almost_equal( distance_radians(ecef_path.points[0], ecef_points[0]), 0.0) assert_almost_equal( distance_radians(ecef_path.points[-1], ecef_points[-1]), 0.0)
def test_SphereTurnArc_invalid_init(self): """Test initialisation of SphereTurnArc class.""" ecef_point_0 = Point3d(ECEF_ICOSAHEDRON[1][0], ECEF_ICOSAHEDRON[1][1], ECEF_ICOSAHEDRON[1][2]) ecef_point_1 = Point3d(ECEF_ICOSAHEDRON[2][0], ECEF_ICOSAHEDRON[2][1], ECEF_ICOSAHEDRON[2][2]) arc_0 = Arc3d(ecef_point_0, ecef_point_1) arc_1 = Arc3d(ecef_point_1, ecef_point_0) turn_0 = SphereTurnArc(arc_0, arc_1, TWENTY_NM) self.assertFalse(turn_0) # Ensure Turn start, centre and end points are at waypoint assert_almost_equal(distance_radians(ecef_point_1, turn_0.start), 0.0) assert_almost_equal(distance_radians(ecef_point_1, turn_0.centre), 0.0) assert_almost_equal(distance_radians(ecef_point_1, turn_0.finish), 0.0)
def calculate_intersection(prev_arc, arc): """ Calculate the intersection point between a pair of arcs. If the arcs are on (or very close to) the same Great Circle, it returns the start point of the second arc. Parameters ---------- prev_arc, arc: Arc3ds The arcs before and after the intersection. Returns ------- The intersection point of the arcs. """ intersection = Arc3d(prev_arc.pole(), arc.pole()) if MIN_LENGTH < intersection.length() < MAX_LENGTH: intersection_point = intersection.pole() # swap sign if intersection_point is antipodal point return -intersection_point \ if distance_radians(arc.a(), intersection_point) > HALF_PI else \ intersection_point else: return arc.a()
def test_SpherePath_calculate_positions(self): ecef_points = global_Point3d(ROUTE_LATS, ROUTE_LONS) ecef_path = SpherePath(ecef_points, TURN_DISTANCES) distances, types = ecef_path.section_distances_and_types() positions = ecef_path.calculate_positions(distances) self.assertEqual(len(positions), len(ecef_path) + 8) assert_almost_equal(distance_radians(positions[0], ecef_points[0]), 0.0) assert_almost_equal(distance_radians(positions[1], ecef_points[1]), 0.0) assert_almost_equal(distance_radians(positions[-2], ecef_points[-2]), 0.0) assert_almost_equal(distance_radians(positions[-1], ecef_points[-1]), 0.0)
def calculate_turn_initiation_distance(prev_arc, arc, point, max_distance, threshold): """ Calculate the turn initation distance from the arc legs and a point. Parameters ---------- prev_arc: Arc3d The inbound path leg. arc: Arc3d The outbound path leg. point: Point3d A point between the inbound and outbound legs. max_distance : float The maximum turn initiation distance [radians]. threshold : float The across track distance threshold [radians] Returns ------- The turn initation distance [radians]. """ # Calculate the distance from the intersection to the point distance = distance_radians(arc.a(), point) if distance < max_distance: xtd_in = abs(prev_arc.cross_track_distance(point)) xtd_out = abs(arc.cross_track_distance(point)) # determine whether the point is close to either leg if (xtd_in > threshold) and (xtd_out > threshold): # calculate the bisector of the turn legs bisector = calculate_bisector(prev_arc, arc) xtd = abs(bisector.cross_track_distance(point)) if xtd < distance: # calculate the angle from the bisector of the turn legs # Note: use arccos since the bisector is prependicular to pole angle = np.arccos(xtd / distance) half_turn_angle = abs(prev_arc.turn_angle(arc.b())) / 2 # calculate the turn radius cos_angle = np.cos(angle) cos_half_turn_angle = np.cos(half_turn_angle) sin2_half_turn_angle = 1 - cos_half_turn_angle**2 # ensure that factor is never negative for sqrt factor = max(cos_angle**2 - sin2_half_turn_angle, 0.0) radius = distance * cos_half_turn_angle * \ (cos_angle + np.sqrt(factor)) / sin2_half_turn_angle # Calculate turn initiation distance from the radius distance = radius * np.tan(half_turn_angle) return min(distance, max_distance)
def test_calculate_intersection(self): ecef_points = global_Point3d(ROUTE_LATS, ROUTE_LONS) # intersection point between arcs on different Great Circles prev_arc = Arc3d(ecef_points[0], ecef_points[1]) arc = Arc3d(ecef_points[1], ecef_points[2]) point_1 = calculate_intersection(prev_arc, arc) assert_almost_equal(distance_radians(point_1, ecef_points[1]), 0.0) # intersection point between arcs on same Great Circles next_point = arc.position(2 * arc.length()) next_arc = Arc3d(ecef_points[2], next_point) point_2 = calculate_intersection(arc, next_arc) assert_almost_equal(distance_radians(point_2, ecef_points[2]), 0.0) # intersection point between arcs on different Great Circles, # opposite direction turn prev_arc = Arc3d(ecef_points[5], ecef_points[6]) arc = Arc3d(ecef_points[6], ecef_points[7]) point_3 = calculate_intersection(prev_arc, arc) assert_almost_equal(distance_radians(point_3, ecef_points[6]), 0.0)
def test_SpherePath_subsection_positions(self): ecef_points = global_Point3d(ROUTE_LATS, ROUTE_LONS) ecef_path = SpherePath(ecef_points, TURN_DISTANCES) positions0 = ecef_path.subsection_positions(0, 1040.0) self.assertEqual(len(positions0), len(ecef_path)) assert_almost_equal(distance_radians(positions0[0], ecef_points[0]), 0.0) assert_almost_equal(distance_radians(positions0[-1], ecef_points[-1]), 0.0) positions1 = ecef_path.subsection_positions(0, 1000.0) self.assertEqual(len(positions1), len(ecef_path)) assert_almost_equal(distance_radians(positions1[0], ecef_points[0]), 0.0) assert_almost_equal(distance_radians(positions1[-2], ecef_points[-2]), 0.0) positions2 = ecef_path.subsection_positions(100.0, 1040.0) self.assertEqual(len(positions2), len(ecef_path) - 1) assert_almost_equal(distance_radians(positions2[1], ecef_points[2]), 0.0) assert_almost_equal(distance_radians(positions2[-1], ecef_points[-1]), 0.0)
def test_fit_arc_to_points(self): LATS_0 = np.zeros(4, dtype=np.float) LONS_0 = np.array([0.0, 1.0, 2.0, 3.0]) # Test points along arc ecef_points_0 = global_Point3d(LATS_0, LONS_0) ecef_arc_0 = Arc3d(ecef_points_0[0], ecef_points_0[-1]) new_arc_0 = fit_arc_to_points(ecef_points_0, ecef_arc_0) assert_almost_equal(distance_radians(new_arc_0.a(), ecef_arc_0.a()), 0.0) assert_almost_equal(distance_radians(new_arc_0.b(), ecef_arc_0.b()), 0.0) # Test slope away from start of arc ecef_points_1 = global_Point3d(LONS_0, LONS_0) ecef_arc_1 = Arc3d(ecef_points_1[0], ecef_points_1[-1]) new_arc_1 = fit_arc_to_points(ecef_points_1, ecef_arc_0) assert_almost_equal(distance_radians(new_arc_1.a(), ecef_arc_0.a()), 0.0) assert_almost_equal( distance_radians(new_arc_1.pole(), ecef_arc_1.pole()), 0.0) # Test slope towards end of arc LATS_2 = np.array([3.0, 2.0, 1.0, 0.0]) ecef_points_2 = global_Point3d(LATS_2, LONS_0) ecef_arc_2 = Arc3d(ecef_points_2[0], ecef_points_2[-1]) new_arc_2 = fit_arc_to_points(ecef_points_2, ecef_arc_0) assert_almost_equal( distance_radians(new_arc_2.pole(), ecef_arc_2.pole()), 0.0) assert_almost_equal(distance_radians(new_arc_2.b(), ecef_arc_0.b()), 0.0)
def distance_nm(a, b): """ Calculate the Great Circle distance between two EcefPoints: a and b. Parameters ---------- a, b: EcefPoints. Returns ------- distance: float The Great Circle distance between a and b in [Nautical Miles]. """ return rad2nm(distance_radians(a, b))
def radial_distance(self, point): """ Calculate the distance of a point from the centre of the turn arc. Parameters ---------- point: Point3d The point to measure. Returns ------- distance: float The distance between point and the centre of the turn [radians]. """ return distance_radians(self.centre, point)
def test_calculate_position(self): ecef_points = global_Point3d(PANDAS_ICOSAHEDRON['LAT'], PANDAS_ICOSAHEDRON['LON']) point_0 = calculate_position(ecef_points, 0) assert_almost_equal(distance_radians(point_0, ecef_points[0]), 0.0) point_11 = calculate_position(ecef_points, 11) assert_almost_equal(distance_radians(point_11, ecef_points[-1]), 0.0) point_11_5 = calculate_position(ecef_points, 11, ratio=0.5) assert_almost_equal(distance_radians(point_11_5, ecef_points[-1]), 0.0) point_5_5 = calculate_position(ecef_points, 5, ratio=0.5) assert_almost_equal(distance_radians(point_5_5, ecef_points[5]), 0.5 * GOLDEN_ANGLE) assert_almost_equal(distance_radians(point_5_5, ecef_points[6]), 0.5 * GOLDEN_ANGLE) point_7_25 = calculate_position(ecef_points, 7, ratio=0.25) assert_almost_equal(distance_radians(point_7_25, ecef_points[7]), 0.25 * GOLDEN_ANGLE) assert_almost_equal(distance_radians(point_7_25, ecef_points[8]), 0.75 * GOLDEN_ANGLE)
def test_SphereTurnArc_init(self): """Test initialisation of SphereTurnArc class.""" ecef_point_0 = Point3d(ECEF_ICOSAHEDRON[1][0], ECEF_ICOSAHEDRON[1][1], ECEF_ICOSAHEDRON[1][2]) ecef_point_1 = Point3d(ECEF_ICOSAHEDRON[2][0], ECEF_ICOSAHEDRON[2][1], ECEF_ICOSAHEDRON[2][2]) ecef_point_2 = Point3d(ECEF_ICOSAHEDRON[3][0], ECEF_ICOSAHEDRON[3][1], ECEF_ICOSAHEDRON[3][2]) arc_0 = Arc3d(ecef_point_0, ecef_point_1) arc_1 = Arc3d(ecef_point_1, ecef_point_2) turn_0 = SphereTurnArc(arc_0, arc_1, TWENTY_NM) self.assertTrue(turn_0) # Ensure Turn start and end points are along inbound and outbound arcs assert_almost_equal(distance_radians(ecef_point_1, turn_0.start), TWENTY_NM) assert_almost_equal(distance_radians(ecef_point_1, turn_0.finish), TWENTY_NM) assert_almost_equal(arc_0.cross_track_distance(turn_0.start), 0.0) assert_almost_equal(arc_1.cross_track_distance(turn_0.finish), 0.0) ANGLE = 0.2 * np.pi RADIUS = 20.0 / np.tan(ANGLE / 2.0) # 61.553670693462841 NM assert_almost_equal(turn_0.angle, -ANGLE) assert_almost_equal(rad2nm(turn_0.radius), RADIUS) assert_almost_equal(rad2nm(turn_0.length()), RADIUS * ANGLE) assert_almost_equal(turn_0.radial_distance(turn_0.start), turn_0.radius, decimal=6) assert_almost_equal(turn_0.radial_distance(turn_0.finish), turn_0.radius, decimal=6) DISTANCE = TWENTY_NM / np.sin(ANGLE / 2.0) # 64.721359549995796 NM assert_almost_equal(turn_0.radial_distance(ecef_point_1), DISTANCE) assert_almost_equal(turn_0.cross_track_distance(turn_0.start), 0.0, decimal=6) assert_almost_equal(turn_0.cross_track_distance(turn_0.finish), 0.0, decimal=6) assert_almost_equal(turn_0.cross_track_distance(ecef_point_1), DISTANCE - turn_0.radius) self.assertEqual(turn_0.point_angle(turn_0.centre), 0.0) assert_almost_equal(turn_0.point_angle(turn_0.start), 0.0) assert_almost_equal(turn_0.point_angle(turn_0.finish), turn_0.angle, decimal=4) assert_almost_equal(turn_0.point_angle(ecef_point_1), turn_0.angle / 2.0, decimal=4) assert_almost_equal(turn_0.along_track_distance(turn_0.start), 0.0) assert_almost_equal(turn_0.along_track_distance(turn_0.finish), turn_0.length(), decimal=6) assert_almost_equal(turn_0.along_track_distance(ecef_point_1), turn_0.length() / 2.0, decimal=6) pos_1 = turn_0.position(turn_0.angle) assert_almost_equal(distance_radians(pos_1, turn_0.centre), turn_0.radius) assert_almost_equal(distance_radians(pos_1, turn_0.finish), 0.0, decimal=6) pos_2 = turn_0.position(turn_0.angle / 2.0) assert_almost_equal(distance_radians(pos_2, turn_0.centre), turn_0.radius) assert_almost_equal(turn_0.point_angle(pos_2), turn_0.angle / 2.0)
def find_extreme_point_index(points, first_index, last_index, threshold, xtd_ratio, calc_along_track): """ Find the index of the point in points furthest from the Arc. The points are searched between first_index and last_index. If the Arc is longer than MINIMUM_ARC_LENGTH, it finds the point with the largest across track distance. If the distance is larger than threshold or xtd_ratio time the arc length, it returns the index of the point. Otherwise, it calculates whether any points are beyond the end of the arc, if so it returns the index of the furthest point. If the Arc is not longer than MINIMUM_ARC_LENGTH, if finds the furthest point the start. If the distance from the point to the start and end points is greater than MINIMUM_ARC_LENGTH, it returns the index of the point. Parameters ---------- points: numpy array of Point3ds The trajectory points in spherical vector coordinates. first_index, last_index: integers Indicies of the first and last points to use. threshold: float The across track distance threshold. Returns ------- The index of the point furthest from the Arc joining the points at first_index and last_index. """ max_xtd_index = last_index # If there is at least a point between first_index and last_index if ((last_index - first_index) > 1): arc = Arc3d(points[first_index], points[last_index]) # first point is after arc start if arc.length() > MINIMUM_ARC_LENGTH: # calculate cross track distances relative to the base arc xtds = calculate_xtds(arc, points[first_index + 1:last_index]) max_xtd, xtd_index = find_most_extreme_value(xtds) xtd_index += 1 # set the threshold to the minimum of the threshold or a # fraction of the arc length xtd_threshold = min(threshold, xtd_ratio * arc.length()) xtd_threshold = max(xtd_threshold, MINIMUM_ARC_LENGTH) if (np.abs(max_xtd) > xtd_threshold): # if the point is further than the threshold, return it max_xtd_index = first_index + xtd_index elif calc_along_track: # points are in-line # test whether a point is past the start or end of the arc atd_index = find_extreme_point_along_track_index( arc, points[first_index:last_index], MINIMUM_ARC_LENGTH) if atd_index: max_xtd_index = first_index + atd_index else: # short arc # calculate the furthest point from the start point distance, xtd_index = \ find_furthest_distance(points[first_index: last_index]) if (distance > MINIMUM_ARC_LENGTH): # ensure that the point is far enough from the end point xtd_index += first_index end_distance = distance_radians(points[xtd_index], points[last_index]) if end_distance > MINIMUM_ARC_LENGTH: max_xtd_index = xtd_index return max_xtd_index
def find_invalid_positions(points_df, *, max_speed=DEFAULT_MAX_SPEED, distance_accuracy=DEFAULT_DISTANCE_ACCURACY, time_precision=DEFAULT_TIME_PRECISION, find_invalid_addresses=True): """ Find invalid positions in points_df. The function searches for: - duplicate positions with an aircraft address, - positions with a different aircraft address to the the flight, - horizontal positions which require an aircraft to fly over max_speed, - changes in vertical state from climbing to descending (or vice versa) without going through a level phase Invalid positions are set to True in the invalid_positions array. Parameters ---------- points_df: a pandas DataFrame A DataFrame containing raw positions for a flight, sorted in time order. max_speed: float The maximum ground speed permitted between adjacent positions [Knots], default: 750 Knots. distance_accuracy: float The maximum distance between positions at the same time [Nautical Miles], default: 0.25 NM. time_precision: float The precision of time measurement [Seconds], default 1.0. find_invalid_addresses: bool A flag to enable/disable finding invalid aircraft addresses for testing. Returns ------- invalid_positions : numpy bool array Invalid postions are set to True in this array. error_counts: array of ints Error counts are: - total number of invalid points - number of duplicate positions - number of invalid addresses - number of distance errors - number of altitude errors """ # Duplicate positions with an aircraft address are invalid aircraft_address = points_df['AIRCRAFT_ADDRESS'] invalid_positions = points_df.duplicated( subset=['TIME', 'LAT', 'LON', 'ALT', 'AIRCRAFT_ADDRESS', 'SSR_CODE']).values & \ (aircraft_address != None) # get the number of duplicate_positions duplicate_positions = np.count_nonzero(invalid_positions) # Different aircraft addresses are invalid invalid_addresses = 0 aircraft_address = aircraft_address.loc[aircraft_address != None] address_counts = aircraft_address.value_counts() if find_invalid_addresses and (len(address_counts) > 1): # More than one aircraft address, mark the extra addresses as invalid for address in address_counts[1:].index: invalid_addr = (aircraft_address == address) invalid_addresses = np.count_nonzero(invalid_addr) invalid_positions |= invalid_addr # Calculate: positions, horizontal and vertical distances, etc. ecef_points = global_Point3d(points_df['LAT'].values, points_df['LON'].values) altitudes = points_df['ALT'].values times = calculate_elapsed_times(points_df['TIME'].values, points_df['TIME'].values[0]) ssr_codes = points_df['SSR_CODE'].values # Counts of errors distance_errors = 0 altitude_errors = 0 ref_attitude = 0 ref_i = 0 # The last known good index prev_i = 0 # The previous position index used for i in range(1, len(points_df)): # Only consider valid positions if not invalid_positions.iloc[i]: # Calculate speed from previous known good position distance = rad2nm(distance_radians(ecef_points[i], ecef_points[ref_i])) delta_time = times[i] - times[ref_i] speed = calculate_min_speed(distance, delta_time, distance_accuracy, time_precision) invalid = False if speed > max_speed: invalid = True distance_errors += 1 # The attitude is: 1, if climbing, -1 if descending and 0 if level attitude = altitudes[i] - altitudes[prev_i] attitude = 1 if (attitude > 0) else -1 if (attitude < 0) else 0 # if the attitude has changed if ref_attitude != attitude: # and the SSR code hasn't if ssr_codes[i] == ssr_codes[ref_i]: ref_attitude = attitude # but if the SSR code is definitely different elif ssr_codes[i] != ssr_codes[prev_i]: invalid = True altitude_errors += 1 if invalid: # Mark the position as invalid invalid_positions.iloc[i] = True else: # update the last known good position ref_i = i # Update the previously used index prev_i = i return invalid_positions, [np.count_nonzero(invalid_positions), duplicate_positions, invalid_addresses, distance_errors, altitude_errors]