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]
