Using HDOP and satellite count to weight GPS fixes

import numpy as np
import pandas as pd


def fix_uncertainty(
    df: pd.DataFrame,
    hdop_col: str = "hdop",
    sats_col: str | None = "sats",
    fixtype_col: str | None = "fix_type",
    sigma_range_m: float = 2.2,
    hdop_reject: float = 20.0,
) -> pd.DataFrame:
    """
    Convert receiver quality fields into a per-fix positional sigma.

    Parameters
    ----------
    sigma_range_m : float
        The receiver's ranging error, in metres. CALIBRATE THIS: it is the
        one number that makes HDOP comparable across fleets, and the default
        is a mid-range consumer module under open sky.
    hdop_reject : float
        A hard gate for the implausible only. HDOP above 20 usually means
        the receiver reported a fix it should not have; below that, weight
        rather than drop.

    Returns
    -------
    pd.DataFrame
        Input plus 'sigma_m', 'variance_m2' and 'usable'.

    Raises
    ------
    ValueError
        On a missing HDOP column or an empty frame.
    """
    if hdop_col not in df.columns:
        raise ValueError(f"Missing quality column: {hdop_col}")
    if df.empty:
        raise ValueError("Input DataFrame is empty.")

    out = df.copy()
    hdop = out[hdop_col].to_numpy(dtype=float)

    # A missing or zero HDOP is not a perfect fix — it is a receiver that did
    # not report one. Treat it as mediocre rather than excellent.
    hdop = np.where(np.isfinite(hdop) & (hdop > 0), hdop, 3.0)

    sigma = sigma_range_m * hdop

    # ── satellite count adjustment ────────────────────────────────────
    # Fewer satellites means a weaker solution than HDOP alone conveys,
    # because the geometry is not just poor but under-determined.
    if sats_col and sats_col in out:
        n = out[sats_col].to_numpy(dtype=float)
        penalty = np.where(n >= 8, 1.0, np.where(n >= 6, 1.25, np.where(n >= 4, 1.8, 3.0)))
        sigma = sigma * np.nan_to_num(penalty, nan=1.5)

    # ── fix type ──────────────────────────────────────────────────────
    # A 2D fix has no vertical constraint, which degrades the horizontal
    # solution as well; a differential fix is substantially better.
    if fixtype_col and fixtype_col in out:
        ft = out[fixtype_col].astype(str).str.lower()
        sigma = sigma * np.select(
            [ft.str.contains("dgps|rtk"), ft.str.contains("2d")],
            [0.35, 1.6],
            default=1.0,
        )

    out["sigma_m"] = sigma
    out["variance_m2"] = sigma ** 2
    out["usable"] = hdop <= hdop_reject
    return out


def measurement_covariance(sigma_m: np.ndarray) -> np.ndarray:
    """
    Per-fix 2x2 measurement covariance for a Kalman filter.

    Passing a per-fix R is what makes a filter downweight a poor fix
    automatically. A constant R forces the same trust in every measurement,
    which is precisely the information HDOP was reporting.
    """
    s = np.asarray(sigma_m, dtype=float)
    if np.any(~np.isfinite(s)) or np.any(s <= 0):
        raise ValueError("sigma_m must be finite and positive for every fix.")
    R = np.zeros((len(s), 2, 2))
    R[:, 0, 0] = s ** 2
    R[:, 1, 1] = s ** 2
    return R

def validate_uncertainty(out: pd.DataFrame, residuals_m: np.ndarray | None = None) -> dict:
    """
    Check the sigma is calibrated, not just plausible.

    residuals_m : distance from each fix to a trusted reference position,
    available from an instrumented run. Without it, only the shape can be
    checked; with it, the scale can.
    """
    s = out["sigma_m"]
    assert (s > 0).all() and np.isfinite(s).all(), "non-positive or non-finite sigma"

    # 1. Range sanity. A consumer receiver under open sky is 2-5 m; anything
    #    claiming sub-metre without RTK is mis-parameterised.
    assert s.min() > 0.8, f"minimum sigma {s.min():.2f} m implies RTK — check sigma_range_m"
    assert s.median() < 40, f"median sigma {s.median():.0f} m — the HDOP column may be scaled"

    out_stats = {"median_sigma_m": float(s.median()),
                 "p95_sigma_m": float(s.quantile(0.95)),
                 "rejected_frac": float((~out["usable"]).mean())}

    # 2. Calibration against truth: about 68% of residuals should fall inside
    #    one sigma if the scale is right. Far below means over-confident.
    if residuals_m is not None:
        within = float((residuals_m <= s.to_numpy()).mean())
        assert 0.55 < within < 0.82, (
            f"{within:.0%} of residuals within 1σ (expect ~68%) — "
            f"{'over-confident' if within < 0.55 else 'over-cautious'}; "
            "recalibrate sigma_range_m"
        )
        out_stats["within_1sigma"] = within

    # 3. The hard gate should be rare. A high rejection rate means the gate
    #    is doing the weighting's job.
    assert out_stats["rejected_frac"] < 0.02, (
        f"{out_stats['rejected_frac']:.1%} hard-rejected — lower the reliance "
        "on the gate and let the weight handle marginal fixes"
    )
    return out_stats