GNSS, NavIC, field data and GPS tracks

"""A receiver's position from pseudoranges: four unknowns, east, north, up and the receiver's clock error, solved from
satellite ranges; then the same range errors under good and poor satellite geometry."""
import math
import random

C = 299_792_458.0  # the speed of light, m/s
RANGE = 22_000_000.0  # a satellite of a 20,000 km orbit is about 20,000 to 26,000 km away; one value keeps it simple


def satellite(azimuth, elevation):
    """A satellite's position east, north and up of the receiver, from its azimuth and elevation in degrees."""
    a, e = math.radians(azimuth), math.radians(elevation)
    return (RANGE * math.cos(e) * math.sin(a), RANGE * math.cos(e) * math.cos(a), RANGE * math.sin(e))


def solve(sats, pseudoranges, steps=8):
    """Least squares for (east, north, up, clock error in metres): start at the origin and refine."""
    x = [0.0, 0.0, 0.0, 0.0]
    for _ in range(steps):
        rows, residuals = [], []
        for s, p in zip(sats, pseudoranges):
            d = math.dist(s, x[:3])
            rows.append([(x[k] - s[k]) / d for k in range(3)] + [1.0])
            residuals.append(p - (d + x[3]))
        n = len(rows[0])
        ata = [[sum(r[i] * r[j] for r in rows) for j in range(n)] for i in range(n)]
        atb = [sum(r[i] * v for r, v in zip(rows, residuals)) for i in range(n)]
        step = gauss(ata, atb)
        x = [a + b for a, b in zip(x, step)]
    return x, rows


def gauss(a, b):
    """Solve a·x = b for a small square system (Gaussian elimination with pivoting)."""
    n = len(b)
    m = [row[:] + [v] for row, v in zip(a, b)]
    for i in range(n):
        p = max(range(i, n), key=lambda r: abs(m[r][i]))
        m[i], m[p] = m[p], m[i]
        for r in range(i + 1, n):
            f = m[r][i] / m[i][i]
            m[r] = [x - f * y for x, y in zip(m[r], m[i])]
    out = [0.0] * n
    for i in reversed(range(n)):
        out[i] = (m[i][n] - sum(m[i][k] * out[k] for k in range(i + 1, n))) / m[i][i]
    return out


def dop(rows):
    """Position and horizontal dilution of precision from the geometry: the square roots of (AᵀA)⁻¹'s diagonal."""
    n = len(rows[0])
    ata = [[sum(r[i] * r[j] for r in rows) for j in range(n)] for i in range(n)]
    inv = [gauss(ata, [1.0 if k == j else 0.0 for k in range(n)]) for j in range(n)]  # columns of the inverse
    return math.sqrt(inv[0][0] + inv[1][1] + inv[2][2]), math.sqrt(inv[0][0] + inv[1][1])


clock = 0.001  # the receiver's clock is 1 ms fast
print(f"a clock error of 1 ms is {C * clock:,.0f} m of range on every satellite")
spread = [satellite(az, el) for az, el in ((0, 75), (90, 30), (200, 40), (300, 25), (45, 15), (250, 60))]
exact = [math.dist(s, (0, 0, 0)) + C * clock for s in spread]
x, _ = solve(spread[:4], exact[:4])
print(f"four exact pseudoranges: position {math.hypot(x[0], x[1]):.3f} m from the truth, clock {x[3] / C * 1000:.6f} ms")

rng = random.Random(4)
for name, sats in (("six satellites spread over the sky", spread),
                   ("six satellites in one part of the sky", [satellite(az, el) for az, el in ((100, 20), (115, 35), (130, 25), (145, 40), (125, 55), (110, 45))])):
    errors = []
    for _ in range(200):  # 200 fixes, each with new range errors of 3 m (standard deviation)
        p = [math.dist(s, (0, 0, 0)) + C * clock + rng.gauss(0, 3.0) for s in sats]
        x, rows = solve(sats, p, steps=4)
        errors.append(math.hypot(x[0], x[1]))
    pdop, hdop = dop(rows)
    errors.sort()
    print(f"{name}: PDOP {pdop:.1f}, HDOP {hdop:.1f}; horizontal error, 95 % of fixes within {errors[189]:.1f} m")

urban = [s for s in spread]  # the spread sky, with one low satellite's signal reflected off a wall
p = [math.dist(s, (0, 0, 0)) + C * clock for s in urban]
p[4] += 25.0  # multipath: the reflected path is 25 m longer
x, _ = solve(urban, p)
print(f"one reflected signal, 25 m too long: the fix moves {math.hypot(x[0], x[1]):.1f} m sideways and {x[2]:+.1f} m up")

a clock error of 1 ms is 299,792 m of range on every satellite
four exact pseudoranges: position 0.000 m from the truth, clock 1.000000 ms
six satellites spread over the sky: PDOP 2.3, HDOP 1.2; horizontal error, 95 % of fixes within 6.2 m
six satellites in one part of the sky: PDOP 15.4, HDOP 12.6; horizontal error, 95 % of fixes within 73.2 m
one reflected signal, 25 m too long: the fix moves 8.6 m sideways and +14.1 m up

"""The cleaned trips written as GPX 1.1, one track segment per trip, then read back and checked."""
import xml.etree.ElementTree as ET

from day import clock, logged
from steps import drop_poor, drop_spikes, find_stops, split_trips
from utm import from_utm

kept, _ = drop_spikes(drop_poor(logged()))
trips, _ = split_trips(kept, find_stops(kept))

NS = "http://www.topografix.com/GPX/1/1"
ET.register_namespace("", NS)
gpx = ET.Element(f"{{{NS}}}gpx", version="1.1", creator="MySmartCoPilot lesson")
trk = ET.SubElement(gpx, f"{{{NS}}}trk")
ET.SubElement(trk, f"{{{NS}}}name").text = "Field day, cleaned"
for trip in trips:
    seg = ET.SubElement(trk, f"{{{NS}}}trkseg")  # GPX: a new segment for each continuous span of the track
    for t, x, y, _ in trip:
        lon, lat = from_utm(x, y)
        pt = ET.SubElement(seg, f"{{{NS}}}trkpt", lat=f"{lat:.7f}", lon=f"{lon:.7f}")
        ET.SubElement(pt, f"{{{NS}}}time").text = f"2025-11-14T{clock(t)}Z"
ET.indent(gpx, "  ")
ET.ElementTree(gpx).write("field_day.gpx", encoding="utf-8", xml_declaration=True)

root = ET.parse("field_day.gpx").getroot()
q = lambda name: f"{{{NS}}}{name}"
segs = root.findall(f"{q('trk')}/{q('trkseg')}")
points = [p for s in segs for p in s.findall(q("trkpt"))]
lats, lons = [float(p.get("lat")) for p in points], [float(p.get("lon")) for p in points]
times = [[p.find(q("time")).text for p in s.findall(q("trkpt"))] for s in segs]
print("field_day.gpx, read back:")
print(f"  version {root.get('version')}, creator '{root.get('creator')}'")
print(f"  {len(segs)} segments, {len(points)} points; latitude {min(lats):.4f} to {max(lats):.4f}, longitude {min(lons):.4f} to {max(lons):.4f}")
print(f"  times in order within every segment: {all(t == sorted(t) for t in times)}")
with open("field_day.gpx", encoding="utf-8") as f:
    print("  " + "\n  ".join(f.read().splitlines()[:7]))

field_day.gpx, read back:
  version 1.1, creator 'MySmartCoPilot lesson'
  4 segments, 332 points; latitude 18.4871 to 18.5686, longitude 73.8017 to 73.8154
  times in order within every segment: True
  <?xml version='1.0' encoding='utf-8'?>
  <gpx xmlns="http://www.topografix.com/GPX/1/1" version="1.1" creator="MySmartCoPilot lesson">
    <trk>
      <name>Field day, cleaned</name>
      <trkseg>
        <trkpt lat="18.4871059" lon="73.8017422">
          <time>2025-11-14T08:00:00Z</time>

"""A synthetic field day as a phone logs it: one fix every 10 s, in metres of UTM zone 43N, with the receiver's
accuracy estimate, planted spikes, two stops with the drift of a receiver standing still, and a tunnel with no fixes.

Each fix is (seconds since 08:00 UTC, east, north, accuracy in m). The true path is known, so cleaning can be checked.
"""
import math
import random

START = (373_500.0, 2_044_500.0)
LEGS = [  # (what, seconds, speed in m/s, heading in degrees clockwise from north)
    ("walk to the first field", 1_200, 1.2, 90),
    ("stop at the field", 1_800, 0.0, 0),
    ("ride north", 900, 10.0, 0),
    ("stop at the market", 1_200, 0.0, 0),
    ("walk back", 1_500, 1.4, 200),
]
TUNNEL = (3_400, 3_640)  # seconds with no fix: an underpass on the ride north
SPIKES = {640: (420.0, -310.0), 2_210: (-650.0, 120.0), 3_250: (300.0, 840.0), 4_120: (-280.0, -520.0), 5_400: (510.0, 380.0)}
BUILDINGS = (4_600, 4_900)  # seconds near tall buildings: poor accuracy, larger errors


def truth():
    """[(t, east, north, leg)] every 10 s along the true path."""
    out, (x, y), t = [], START, 0
    for what, seconds, speed, heading in LEGS:
        for _ in range(seconds // 10):
            out.append((t, x, y, what))
            x += 10 * speed * math.sin(math.radians(heading))
            y += 10 * speed * math.cos(math.radians(heading))
            t += 10
    return out


def logged(seed=11):
    """The fixes the phone logs: truth plus noise, spikes and gaps, each with its accuracy estimate."""
    rng = random.Random(seed)
    out = []
    for t, x, y, _ in truth():
        if TUNNEL[0] <= t < TUNNEL[1]:
            continue
        poor = BUILDINGS[0] <= t < BUILDINGS[1]
        sigma, accuracy = (9.0, round(rng.uniform(18, 30), 1)) if poor else (2.5, round(rng.uniform(3.5, 6.0), 1))
        dx, dy = SPIKES.get(t, (0.0, 0.0))
        out.append((t, x + dx + rng.gauss(0, sigma), y + dy + rng.gauss(0, sigma), accuracy))
    return out


def clock(t):
    """Seconds since 08:00 as a time of day."""
    return f"{8 + t // 3600:02d}:{t // 60 % 60:02d}:{t % 60:02d}"

"""The four cleaning steps of a GPS track, each a small function over fixes (t, east, north, accuracy)."""
import math


def length(points):
    """The length of a track in metres, fix to fix."""
    return sum(math.dist(a[1:3], b[1:3]) for a, b in zip(points, points[1:]))


def drop_poor(fixes, worst=20.0):
    """Keep the fixes whose accuracy estimate is `worst` metres or better."""
    return [f for f in fixes if f[3] <= worst]


def drop_spikes(fixes, v_max=30.0):
    """Keep a fix only if reaching it from the last kept fix needs no more than v_max m/s; return (kept, dropped)."""
    kept, dropped = [], []
    for f in fixes:
        if kept and math.dist(f[1:3], kept[-1][1:3]) / (f[0] - kept[-1][0]) > v_max:
            dropped.append(f)
        else:
            kept.append(f)
    return kept, dropped


def find_stops(points, diameter=60.0, minimum=300):
    """(first, last) index of every run that stays within `diameter` m of its first fix for `minimum` s or more."""
    out, i = [], 0
    while i < len(points):
        j = i
        while j + 1 < len(points) and math.dist(points[j + 1][1:3], points[i][1:3]) <= diameter / 2:
            j += 1
        if points[j][0] - points[i][0] >= minimum:
            out.append((i, j))
            i = j + 1
        else:
            i += 1
    return out


def split_trips(points, stops, gap=120):
    """Trips between stops, also split where no fix came for more than `gap` s; returns (trips, gap lengths in m)."""
    stopped = {k: (i, j) for i, j in stops for k in range(i, j + 1)}
    trips, current, gaps = [], [], []
    for k, f in enumerate(points):
        if k in stopped:  # a trip ends at a stop's first fix, and the next one starts at its last
            i, j = stopped[k]
            if k == i and current:
                trips.append(current + [f])
                current = []
            if k == j:
                current = [f]
            continue
        if current and f[0] - current[-1][0] > gap:
            trips.append(current)
            gaps.append(math.dist(current[-1][1:3], f[1:3]))
            current = []
        current.append(f)
    if len(current) > 1:
        trips.append(current)
    return trips, gaps

"""UTM zone 43N on WGS 84, both ways, and the size of a degree on the ground: what the lesson's programs reproject with.

The transverse Mercator projection is computed with Krüger's series to the sixth order of the ellipsoid's third
flattening, as Karney (2011) gives it, which is good to well under a millimetre across the zone.
"""
import math

A, F = 6_378_137.0, 1 / 298.257223563  # WGS 84 semi-major axis (m) and flattening
K0, LON0, FALSE_EASTING = 0.9996, math.radians(75.0), 500_000.0  # zone 43N: central meridian 75° E
E = math.sqrt(F * (2 - F))  # eccentricity
N = F / (2 - F)  # third flattening
RECT = A / (1 + N) * (1 + N**2 / 4 + N**4 / 64 + N**6 / 256)  # the rectifying radius
ALPHA = [
    N / 2 - 2 * N**2 / 3 + 5 * N**3 / 16 + 41 * N**4 / 180 - 127 * N**5 / 288 + 7891 * N**6 / 37800,
    13 * N**2 / 48 - 3 * N**3 / 5 + 557 * N**4 / 1440 + 281 * N**5 / 630 - 1983433 * N**6 / 1935360,
    61 * N**3 / 240 - 103 * N**4 / 140 + 15061 * N**5 / 26880 + 167603 * N**6 / 181440,
    49561 * N**4 / 161280 - 179 * N**5 / 168 + 6601661 * N**6 / 7257600,
    34729 * N**5 / 80640 - 3418889 * N**6 / 1995840,
    212378941 * N**6 / 319334400,
]
BETA = [
    N / 2 - 2 * N**2 / 3 + 37 * N**3 / 96 - N**4 / 360 - 81 * N**5 / 512 + 96199 * N**6 / 604800,
    N**2 / 48 + N**3 / 15 - 437 * N**4 / 1440 + 46 * N**5 / 105 - 1118711 * N**6 / 3870720,
    17 * N**3 / 480 - 37 * N**4 / 840 - 209 * N**5 / 4480 + 5569 * N**6 / 90720,
    4397 * N**4 / 161280 - 11 * N**5 / 504 - 830251 * N**6 / 7257600,
    4583 * N**5 / 161280 - 108847 * N**6 / 3991680,
    20648693 * N**6 / 638668800,
]


def to_utm(lon, lat):
    """Easting and northing (m) in UTM zone 43N of a longitude and latitude in degrees."""
    phi, lam = math.radians(lat), math.radians(lon) - LON0
    t = math.sinh(math.atanh(math.sin(phi)) - E * math.atanh(E * math.sin(phi)))
    xi0, eta0 = math.atan2(t, math.cos(lam)), math.atanh(math.sin(lam) / math.sqrt(1 + t * t))
    xi = xi0 + sum(a * math.sin(2 * j * xi0) * math.cosh(2 * j * eta0) for j, a in enumerate(ALPHA, 1))
    eta = eta0 + sum(a * math.cos(2 * j * xi0) * math.sinh(2 * j * eta0) for j, a in enumerate(ALPHA, 1))
    return FALSE_EASTING + K0 * RECT * eta, K0 * RECT * xi


def from_utm(x, y):
    """Longitude and latitude in degrees of an easting and northing in UTM zone 43N."""
    xi, eta = y / (K0 * RECT), (x - FALSE_EASTING) / (K0 * RECT)
    xi0 = xi - sum(b * math.sin(2 * j * xi) * math.cosh(2 * j * eta) for j, b in enumerate(BETA, 1))
    eta0 = eta - sum(b * math.cos(2 * j * xi) * math.sinh(2 * j * eta) for j, b in enumerate(BETA, 1))
    tau0 = math.sin(xi0) / math.sqrt(math.sinh(eta0) ** 2 + math.cos(xi0) ** 2)
    tau = tau0
    for _ in range(5):  # Newton's method for the conformal latitude
        s = math.sinh(E * math.atanh(E * tau / math.sqrt(1 + tau * tau)))
        guess = tau * math.sqrt(1 + s * s) - s * math.sqrt(1 + tau * tau)
        tau += (tau0 - guess) / math.sqrt(1 + guess * guess) * (1 + (1 - E * E) * tau * tau) / ((1 - E * E) * math.sqrt(1 + tau * tau))
    return math.degrees(LON0 + math.atan2(math.sinh(eta0), math.cos(xi0))), math.degrees(math.atan(tau))


def metres_per_degree(lat):
    """(east-west, north-south) length on the ground of one degree at a latitude, on the ellipsoid."""
    phi = math.radians(lat)
    w = 1 - E * E * math.sin(phi) ** 2
    across = A * math.cos(phi) / math.sqrt(w)  # radius of the parallel
    along = A * (1 - E * E) / w**1.5  # radius of curvature of the meridian
    return math.radians(1) * across, math.radians(1) * along

"""Cleaning the field day: drop the fixes the receiver marks as poor, drop spikes by speed, find the stops, and split
the rest into trips at stops and gaps, checked against the true path."""
from day import SPIKES, clock, logged, truth
from steps import drop_poor, drop_spikes, find_stops, length, split_trips

fixes = logged()
print(f"{len(fixes):,} fixes logged, every 10 s, from {clock(fixes[0][0])} to {clock(fixes[-1][0])} UTC")
print(f"true path: {length(truth()) / 1000:.2f} km; the raw fixes add up to {length(fixes) / 1000:.2f} km")

good = drop_poor(fixes)
print(f"accuracy 20 m or better: {len(good):,} fixes kept, {len(fixes) - len(good)} dropped")
kept, spikes = drop_spikes(good)
print(f"faster than 30 m/s from the last kept fix: {len(spikes)} spikes dropped, at "
      f"{', '.join(clock(f[0]) for f in spikes)}; {len(SPIKES)} were planted")

stops = find_stops(kept)
for i, j in stops:
    print(f"stop: {clock(kept[i][0])} to {clock(kept[j][0])}, {j - i + 1} fixes, drifting {length(kept[i : j + 1]):,.0f} m while standing still")
trips, gaps = split_trips(kept, stops)
for t in trips:
    print(f"trip: {clock(t[0][0])} to {clock(t[-1][0])}, {length(t) / 1000:.2f} km, {len(t)} fixes")
moving = sum(map(length, trips))
print(f"trips: {moving / 1000:.2f} km split at the gap; {(moving + sum(gaps)) / 1000:.2f} km with the gap bridged "
      f"by a straight line of {sum(gaps):,.0f} m")

636 fixes logged, every 10 s, from 08:00:00 to 09:49:50 UTC
true path: 12.53 km; the raw fixes add up to 20.47 km
accuracy 20 m or better: 608 fixes kept, 28 dropped
faster than 30 m/s from the last kept fix: 5 spikes dropped, at 08:10:40, 08:36:50, 08:54:10, 09:08:40, 09:30:00; 5 were planted
stop: 08:19:50 to 08:50:00, 181 fixes, drifting 774 m while standing still
stop: 09:05:00 to 09:25:20, 94 fixes, drifting 388 m while standing still
trip: 08:00:00 to 08:19:50, 1.48 km, 119 fixes
trip: 08:50:00 to 08:56:30, 3.90 km, 39 fixes
trip: 09:00:40 to 09:05:00, 2.60 km, 27 fixes
trip: 09:25:20 to 09:49:50, 2.14 km, 147 fixes
trips: 10.12 km split at the gap; 12.63 km with the gap bridged by a straight line of 2,507 m