ublox-gps-data-processing/experiments/vbs_compare.py

#!/usr/bin/env python3
"""
vbs_compare.py - Observation-domain comparison: Pivot VRS vs DIY geometric VBS.

Synthesizes a DIY virtual base at a Pivot VRS file's exact declared position
(same master frame), then differences the two files' observables per epoch and
satellite, double-differencing against the highest-elevation satellite to
remove both files' receiver-clock-like common terms.

What remains in the DD series is (Pivot's embedded network corrections +
Pivot's master data) minus (our master's raw atmosphere carried verbatim) -
i.e. the correction content the commercial network added relative to pure
geometric displacement, plus master-choice differences. Tracking its RMS
against extrapolation distance measures how much network correction actually
exists where the buoys will operate.

Usage:
  vbs_compare.py PIVOT.obs --master MASTER.obs --master-xyz X Y Z
                 --sp3 FILE --nav BRDC [--label NAME]
"""

import argparse
import sys
from pathlib import Path

import numpy as np

sys.path.insert(0, str(Path(__file__).resolve().parent))
from vbs_synth import (Sp3, parse_header, glonass_slots, glo_freq, FREQ, C,  # noqa
                       synthesize)


def read_l1p1(path, slots):
    """(epoch_key, sat) -> (L1_m, P1_m); L1 converted to metres."""
    lines = open(path, errors="replace").read().splitlines()
    types, xyz, hdr_end = parse_header(lines)
    nlps = (len(types) + 4) // 5
    idx = {t: k for k, t in enumerate(types)}
    i_l1 = idx["L1"]
    i_p1, i_c1 = idx.get("P1"), idx.get("C1")
    out = {}
    i = hdr_end + 1
    while i < len(lines):
        line = lines[i]
        if len(line) < 32 or not line[:26].strip():
            i += 1
            continue
        try:
            flag = int(line[26:29]); nsat = int(line[29:32])
        except ValueError:
            i += 1
            continue
        if flag > 1:
            i += nsat + 1
            continue
        tp = line[:26].split()
        tkey = tuple(int(v) for v in tp[:5]) + (round(float(tp[5]), 3),)
        sats = []
        nlin = (nsat + 11) // 12
        for k in range(nlin):
            seg = lines[i + k][32:68]
            for j in range(12):
                s = seg[j * 3:(j + 1) * 3]
                if s.strip():
                    sats.append(s.replace(" ", "0"))
        i += nlin
        p = line[:26].split()
        tsec = int(p[3]) * 3600 + int(p[4]) * 60 + float(p[5])
        for sat in sats:
            block = lines[i:i + nlps]
            i += nlps
            sysid = sat[0]
            if sysid == "G":
                f1 = FREQ[("G", "1")]
            elif sysid == "R" and sat in slots:
                f1 = glo_freq("1", slots[sat])
            else:
                continue

            def val(ti):
                if ti is None:
                    return None
                ln = block[ti // 5] if ti // 5 < len(block) else ""
                seg = ln[(ti % 5) * 16:(ti % 5) * 16 + 14]
                return float(seg) if seg.strip() else None

            l1 = val(i_l1)
            p1 = val(i_p1)
            if p1 is None:
                p1 = val(i_c1)
            if l1 is None or p1 is None:
                continue
            out[(tkey, sat)] = (tsec, l1 * (C / f1), p1)
    return out


def main():
    ap = argparse.ArgumentParser(description=__doc__,
                                 formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("pivot", type=Path)
    ap.add_argument("--master", type=Path, required=True)
    ap.add_argument("--master-xyz", nargs=3, type=float, required=True,
                    help="master position in the SAME frame as the pivot file's "
                         "declared position (NAD83 for GCGC)")
    ap.add_argument("--sp3", type=Path, required=True)
    ap.add_argument("--nav", type=Path, required=True)
    ap.add_argument("--label", default=None)
    ap.add_argument("--keep-synth", action="store_true")
    args = ap.parse_args()
    label = args.label or args.pivot.stem

    # pivot's declared position
    lines = open(args.pivot, errors="replace").read().splitlines()
    _, pivot_xyz, _ = parse_header(lines)
    x_v = np.array(pivot_xyz)
    disp_km = np.linalg.norm(x_v - np.array(args.master_xyz)) / 1e3

    sp3 = Sp3(args.sp3)
    slots = glonass_slots(args.nav)
    synth_path = args.pivot.with_suffix(".diy_synth.obs")
    if not synth_path.is_file():
        synthesize(args.master, synth_path, pivot_xyz, sp3, slots,
                   master_xyz=args.master_xyz, marker="DIYC")

    piv = read_l1p1(args.pivot, slots)
    diy = read_l1p1(synth_path, slots)

    # group common records per epoch
    epochs = {}
    for key in piv.keys() & diy.keys():
        tkey, sat = key
        epochs.setdefault(tkey, []).append(sat)

    dd_l1, dd_p1, used_epochs = {}, [], 0     # phase grouped per satellite
    x_rcv = x_v
    for tkey, sats in epochs.items():
        if len(sats) < 4:
            continue
        tsec = piv[(tkey, sats[0])][0]
        # reference satellite: highest elevation GPS
        def elev(sat):
            pos = sp3.pos(sat, tsec)
            if pos is None:
                return -99
            d = pos - x_rcv
            up = x_rcv / np.linalg.norm(x_rcv)
            return float(np.dot(d, up) / np.linalg.norm(d))
        gps = [s for s in sats if s[0] == "G"]
        if len(gps) < 2:
            continue
        ref = max(gps, key=elev)
        dl_ref = piv[(tkey, ref)][1] - diy[(tkey, ref)][1]
        dp_ref = piv[(tkey, ref)][2] - diy[(tkey, ref)][2]
        used_epochs += 1
        for sat in sats:
            if sat == ref:
                continue
            dl = piv[(tkey, sat)][1] - diy[(tkey, sat)][1] - dl_ref
            dp = piv[(tkey, sat)][2] - diy[(tkey, sat)][2] - dp_ref
            dd_l1.setdefault(sat, []).append(dl)
            if abs(dp) < 50:
                dd_p1.append(dp)

    # Phase DD contains an arbitrary integer-ambiguity constant per satellite
    # (each file's own arcs): remove the per-satellite median and analyse the
    # remaining VARIATION - that is the differential correction content the
    # network embedded relative to pure geometry (plus master noise).
    var_l1 = []
    for sat, vals in dd_l1.items():
        v = np.array(vals)
        v = v - np.median(v)
        v = v[np.abs(v) < 5.0]                   # drop cycle-slip re-levelings
        var_l1.append(v)
    var_l1 = np.concatenate(var_l1) if var_l1 else np.array([])
    dd_p1 = np.array(dd_p1)
    print(f"[compare] {label}: displacement master->virtual {disp_km:.1f} km, "
          f"{used_epochs} epochs")
    if len(var_l1):
        print(f"  L1 phase DD variation (per-sat ambiguity removed): "
              f"median|d|={np.median(np.abs(var_l1)):6.3f} m  "
              f"RMS={np.sqrt((var_l1**2).mean()):6.3f} m  "
              f"95%|d|={np.percentile(np.abs(var_l1),95):6.3f} m  n={len(var_l1)}")
    if len(dd_p1):
        print(f"  P1 code DD (incl. master code noise):         "
              f"median|d|={np.median(np.abs(dd_p1)):6.3f} m  "
              f"RMS={np.sqrt((dd_p1**2).mean()):6.3f} m  "
              f"95%|d|={np.percentile(np.abs(dd_p1),95):6.3f} m")
    if not args.keep_synth:
        synth_path.unlink(missing_ok=True)


if __name__ == "__main__":
    main()