Source code for suboptimumg.log_analysis.preprocess_gps

import numpy as np
from perda.core_data_structures import (
    DataInstance,
    SingleRunData,
    left_join_data_instances,
)
from perda.plotting.plotting_constants import GpsMapConfig
from perda.utils import filter_gps_cartesian, gps_to_enu

GPS_LATITUDE = "latitudeFiltered"
GPS_LONGITUDE = "longitudeFiltered"
POS_X = "posX"
POS_Y = "posY"




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()

    valid_pts = filter_gps_cartesian(
        lat_filtered, lon_filtered, GpsMapConfig(max_radius_m=1500.0, max_jump_m=30.0)
    )
    lat_filtered = lat_filtered[valid_pts]
    lon_filtered = lon_filtered[valid_pts]
    ts_filtered = ts_filtered[valid_pts]

    x_m, y_m = gps_to_enu(lat_filtered, lon_filtered)

    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