from typing import Tuple
import numpy as np
import numpy.typing as npt
from perda.core_data_structures.data_instance import (
DataInstance,
left_join_data_instances,
)
from perda.core_data_structures.single_run_data import SingleRunData
from ..constants import EARTH_RADIUS
GPS_LATITUDE = "latitudeFiltered"
GPS_LONGITUDE = "longitudeFiltered"
POS_X = "posX"
POS_Y = "posY"
[docs]
def gps_to_local_xy(
latitudes: npt.NDArray[np.float64],
longitudes: npt.NDArray[np.float64],
origin: Tuple[float, float] | None = None,
) -> Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], Tuple[float, float]]:
"""
Equirectangular projection from lat/lon to local East/North coordinates (meters).
Parameters
----------
latitudes : NDArray[float64]
Array of latitude values in degrees (y/North)
longitudes : NDArray[float64]
Array of longitude values in degrees (x/East)
origin : (lat_deg, lon_deg) or None
Reference point for projection. If None, uses the median position.
Returns
-------
NDArray[float64]
x_m - East coordinates in meters (same axis as longitude/x)
NDArray[float64]
y_m - North coordinates in meters (same axis as latitude/y)
Tuple[float, float]
origin_latlon - The (lat, lon) reference point used for projection
"""
if origin is None:
lat_ref_deg = float(np.median(latitudes))
lon_ref_deg = float(np.median(longitudes))
else:
lat_ref_deg, lon_ref_deg = origin
lat_ref = np.radians(lat_ref_deg)
lon_ref = np.radians(lon_ref_deg)
# East (x/lon)
x_m = (np.radians(longitudes) - lon_ref) * EARTH_RADIUS * np.cos(lat_ref)
# North (y/lat)
y_m = (np.radians(latitudes) - lat_ref) * EARTH_RADIUS
return x_m, y_m, (lat_ref_deg, lon_ref_deg)
[docs]
def clean_gps_xy(
x_m: npt.NDArray[np.float64],
y_m: npt.NDArray[np.float64],
max_jump_m: float = 30.0,
max_radius_m: float = 1500.0,
) -> Tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], npt.NDArray[np.bool_]]:
"""
Remove outliers from projected GPS coordinates. Applies radius filter first to
drop points further than max_radius_m from the median position, then applies jump
filter to drop points that are large jumps (> max_jump_m) from the previous kept point.
Parameters
----------
x_m : NDArray[float64]
East coordinates in meters
y_m : NDArray[float64]
North coordinates in meters
max_jump_m : float
Maximum allowed step distance between consecutive points (meters).
max_radius_m : float
Maximum allowed distance from the median position (meters).
Returns
-------
NDArray[float64]
Cleaned x_m
NDArray[float64]
Cleaned y_m
NDArray[bool_]
Boolean mask, true where a point was kept.
"""
dist_from_median = np.sqrt(
(x_m - np.median(x_m)) ** 2 + (y_m - np.median(y_m)) ** 2
)
radius_mask = dist_from_median <= max_radius_m
x_r, y_r = x_m[radius_mask], y_m[radius_mask]
dx = np.diff(x_r, prepend=x_r[0])
dy = np.diff(y_r, prepend=y_r[0])
jump_mask_r = np.sqrt(dx**2 + dy**2) <= max_jump_m
# Combine back into a mask over the original arrays
keep = radius_mask.copy()
keep[radius_mask] = jump_mask_r
return x_m[keep], y_m[keep], keep
[docs]
def preprocess_gps_data_instances(
data: SingleRunData,
ins_lat_col: str = "pcm.vnav.posLla.latitude",
ins_lon_col: str = "pcm.vnav.posLla.longitude",
fix_col: str | None = "pcm.vnav.gpsFix",
) -> SingleRunData:
"""Extract GPS position DataInstances and inject them into SingleRunData.
Filters invalid samples (no GPS fix, zero lat/lon pairs), projects to
local East/North Cartesian coordinates, and stores the results as
``gps.latitude``, ``gps.longitude``, ``posE``, and ``posN`` in ``data``.
Parameters
----------
data : SingleRunData
ins_lat_col : str
Key for the INS latitude signal.
ins_lon_col : str
Key for the INS longitude signal.
fix_col : str or None
Key for the GPS fix-status signal. ``None`` skips the fix check.
Returns
-------
SingleRunData
The same ``data`` object with ``gps.latitude``, ``gps.longitude``,
``posN``, and ``posE`` added.
"""
lat_di = data[ins_lat_col]
lon_di = data[ins_lon_col]
# Join lon (and optionally fix) onto the lat timestamp grid
deps = [lon_di]
if fix_col is not None and fix_col in data:
deps.append(data[fix_col])
joined = left_join_data_instances(lat_di, deps)
lat_di_aligned = joined[0]
lon_di_aligned = joined[1]
# Filter invalid samples: zero pairs, out-of-physical-range, or non-finite
valid = (
np.isfinite(lat_di_aligned.value_np)
& np.isfinite(lon_di_aligned.value_np)
& ~((lat_di_aligned.value_np == 0.0) & (lon_di_aligned.value_np == 0.0))
& (np.abs(lat_di_aligned.value_np) <= 90.0)
& (np.abs(lon_di_aligned.value_np) <= 180.0)
)
if fix_col is not None and fix_col in data:
fix_di_aligned = joined[2]
valid = valid & (fix_di_aligned.value_np != 0.0)
lat_filtered = lat_di_aligned.value_np[valid].copy()
lon_filtered = lon_di_aligned.value_np[valid].copy()
ts_filtered = lat_di_aligned.timestamp_np[valid].copy()
x_m, y_m, _ = gps_to_local_xy(lat_filtered, lon_filtered)
x_m, y_m, valid_pts = clean_gps_xy(x_m, y_m)
lat_filtered = lat_filtered[valid_pts]
lon_filtered = lon_filtered[valid_pts]
ts_filtered = ts_filtered[valid_pts]
data[GPS_LATITUDE] = DataInstance(
timestamp_np=ts_filtered,
value_np=lat_filtered,
label="GPS Latitude (deg)",
cpp_name=GPS_LATITUDE,
)
data[GPS_LONGITUDE] = DataInstance(
timestamp_np=ts_filtered,
value_np=lon_filtered,
label="GPS Longitude (deg)",
cpp_name=GPS_LONGITUDE,
)
data[POS_X] = DataInstance(
timestamp_np=ts_filtered,
value_np=x_m,
label="X-axis/East Position (m)",
cpp_name=POS_X,
)
data[POS_Y] = DataInstance(
timestamp_np=ts_filtered,
value_np=y_m,
label="Y-axis/North Position (m)",
cpp_name=POS_Y,
)
return data