feat(urbannav): run the filter on Tunnel-1 with the receiver's own fixes and outage (T2-0b)

[Telschow]

Closes #109
Closes #95

By the maintainer's instruction this closes #95. The README credit it asks for is in. The Deep-Urban-1 rows and the INSPVAX decision it also lists are not done here, so they need a follow-up issue.

All of this is one sequence (Tunnel-1) and one receiver (u-blox F9P). Not field performance. Branch from origin/main at 5f5abef, in a fresh clone.

Commits

1. 9c14892 the stranded commit (a23512c), cherry-picked with its author.
2. feb8c0f the CSV and the generated page tables regenerated on current main.
3. 7599935 oracle diagnostics, tilt and NIS rows, the reader's report, and the page test added to the CI documentation step.
4. ecc52c8 the page findings, the README credit, ADR-0026 context and options.
5. ad4cbf8 a synthetic Tunnel-1 sequence written by the tests, so CI runs the code that otherwise needs the dataset (see below), and a fix it found.

Conflict resolutions (step 1)

git cherry-pick --no-commit a23512c conflicted only in the generated count markers: CONSTRAINTS.md (two markers), CONTRIBUTING.md, ROADMAP.md and docs/data/metrics.json. I took main's version of all four (git checkout --ours) and did not edit them by hand. metrics.py --write regenerated them at the end (1565 tests collected). ADR-0026 merged without a conflict. Its decision section is the blank placeholder on main and is unchanged ("To be written by the maintainer.").

Step 2: the committed CSV did not reproduce

navkit urbannav run on main did not give the committed docs/data/urbannav_run.csv. First differing row: line 2, tunnel1,declared,0,hor_rmse_m, committed 4878, reproduced 4944; 160 of 250 data rows differ.

Cause: the commit was written before T1-1 (b8cb8e8, the sign of the attitude reset Jacobian). Evidence, declared seed 0:

Tree	RMSE	Mean NEES	Median error before	Error at outage end	Attitude error at start
a23512c as committed	4878	26.9	7.28	3675	4.111
a23512c plus only the T1-1 source diff	4944	27.33	7.265	3726	6.369
current main	4944	27.33	7.265	3726	6.369

The run reads three named files (tunnel1/UrbanNav_tunnel_GT_raw.txt, xsense_imu_tunnel_1.csv, 20210518.tunnel.cht.ublox.f9p.nmea). There is no glob, so it does not depend on what else is on disk. The CSV and the page tables were regenerated, and the run was done twice with byte-identical output (SHA-256 25c8cee4930c54d11e608254528e75a3ab37c598cddf7352de99be1915c88168 for both).

Investigation (steps 3 and the follow-up), seed 0, current main

Scratch scripts, not in the repository, are at the end of this description. Everything labelled "scratch" is not blind, one sequence, not a fix.

Sanity checks (a to g)

	Check	Result
a	Start state	Position, velocity and attitude come from the reference at the first IMU sample, perturbed by the filter's start sigmas per seed. Horizontal error at 10 s: 1.9 m. Yaw error at 10 s: 5.1 degrees on seed 0.
b	Attitude convention	Reference velocity rotated by the matrices matches d(position)/dt (median 0.055 m/s over 309 moving epochs). Body y (forward) is 1.3 degrees from the velocity heading (95th percentile 2.2); body x is 88.7. The filter takes rotation matrices, so quaternion order does not enter this path. ENU, not NED.
c	IMU frame and gravity	Specific force rotated to ENU over the first 2 s: (0.036, -0.022, 9.786) m/s²; the filter's gravity is (0, 0, -9.80665). Gyro z in ENU against -d(heading)/dt: correlation 0.966.
d	Clock offset	Shifting the GNSS by +0.26 s and -0.26 s changes the end error by about 1% (table below) and the pre-outage NEES from 10.20 to 9.81 and 10.95. Not the cause. A cross-correlation of yaw rates peaks at +0.26 s (0.978 against 0.966 at zero) but the reference is 1 Hz, so that peak is not resolved.
e	NEES before the outage	Horizontal position (2 dof, expected 2): 10.20 declared, 20.32 per-fix. Attitude (3 dof, expected 3, scratch): 32.57 and 22.17. Raw fixes against the reference, mean (error / receiver sigma)² (expected 2): 3.87. The filter is inconsistent with GNSS available.
f	Error against the IMU noise	Declared white noise alone gives a 1-sigma of 322 m at the end of the 151 s outage through gyro noise and gravity (0.84 m through the accelerometer). Observed 3726 m, 11.6 times that. Accelerometer-only with the reference attitude: 427 m. Claimed sigma at the end: about 530 m.
g	After the outage	First return NIS 51.0, the 0.1% level (3 dof) is 16.27, so 3.1 times over. 111 of 111 fixes after the outage are over the level, from 288.5 s to 398.5 s, smallest NIS 51. Before the outage 7 of 137 fixes are over (declared) and 0 of 137 (per-fix).

Attitude error and what explains the position error (declared; Euler angles in the dataset's convention; estimate minus reference)

Time (s)	Roll	Pitch	Yaw	Horizontal error (m)
10	-1.00	0.64	5.11	1.9
60	-0.53	-0.59	4.54	7.9
100	-0.68	-0.73	5.27	8.8
137.5 (outage start)	-0.16	-0.16	6.37	16.0
175.2 (25%)	-1.27	-0.87	5.99	46.2
213.0 (50%)	-2.51	-1.88	4.36	506.7
250.7 (75%)	-3.19	-1.81	4.10	1673.5
288.5 (end)	-3.78	0.20	3.61	3725.8

Position error at the end predicted from the attitude error (scratch): tilt term (g times the tilt error, integrated twice) 3885 m; yaw term (speed times yaw error, integrated) 147 m; start error plus start velocity error 118 m; sum 3660 m against 3726 m actual; the tilt term alone points the same way as the error (cosine 0.998). Per-fix gives the same picture (tilt 3706 m, yaw 91 m, actual 3574 m). For scale: a constant 1 degree tilt over 151 s gives 1951 m.

Counterfactuals (now in the code as oracle variants, generated on the page)

Run	Error at outage end	Fixes rejected
declared	3726 m	118
ORACLE DIAGNOSTIC, reference roll and pitch in the outage	160 m	16
ORACLE DIAGNOSTIC, reference yaw in the outage	3738 m	118
ORACLE DIAGNOSTIC, reference attitude in the outage	149 m	16

Sensitivity runs (scratch). Not blind, one sequence, not a fix. Not on the page.

Run	NEES before	Error at the outage start	Error at the outage end
declared	10.20	16.0 m	3726 m
GNSS sigma x2	11.07	43.4 m	2816 m
GNSS sigma x4	8.83	29.8 m	3779 m
IMU white-noise densities x3 only	23.44	79.7 m	52.8 m
all four IMU noise terms x3	23.54	80.1 m	52.8 m
bias random walks x10 only	27.34	76.0 m	9730 m
bias random walks x100 only	22.71	90.4 m	27430 m
gyro bias inferred from the data as the start bias	12.21	48.4 m	2858 m
GNSS shifted +0.26 s	9.81	18.6 m	3693 m
GNSS shifted -0.26 s	10.95	13.4 m	3760 m

The x3 white-noise result makes the pre-outage state worse while it shrinks the outage error. It is tuning on one sequence, not a finding. Lever arm: the mean fix residual in the body frame (38 moving fixes within 5 m of the reference) is 1.1 m forward and 0.2 m right, consistent with the antenna offset the page names (not corrected). It is too small to explain a median error of several metres or a NEES of 10. The gyro bias the filter holds at the outage start is within about 5e-5 rad/s of its start guess; the bias inferred from the 1 Hz reference differs on one axis by about 4.5e-4 rad/s, and starting from the inferred biases still ended in a kilometre-scale error. So the tilt growth is not explained. The page says so and marks the weak-observability reason HYPOTHESIS.

Steps 4 and 5

Variants. DECLARED in the code lists the settings with the first variant fixed before the first run and variant: per_fix marked "added AFTER the first run ... Not blind". In run_once the two differ only by gnss_per_fix_covariance. Both are on the page.

Interface after #129. Of the 250 valid fixes none has a NaN, negative, zero or below-floor variance. The receiver sigma ranges from 0.70 to 31 m; every fix has a matching GST sentence, so the scalar fallback is never used. The floor (0.01 m) therefore changed nothing, and no fix would raise. The reader does not pass on 151 receiver epochs (quality 0 or no position). The run now drops a fix with an unusable variance before the filter sees it, reports the count, and reports the epochs not passed on and the fixes below the floor. All three are generated rows (151, 0, 0 here) with tests that they match the data (skipped when the data is absent).

What changed on the page because of the checks

• The old "fits the attitude as the driver" is replaced by the oracle rows: tilt error inside the outage drives the outage error, yaw does not.
• New first statement: with GNSS available the filter is already inconsistent on this sequence. A link between that and the tilt growth is not shown.
• The lost channel is stated as a consequence of the position error, with the first-return NIS and the 111 of 111 as generated rows.
• The cause of the tilt growth: not shown, with the weak-observability sentence marked HYPOTHESIS, not tested.
• "The two clocks agree" became "No clock offset was applied", with the limits of the search.
• Every oracle row is labelled ORACLE DIAGNOSTIC in the code, the CSV variant name, the declared table and the page. The sensitivity runs are not on the page.
• The pitch error is small at both ends of the outage, so the page says the rows do not show pitch growing.
• README: an acknowledgement with the citation the authors ask for, the dataset link, the MIT licence statement, and "fetched, not shipped".
• ADR-0026: context (what the dataset gives, what it cannot show, what a decision commits the repository to) and options with their commitments. The decision section is untouched.

Tests (written first)

Euler inverse round trip; each oracle mode overwrites only the named attitude only inside the outage and the patch is removed on exit and on error; unknown mode refused; variance report and the drop mask; epochs not passed on; page labels (ORACLE DIAGNOSTIC, HYPOTHESIS, the missing link); claim tests on the CSV (tilt grows, reference tilt removes most of the error and reference yaw does not, oracles equal the declared filter before the outage, every fix after the outage is over the gate, the CSV holds aggregates only). Data-dependent tests skip without the dataset: counts match the data, the oracle is deterministic, the committed numbers reproduce. tests/test_urbannav_run.py is added to the CI documentation step because the guard test required it.

Coverage: why CI failed the first time, and what changed

The first CI run failed only at the recorded-coverage step (coverage is 92.32%, metrics.json records 94.31%). The new run code (build, run_once, main, the oracle wrapper) ran only with the dataset, which CI does not have. The ratchet itself passed. I did not re-record the figure or widen the allowed gap. Instead tests/test_urbannav_synthetic.py writes a small Tunnel-1-shaped sequence from code (made-up numbers on a made-up spot, a straight drive, a 1 Hz reference, a 20 Hz IMU, an NMEA file with RMC, GGA and GST sentences and a 40 s gap as the outage, a gyroscope bias that drifts the roll from just before the outage). Nothing in it is a row of the real dataset and no data file is committed. The tests do not skip without the dataset, and the whole file takes about 8 s.

What the new tests assert (not just execute): the outage has no offered fix and the reader's not-valid count equals the epochs written without a fix; all five variants run and give the documented quantities, CSV columns and labels, and the oracle rows are labelled ORACLE DIAGNOSTIC; on the known roll drift the tilt oracle and the attitude oracle end the outage under half the declared filter's error while the yaw oracle does not change it, and the tilt oracle removes the roll error; a fix with a NaN or infinite variance is dropped before the filter, counted, and no ValueError reaches the caller (a negative variance is covered by the mask test, since a GST sigma is squared); a sigma below the floor is counted and kept; main writes the documented CSV and two runs give the same SHA-256; main --from-csv --page fills the blocks; a missing path and a missing NMEA file give the documented error: message and no traceback; too few finite sigmas, and an oracle without an outage, are refused. Mutation check: disabling the oracle's overwrite fails 5 tests, and the NaN-sigma test failed first against the old code.

The fixture found a real defect: one NaN or infinite GST sigma made the declared horizontal sigma NaN, because the average was taken before the unusable fixes were dropped. The average now uses finite values only. The real sequence has no such sigma, so its numbers and CSV are unchanged.

Measured locally in a scratch venv outside the repository (coverage 7.16.2, PYTHONPATH=src, dataset hidden, the UrbanNav, EuRoC and recorded-orientation test files):

File	Statements	Missed	Cover	Missing lines
src/navkit/urbannav_run.py	290	0	100%	none
src/navkit/urbannav_data.py	294	22	93%	96, 132-133, 140, 153, 215-216, 226-227, 229, 279, 285-286, 304, 316-317, 348, 382, 437-438, 441, 455
src/navkit/euroc_eval.py	433	63	85%	147, 181-182, 185, 252, 254, 289-326, 452, 526-528, 533-549, 565-566, 582, 584, 778-781, 784, 840-841, 867-869, 881

The missing lines are error branches and the real-receiver cases, which I did not chase. Against origin/main with the same files (hidden data): euroc_eval.py 420 statements and 86 missed, urbannav_data.py 243 and 19. So this branch adds 354 statements and 374 hit lines.

Expected total. docs/data/metrics.json on main records 10033 of 10638 lines. coverage.py counts lines differently from scripts/coverage_report.py, so I calibrated: for the tree CI measured at 92.32%, the same estimate gives 93.05%, an offset of 0.73 points. Now: (10033 + 374) / (10638 + 354) = 94.68% by the raw estimate, about 93.95% after the offset. The recorded figure is 94.31% and the allowed gap is 0.5, so I expect the check to pass with about 0.36 points to spare, which is thin. If CI still disagrees I will report the measured gap and the missing lines, and not widen the tolerance or re-record.

Checks

ruff check, ruff format --check, mypy src, check_doc_tables.py, metrics.py --check, check_links.py pass (1586 tests collected). Full suite:

• with the dataset present (before the synthetic tests were added): 1563 passed, 2 xfailed in 666.20s (0:11:06) (the dataset read through a read-only symlink to the old checkout's data/raw, not committed)
• with the dataset hidden, on the final tree: 1555 passed, 29 skipped, 2 xfailed in 594.34s (0:09:54). The real-data tests skip with a reason, nothing fails, and the synthetic tests run

The coverage ratchet is left to CI. Nothing under src/navkit/estimators changed. No dataset file is committed.

Scratch scripts for the investigation

investigate.py (modes base, cf, shift, sens)

"""Investigation of the Tunnel-1 failure. Scratch script, not part of the repository.

usage: investigate.py MODE   with MODE in base, cf, shift, sens, interface
"""

import math
import sys

import numpy as np

import navkit.euroc_eval as ee
from navkit import urbannav_data as ud
from navkit import urbannav_run as ur
from navkit.estimators.eskf import ErrorStateKalmanFilter
from navkit.euroc_eval import RunOptions, run_sequence
from navkit.fdir.gating import chi2_threshold
from navkit.geometry.rigid import rot_exp, rot_log
from navkit.types import GRAVITY, GnssFix, interpolate_trajectory, rot_log_batch

ROOT = "/Users/daniel/Development/contested-nav-fresh/data/raw/urbannav"
G = float(np.linalg.norm(GRAVITY))
seq, gnss, spans, info = ur.build(ROOT)
t0 = float(seq.imu.t[0])
A, B = spans[0][0] - t0, spans[0][1] - t0
T = B - A
THR = chi2_threshold(3, ur.GATE_ALPHA)
DEG = 180.0 / math.pi


def rpy_from_R(R):
    """Inverse of urbannav_data.rpy_to_rotation (roll about forward, pitch nose up, heading clockwise from north)."""
    p = np.arcsin(np.clip(R[..., 2, 1], -1, 1))
    r = np.arctan2(-R[..., 2, 0], R[..., 2, 2])
    h = np.arctan2(R[..., 0, 1], R[..., 1, 1])
    return r, p, h


def R_from_rpy(r, p, h):
    return ud.rpy_to_rotation(np.degrees(np.atleast_1d(r)), np.degrees(np.atleast_1d(p)), np.degrees(np.atleast_1d(h)))


def wrap(x):
    return (x + math.pi) % (2 * math.pi) - math.pi


# round-trip check of the Euler inverse
_r, _p, _h = np.array([0.1, -0.2]), np.array([0.05, 0.3]), np.array([1.0, 3.0])
_rr, _pp, _hh = rpy_from_R(R_from_rpy(_r, _p, _h))
assert np.allclose([_rr, _pp, wrap(_hh)], [_r, _p, wrap(_h)], atol=1e-9), "euler inverse is wrong"

REF_AT_IMU = interpolate_trajectory(seq.truth, seq.imu.t)
REF_RPY = rpy_from_R(REF_AT_IMU.rotations)
TIMELINE = seq.imu.t - t0


def run(variant="declared", seed=0, shift=0.0, sigma_scale=1.0, noise_scale=1.0, cf=None, capture=False, bias_walk_scale=1.0):
    g = GnssFix(t=gnss.t + shift, positions=gnss.positions, cov=gnss.cov, name="f9p")
    opts = RunOptions(
        gnss_sigma_m=info["sigma_h"] * sigma_scale,
        gnss_vertical_sigma_m=ur.GNSS_VERTICAL_SIGMA_M,
        outages=tuple((s - t0, e - s) for s, e in spans),
        seed=seed,
        init_bias="static",
        noise_scale=noise_scale,
        bias_walk_scale=bias_walk_scale,
        gnss_per_fix_covariance=variant == "per_fix",
    )
    inn, det, ev, bias = [], {}, [], []
    orig = ErrorStateKalmanFilter._propagate
    state = {"t": 0.0}
    att_samples = []

    def patched(self, x, accel, gyro, dt):
        orig(self, x, accel, gyro, dt)
        state["t"] += dt
        t = state["t"]
        k = min(int(np.searchsorted(TIMELINE, t)), len(TIMELINE) - 1)
        if capture and k % 400 == 0:
            att_samples.append((t, x["P"][0:3, 0:3].copy()))
        if cf is not None and A <= t < B:
            r_e, p_e, h_e = rpy_from_R(x["R"])
            r_r, p_r, h_r = REF_RPY[0][k], REF_RPY[1][k], REF_RPY[2][k]
            if cf == "tilt_ref":  # reference roll and pitch, the filter's own yaw
                new = R_from_rpy(r_r, p_r, h_e)[0]
            elif cf == "yaw_ref":  # reference yaw, the filter's own roll and pitch
                new = R_from_rpy(r_e, p_e, h_r)[0]
            elif cf == "all_ref":
                new = R_from_rpy(r_r, p_r, h_r)[0]
            else:
                raise ValueError(cf)
            x["R"] = new

    ErrorStateKalmanFilter._propagate = patched
    try:
        rec = run_sequence(seq, opts, innovations_out=inn, gnss_override=g, detail_out=det, events_out=ev, bias_out=bias)
    finally:
        ErrorStateKalmanFilter._propagate = orig
    est, ref, cov = det["est"], det["reference"], det["position_cov"]
    t = est.t - t0
    err = est.positions - ref.positions
    eh = np.linalg.norm(err[:, :2], axis=1)
    ph = cov[:, :2, :2]
    nees = np.einsum("ni,ni->n", err[:, :2], np.linalg.solve(ph, err[:, :2][..., None])[..., 0])
    return dict(rec=rec, inn=inn, ev=ev, bias=bias, est=est, ref=ref, cov=cov, t=t, err=err, eh=eh, nees=nees, att=att_samples)


def summary(r, label):
    t, eh, nees = r["t"], r["eh"], r["nees"]
    ib = min(int(np.searchsorted(t, B)), len(t) - 1)
    ia = max(int(np.searchsorted(t, A)) - 1, 0)
    print(
        f"{label:<40} NEES before {np.mean(nees[t < A]):7.2f} | err median before {np.median(eh[t < A]):6.2f} m"
        f" | at start {eh[ia]:7.1f} m | at end {eh[ib]:8.1f} m | rejected {r['rec']['stats'].get('gnss_updates_rejected')}"
    )


def attitude_table(r, label):
    est, ref, t = r["est"], r["ref"], r["t"]
    re_, pe_, he_ = rpy_from_R(est.rotations)
    rr_, pr_, hr_ = rpy_from_R(ref.rotations)
    d_roll, d_pitch, d_yaw = (re_ - rr_) * DEG, (pe_ - pr_) * DEG, wrap(he_ - hr_) * DEG
    nav = rot_log_batch(np.einsum("nij,nkj->nik", est.rotations, ref.rotations))  # nav-frame rotation vector of R_est R_ref^T
    print(f"\n--- attitude error, {label} (estimate minus reference; Euler angles in the dataset's convention: roll about forward axis, pitch nose up, heading clockwise)")
    print("   time (s)   roll (deg)  pitch (deg)  yaw (deg) | nav-frame tilt about E, N (deg) | horizontal position error (m)")
    marks = [10, 30, 60, 100, A - 1.0] + [A + f * T for f in (0.0, 0.25, 0.5, 0.75)] + [B - 0.01]
    for m in marks:
        i = min(int(np.searchsorted(t, m)), len(t) - 1)
        print(f"  {t[i]:8.1f}  {d_roll[i]:10.3f}  {d_pitch[i]:11.3f}  {d_yaw[i]:9.3f} | {nav[i,0]*DEG:8.3f} {nav[i,1]*DEG:8.3f}            | {r['eh'][i]:9.1f}")
    # prediction through the outage
    ia = max(int(np.searchsorted(t, A)) - 1, 0)
    sel = np.flatnonzero((t >= A) & (t <= B))
    ts = t[sel]
    dt = np.diff(ts, prepend=ts[0])
    a_tilt = np.column_stack([G * nav[sel, 1], -G * nav[sel, 0]])  # delta x z_hat * g, horizontal part
    v_tilt = np.cumsum(a_tilt * dt[:, None], axis=0)
    p_tilt = np.cumsum(v_tilt * dt[:, None], axis=0)
    vel = np.column_stack([np.interp(ts + t0, seq.truth.t, seq.velocity[:, k]) for k in range(2)])
    yaw_v = np.column_stack([-vel[:, 1], vel[:, 0]]) * nav[sel, 2][:, None]  # (z_hat x v) * yaw error, nav frame
    p_yaw = np.cumsum(yaw_v * dt[:, None], axis=0)
    # initial velocity error from the last 2 s before the outage
    j = np.flatnonzero((t >= A - 2.0) & (t <= A))
    v_err0 = (r["err"][j[-1], :2] - r["err"][j[0], :2]) / (t[j[-1]] - t[j[0]])
    e0 = r["err"][ia, :2]
    lin = e0 + v_err0 * (ts - A)[:, None]
    pred = lin + p_tilt + p_yaw
    act = r["err"][sel, :2]
    print(f"   velocity error at the outage start (from the position error over the last 2 s): {np.linalg.norm(v_err0):.3f} m/s")
    print("   fraction of the outage:       25%      50%      75%     100%     (horizontal vectors in the navigation frame, metres)")
    for name, arr in (("actual error", act), ("start error + velocity error", lin), ("tilt term (g x tilt, twice integrated)", p_tilt), ("yaw term (speed x yaw error integrated)", p_yaw), ("sum of the three", pred)):
        vals = []
        for f in (0.25, 0.5, 0.75, 1.0):
            k = min(int(round(f * (len(ts) - 1))), len(ts) - 1)
            vals.append(np.linalg.norm(arr[k]))
        print(f"   |{name:<42}| " + "  ".join(f"{v:8.1f}" for v in vals))
    res = np.linalg.norm(act[-1] - pred[-1])
    print(f"   residual |actual - sum| at the end: {res:.1f} m; tilt term alone vs actual cosine {np.dot(p_tilt[-1], act[-1] - lin[-1]) / (np.linalg.norm(p_tilt[-1]) * np.linalg.norm(act[-1] - lin[-1]) + 1e-12):.3f}")
    print(f"   mean |tilt| through the outage {np.mean(np.linalg.norm(nav[sel,:2],axis=1))*DEG:.3f} deg, mean |yaw| {np.mean(np.abs(nav[sel,2]))*DEG:.3f} deg")


mode = sys.argv[1]
print(f"outage {A:.1f} to {B:.1f} s ({T:.1f} s); a constant 1 degree tilt error over the outage gives 0.5*g*theta*T^2 = {0.5 * G * math.radians(1.0) * T**2:.0f} m")
print(f"reference horizontal path length in the outage {np.sum(np.linalg.norm(np.diff(seq.truth.positions[:, :2], axis=0), axis=1)[(seq.truth.t[1:] >= A + t0) & (seq.truth.t[1:] <= B + t0)]):.0f} m")

if mode == "base":
    for variant in ("declared", "per_fix"):
        r = run(variant, capture=True)
        summary(r, f"{variant} seed 0")
        attitude_table(r, variant)
        # biases
        bl = r["bias"]
        tb = np.array([b[0] for b in bl]) - (t0 if bl and bl[0][0] > 1e6 else 0.0)
        ia = int(np.searchsorted(tb, A)) - 1
        print(f"   filter bias estimate at the last fix before the outage (t={tb[ia]:.1f} s): gyro {np.round(bl[ia][1], 5).tolist()} rad/s, accel {np.round(bl[ia][2], 4).tolist()} m/s^2")
        print(f"   filter bias estimate at the first fix (t={tb[0]:.1f} s): gyro {np.round(bl[0][1], 5).tolist()}, accel {np.round(bl[0][2], 4).tolist()}")
        # inferred biases from the data over [60, A]
        imu = seq.imu
        ref = REF_AT_IMU
        for lo in (40.0, 80.0):
            m = (TIMELINE >= lo) & (TIMELINE < A)
            Rm = ref.rotations[m]
            dtm = np.diff(TIMELINE[m])
            w_ref = rot_log_batch(np.einsum("nji,njk->nik", Rm[:-1], Rm[1:])) / dtm[:, None]
            gb = imu.gyro[m][:-1].mean(axis=0) - w_ref.mean(axis=0)
            vel_i = np.column_stack([np.interp(TIMELINE[m] + t0, seq.truth.t, seq.velocity[:, k]) for k in range(3)])
            # specific force expected from the reference: R^T (a + g z). a from the velocity change over the whole window, as a mean
            a_mean = (vel_i[-1] - vel_i[0]) / (TIMELINE[m][-1] - TIMELINE[m][0])
            f_exp = np.einsum("nji,j->ni", Rm, a_mean + np.array([0, 0, G]))
            ab = imu.accel[m].mean(axis=0) - f_exp.mean(axis=0)
            print(f"   bias inferred from the data over {lo:.0f}-{A:.0f} s (gyro minus reference rate; accelerometer minus reference-implied): gyro {np.round(gb, 5).tolist()} rad/s, accel {np.round(ab, 3).tolist()} m/s^2")
        # NEES: attitude and position split, before the outage
        t = r["t"]
        att = r["att"]
        vals = []
        for (ts, P3) in att:
            if ts < A:
                i = min(int(np.searchsorted(t, ts)), len(t) - 1)
                dth = rot_log(r["est"].rotations[i].T @ r["ref"].rotations[i])
                vals.append(float(dth @ np.linalg.solve(P3, dth)))
        print(f"   NEES before the outage: horizontal position (2 dof, expected 2) {np.mean(r['nees'][t < A]):.2f}; attitude (3 dof, expected 3, body-frame right perturbation) {np.mean(vals):.2f} (mean of {len(vals)} 1 s samples)")
        # (g) NIS after the outage
        ti = np.array([x[0] for x in r["inn"]])
        ti = ti - t0 if ti.size and ti.min() > 1e6 else ti
        nis = np.array([float(rr @ np.linalg.solve(s, rr)) for _, rr, s in r["inn"]])
        aft = np.flatnonzero(ti >= B)
        print(f"   [g] chi-square 0.1% level for 3 dof: {THR:.2f}. Fixes offered after the outage: {aft.size}")
        for k in aft[:6]:
            print(f"       fix at {ti[k]:.1f} s: NIS {nis[k]:10.1f}  ({nis[k] / THR:8.1f} x threshold)")
        over = aft[nis[aft] > THR]
        print(f"       over the threshold after the outage: {over.size} of {aft.size}; first at {ti[over[0]]:.1f} s, last at {ti[over[-1]]:.1f} s; smallest NIS after the outage {nis[aft].min():.1f}")
        last_before = np.flatnonzero(ti < A)
        print(f"       fixes before the outage over the threshold: {int((nis[last_before] > THR).sum())} of {last_before.size}; total rejected in the run {r['rec']['stats'].get('gnss_updates_rejected')}")
        for e in [e for e in r["ev"] if e["t_s"] >= B - 5][:6]:
            print(f"       event t={e['t_s']:.1f} {e['status']} d2={e['mahalanobis_sq']:.1f} thr={e['threshold']:.1f}")
        err_fix = np.linalg.norm(
            gnss.positions[:, :2] - np.column_stack([np.interp(gnss.t, seq.truth.t, seq.truth.positions[:, k]) for k in range(2)]), axis=1
        )
        pre = (gnss.t - t0) < A
        sg = np.sqrt(0.5 * (gnss.cov[:, 0] + gnss.cov[:, 1]))
        print(f"   raw fixes before the outage against the reference: median error {np.median(err_fix[pre]):.2f} m, rms {np.sqrt(np.mean(err_fix[pre]**2)):.2f} m; receiver sigma median {np.median(sg[pre]):.2f} m; "
              f"mean of (error/ receiver sigma)^2 (2 dof, expected 2) {np.mean((err_fix[pre] / sg[pre]) ** 2):.2f}; scalar sigma {info['sigma_h']:.2f} m -> {np.mean((err_fix[pre] / info['sigma_h']) ** 2):.2f}")

if mode == "cf":
    base = run("declared")
    summary(base, "declared baseline")
    for cf, label in (("tilt_ref", "(i) reference roll and pitch in the outage"), ("yaw_ref", "(ii) reference yaw only in the outage"), ("all_ref", "(iii) reference attitude fully in the outage")):
        summary(run("declared", cf=cf), label)

if mode == "shift":
    for sh in (0.0, 0.26, -0.26):
        summary(run("declared", shift=sh), f"GNSS shifted {sh:+.2f} s (declared)")

if mode == "sens":
    summary(run("declared"), "declared baseline")
    for k in (2.0, 4.0):
        summary(run("declared", sigma_scale=k), f"GNSS sigma x{k:g} (declared)")
    for k in (3.0, 10.0):
        summary(run("declared", noise_scale=k), f"IMU noise x{k:g} (declared)")

if mode == "interface":
    cov = gnss.cov
    floor2 = 1e-4
    hz = cov[:, :2]
    print(f"valid fixes {len(gnss)}; covariance array shape {cov.shape}")
    print(f"NaN entries: {int(np.isnan(cov).sum())}; negative: {int((cov < 0).sum())}; infinite: {int(np.isinf(cov).sum())}; zero: {int((cov == 0).sum())}; below floor squared (1e-4 m^2, i.e. sigma below 0.01 m): {int((cov < floor2).sum())}")
    print(f"receiver horizontal sigma range {np.sqrt(hz.min()):.3f} to {np.sqrt(hz.max()):.3f} m; fixes whose entry equals the scalar fallback {int((np.isclose(hz[:, 0], info['sigma_h'] ** 2) & np.isclose(hz[:, 1], info['sigma_h'] ** 2)).sum())}")
    fx = ud.load_gga(ROOT, "tunnel1")
    valid = (fx.quality > 0) & np.isfinite(fx.lat_deg)
    print(f"GGA epochs {len(fx.utc_s)}, valid {int(valid.sum())}, dropped by the reader as not valid {int((~valid).sum())}")

bias_cf.py and bw.py

"""Scratch counterfactual: start the filter with biases inferred from the data and the reference (not blind)."""

import sys

sys.argv = ["x", "none"]
exec(open("/private/tmp/claude-501/-Users-daniel-Development-contested-nav/c1a0ff18-ac07-477a-b6af-3a7286e1b092/scratchpad/investigate.py").read())

imu = seq.imu
ref = REF_AT_IMU
m = (TIMELINE >= 40.0) & (TIMELINE < A)
Rm = ref.rotations[m]
dtm = np.diff(TIMELINE[m])
w_ref = rot_log_batch(np.einsum("nji,njk->nik", Rm[:-1], Rm[1:])) / dtm[:, None]
gb = imu.gyro[m][:-1].mean(axis=0) - w_ref.mean(axis=0)
vel_i = np.column_stack([np.interp(TIMELINE[m] + t0, seq.truth.t, seq.velocity[:, k]) for k in range(3)])
a_mean = (vel_i[-1] - vel_i[0]) / (TIMELINE[m][-1] - TIMELINE[m][0])
f_exp = np.einsum("nji,j->ni", Rm, a_mean + np.array([0, 0, G]))
ab = imu.accel[m].mean(axis=0) - f_exp.mean(axis=0)
print("inferred gyro bias", np.round(gb, 5).tolist(), "accel bias", np.round(ab, 4).tolist())

orig_sbe = ee.static_bias_estimate
real = orig_sbe(seq.imu, seq.truth)


def inferred_both(*a, **k):
    out = dict(real)
    out["gyro_bias"], out["accel_bias"] = gb, ab
    return out


def inferred_gyro(*a, **k):
    out = dict(real)
    out["gyro_bias"] = gb
    return out


summary(run("declared"), "declared baseline (static start biases)")
for name, fn in (("gyro bias inferred from the data", inferred_gyro), ("gyro and accel bias inferred from the data", inferred_both)):
    ee.static_bias_estimate = fn
    try:
        r = run("declared")
    finally:
        ee.static_bias_estimate = orig_sbe
    summary(r, name)
    attitude_table(r, name)

"""Scratch: which part of the IMU model makes the difference, the white-noise densities or the bias random walks?"""

import sys

sys.argv = ["x", "none"]
exec(open("/private/tmp/claude-501/-Users-daniel-Development-contested-nav/c1a0ff18-ac07-477a-b6af-3a7286e1b092/scratchpad/investigate.py").read())

summary(run("declared"), "declared baseline")
summary(run("declared", bias_walk_scale=10.0), "bias random walks x10 only")
summary(run("declared", bias_walk_scale=100.0), "bias random walks x100 only")
summary(run("declared", noise_scale=3.0, bias_walk_scale=1.0 / 3.0), "white-noise densities x3 only")
summary(run("declared", noise_scale=3.0), "all four IMU noise terms x3")
r = run("declared", bias_walk_scale=100.0)
attitude_table(r, "bias random walks x100 only")
r = run("per_fix", noise_scale=3.0)
summary(r, "per_fix, all four IMU noise terms x3")

🤖 Generated with Claude Code