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

#!/usr/bin/env python3
"""
vbs_synth.py - Virtual base station synthesis: geometric displacement engine.

Patches a RINEX 2.11 observation file so its observations appear to have been
collected at a different (virtual) position: per epoch/satellite, the change in
geometric range (independent light-time iteration + Earth-rotation correction at
each position, SP3 orbits) is added to every code observable (metres) and every
carrier-phase observable (cycles, per-signal wavelength). Everything else -
LLI/SSI flags, S/D observables, obs-type order, epoch structure - is preserved.
Satellites without SP3 orbits or (GLONASS) without a known frequency slot are
dropped, with epoch satellite lists rewritten accordingly.

The master receiver's clock is inherited by the virtual station (harmless: it is
one clock, absorbed by the processor's per-epoch receiver-clock estimation; see
the --perturb-clock-ms test mode). The atmospheric content of the master's
observations is NOT changed by this engine - a geometric-only virtual base
carries the master's atmosphere verbatim (Phase 2 applies interpolated
corrections on top via --iono).

Usage:
  vbs_synth.py master.obs --xyz X Y Z --sp3 FILE --nav BRDC.rnx -o out.obs
               [--master-xyz X Y Z] [--perturb-clock-ms N] [--marker NAME]
"""

import argparse
import math
import sys
from datetime import datetime, timedelta, timezone
from pathlib import Path

import numpy as np

C = 299792458.0
OMEGA_E = 7.2921151467e-5          # rad/s
GAMMA_12 = (1575.42 / 1227.60) ** 2

# carrier frequency by (system, band); GLONASS handled via slot
FREQ = {
    ("G", "1"): 1575.42e6, ("G", "2"): 1227.60e6, ("G", "5"): 1176.45e6,
    ("E", "1"): 1575.42e6, ("E", "5"): 1176.45e6,      # E1, E5a
    ("S", "1"): 1575.42e6,
}


def glo_freq(band, slot):
    if band == "1":
        return 1602.0e6 + slot * 562.5e3
    if band == "2":
        return 1246.0e6 + slot * 437.5e3
    return None


# ----------------------------------------------------------------------------
# SP3 orbits
# ----------------------------------------------------------------------------

class Sp3:
    """SP3 position source with sliding-window Lagrange interpolation."""

    def __init__(self, path):
        self.t0 = None
        times = []
        pos = {}           # sat -> {tidx: (x,y,z) m}
        tidx = -1
        for line in open(path, errors="replace"):
            if line.startswith("*"):
                p = line.split()
                t = datetime(int(p[1]), int(p[2]), int(p[3]), int(p[4]), int(p[5]),
                             int(float(p[6])), tzinfo=timezone.utc)
                if self.t0 is None:
                    self.t0 = t
                times.append((t - self.t0).total_seconds())
                tidx += 1
            elif line.startswith("P") and tidx >= 0:
                sat = line[1:4].replace(" ", "0")
                try:
                    x, y, z = (float(line[4:18]), float(line[18:32]), float(line[32:46]))
                except ValueError:
                    continue
                if abs(x) > 900000 or (x == 0 and y == 0):   # bad/absent
                    continue
                pos.setdefault(sat, {})[tidx] = (x * 1e3, y * 1e3, z * 1e3)
        self.times = np.array(times)
        self.sats = {}
        n = len(times)
        for sat, d in pos.items():
            if len(d) < n * 0.9:       # incomplete arcs: drop satellite
                continue
            arr = np.full((n, 3), np.nan)
            for i, xyz in d.items():
                arr[i] = xyz
            if np.isnan(arr).any():
                continue
            self.sats[sat] = arr

    def pos(self, sat, t_sec, order=10):
        """Satellite ECEF position (m) at t_sec (seconds from SP3 start)."""
        arr = self.sats.get(sat)
        if arr is None:
            return None
        i = np.searchsorted(self.times, t_sec)
        half = order // 2
        lo = max(0, min(i - half, len(self.times) - order))
        idx = slice(lo, lo + order)
        tt = self.times[idx]
        out = np.empty(3)
        # Lagrange interpolation per coordinate
        for k in range(3):
            y = arr[idx, k]
            acc = 0.0
            for j in range(len(tt)):
                lj = 1.0
                for m in range(len(tt)):
                    if m != j:
                        lj *= (t_sec - tt[m]) / (tt[j] - tt[m])
                acc += y[j] * lj
            out[k] = acc
        return out


def glonass_slots(nav_path):
    """sat 'Rnn' -> frequency slot number, from a RINEX 3 mixed nav file."""
    slots = {}
    lines = open(nav_path, errors="replace").read().splitlines()
    i = 0
    while i < len(lines):
        line = lines[i]
        if line[:1] == "R" and len(line) > 23 and line[1:3].strip().isdigit():
            sat = "R" + line[1:3].replace(" ", "0")
            if sat not in slots and i + 2 < len(lines):
                orbit2 = lines[i + 2]
                try:                       # 4th 19-char field on broadcast orbit 2
                    slots[sat] = int(float(orbit2[4 + 3 * 19: 4 + 4 * 19].replace("D", "E")))
                except ValueError:
                    pass
            i += 4
        else:
            i += 1
    return slots


# ----------------------------------------------------------------------------
# Geometry
# ----------------------------------------------------------------------------

def geometric_range(sp3, sat, t_rx_sec, x_rcv):
    """Light-time-iterated, Earth-rotation-corrected range (m); None if no orbit."""
    tau = 0.075
    rho = None
    for _ in range(3):
        xs = sp3.pos(sat, t_rx_sec - tau)
        if xs is None:
            return None
        ang = OMEGA_E * tau            # rotate satellite into reception-time frame
        ca, sa = math.cos(ang), math.sin(ang)
        xr = np.array([xs[0] * ca + xs[1] * sa, -xs[0] * sa + xs[1] * ca, xs[2]])
        rho = float(np.linalg.norm(xr - x_rcv))
        tau = rho / C
    return rho


# ----------------------------------------------------------------------------
# RINEX 2.11 observation patcher
# ----------------------------------------------------------------------------

def parse_header(lines):
    """Returns (obs_types, approx_xyz, end_idx)."""
    types, xyz = [], None
    for i, line in enumerate(lines):
        label = line[60:].strip()
        if label == "# / TYPES OF OBSERV":
            p = line[:60].split()
            if types == [] and p and p[0].isdigit():
                types.extend(p[1:])
            else:
                types.extend(p)
        elif label == "APPROX POSITION XYZ":
            xyz = tuple(float(v) for v in line[:60].split())
        elif label == "END OF HEADER":
            return types, xyz, i
    raise ValueError("no END OF HEADER")


def fmt_obs(value, lli, ssi):
    if value is None:
        return " " * 14 + lli + ssi
    return f"{value:14.3f}" + lli + ssi


def synthesize(master_path, out_path, virtual_xyz, sp3, slots, master_xyz=None,
               perturb_ms=0.0, marker="VBS"):
    lines = open(master_path, errors="replace").read().splitlines()
    types, hdr_xyz, hdr_end = parse_header(lines)
    ntypes = len(types)
    nlines_per_sat = (ntypes + 4) // 5
    x_mas = np.array(master_xyz if master_xyz else hdr_xyz)
    x_vrs = np.array(virtual_xyz)
    disp_km = np.linalg.norm(x_vrs - x_mas) / 1e3

    out = []
    for line in lines[:hdr_end + 1]:
        label = line[60:].strip()
        if label == "APPROX POSITION XYZ":
            out.append(f"{x_vrs[0]:14.4f}{x_vrs[1]:14.4f}{x_vrs[2]:14.4f}"
                       + " " * 18 + "APPROX POSITION XYZ")
        elif label == "MARKER NAME":
            out.append(f"{marker:<60}MARKER NAME")
        elif label == "END OF HEADER":
            out.append(f"{'VBS synthesized from ' + Path(master_path).name:<60}COMMENT")
            out.append(f"{f'displacement {disp_km:.3f} km; geometric only':<60}COMMENT")
            out.append(line)
        else:
            out.append(line)

    i = hdr_end + 1
    stats = {"epochs": 0, "sats": 0, "dropped": {}, "dr_min": 1e9, "dr_max": -1e9}
    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:                       # event records: copy verbatim
            out.append(line)
            for k in range(nsat):
                i += 1
                out.append(lines[i])
            i += 1
            continue
        # epoch time (GPS time system)
        p = line[:26].split()
        t_rx = datetime(2000 + int(p[0]), int(p[1]), int(p[2]), int(p[3]), int(p[4]),
                        tzinfo=timezone.utc) + timedelta(seconds=float(p[5]))
        t_sec = (t_rx - sp3.t0).total_seconds()
        # satellite list (12 per line, continuations)
        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
        # per-satellite observation blocks
        keep = []
        for sat in sats:
            block = lines[i:i + nlines_per_sat]
            i += nlines_per_sat
            sys_id = sat[0]
            if sys_id == "R" and sat not in slots:
                stats["dropped"][sat] = stats["dropped"].get(sat, 0) + 1
                continue
            rho_m = geometric_range(sp3, sat, t_sec, x_mas)
            if rho_m is None:
                stats["dropped"][sat] = stats["dropped"].get(sat, 0) + 1
                continue
            rho_v = geometric_range(sp3, sat, t_sec, x_vrs)
            dr = rho_v - rho_m
            stats["dr_min"] = min(stats["dr_min"], dr)
            stats["dr_max"] = max(stats["dr_max"], dr)
            # parse + patch the observation fields
            fields = []
            ok = True
            for ti, typ in enumerate(types):
                ln = block[ti // 5] if ti // 5 < len(block) else ""
                seg = ln[(ti % 5) * 16:(ti % 5) * 16 + 16].ljust(16)
                raw, lli, ssi = seg[:14], seg[14], seg[15]
                val = float(raw) if raw.strip() else None
                if val is not None:
                    if typ[0] in ("C", "P"):
                        val += dr + C * perturb_ms * 1e-3
                    elif typ[0] == "L":
                        f = (glo_freq(typ[1], slots[sat]) if sys_id == "R"
                             else FREQ.get((sys_id, typ[1])))
                        if f is None:
                            ok = False
                            break
                        lam = C / f
                        val += dr / lam + C * perturb_ms * 1e-3 / lam
                fields.append((val, lli, ssi))
            if not ok:
                stats["dropped"][sat] = stats["dropped"].get(sat, 0) + 1
                continue
            keep.append((sat, fields))
        # rewrite epoch record with surviving satellites
        if not keep:
            continue
        stats["epochs"] += 1
        stats["sats"] += len(keep)
        ids = [s for s, _ in keep]
        head = line[:29] + f"{len(ids):3d}"
        for k in range(0, len(ids), 12):
            seg = "".join(ids[k:k + 12])
            out.append((head if k == 0 else " " * 32) + seg)
        for sat, fields in keep:
            for li in range(nlines_per_sat):
                chunk = fields[li * 5:(li + 1) * 5]
                out.append("".join(fmt_obs(*f) for f in chunk).rstrip())

    Path(out_path).write_text("\n".join(out) + "\n")
    return stats, disp_km


def main():
    ap = argparse.ArgumentParser(description=__doc__,
                                 formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("master", type=Path, help="master RINEX 2.11 obs file")
    ap.add_argument("--xyz", nargs=3, type=float, required=True, metavar=("X", "Y", "Z"),
                    help="virtual position, ECEF metres (same frame as --master-xyz)")
    ap.add_argument("--sp3", type=Path, required=True)
    ap.add_argument("--nav", type=Path, required=True,
                    help="RINEX 3 mixed nav (for GLONASS frequency slots)")
    ap.add_argument("-o", "--out", type=Path, required=True)
    ap.add_argument("--master-xyz", nargs=3, type=float, metavar=("X", "Y", "Z"),
                    help="master position override (else RINEX header position)")
    ap.add_argument("--perturb-clock-ms", type=float, default=0.0,
                    help="TEST MODE: add a simulated receiver-clock offset to all "
                         "code+phase observables")
    ap.add_argument("--marker", default="VBS0")
    args = ap.parse_args()

    sp3 = Sp3(args.sp3)
    slots = glonass_slots(args.nav)
    stats, disp = synthesize(args.master, args.out, args.xyz, sp3, slots,
                             master_xyz=args.master_xyz,
                             perturb_ms=args.perturb_clock_ms, marker=args.marker)
    dropped = sum(stats["dropped"].values())
    print(f"[vbs_synth] {args.master.name} -> {args.out.name}: "
          f"displacement {disp:.3f} km, {stats['epochs']} epochs, "
          f"{stats['sats']} sat-epochs kept, {dropped} dropped "
          f"({len(stats['dropped'])} sats), dRho [{stats['dr_min']:.1f}, "
          f"{stats['dr_max']:.1f}] m")


if __name__ == "__main__":
    main()