diff --git a/ch4_rf_point_positioning/README.md b/ch4_rf_point_positioning/README.md index 31304d0..e2bec3b 100644 --- a/ch4_rf_point_positioning/README.md +++ b/ch4_rf_point_positioning/README.md @@ -395,7 +395,7 @@ round trip measures distance better. Running `python -m ch4_rf_point_positioning.example_comparison` generates a comprehensive comparison: The left four columns are the noise injected at each level; the middle columns -are the resulting median position error over all 50 fixes; the right two count +are the resulting median position error over all 50 fixes; the right three count the fixes that failed. One draw is not a measurement, so this table reports medians rather than the single realisation it used to — see `.cursor/rules/030-figures-and-claims.mdc`. @@ -404,25 +404,78 @@ medians rather than the single realisation it used to — see ``` Results Summary (median error in metres) ====================================================================== - Clock bias: 1.5 m (TOA only; cancels in TDOA) + Clock bias: 1.5 m (TOA only; cancels in TDOA and RTT) RSS config: Rayleigh short-term (sigma=0.5), 5 samples averaged AOA anchor 3 is 10x noisier than the others; 'AOA unw' solves the same bearings unweighted -Level TOA(m) TDOA(m) AOA(deg) RSS(dB) TOA TDOA AOA AOA unw RSS AOA fail RSS fail --------------------------------------------------------------------------------------------------------------------- -1 0.00 0.00 0.0 0.0 0.000 0.000 0.000 0.000 1.652 0 ~ -2 0.05 0.05 1.0 2.0 0.044 0.047 0.121 0.355 2.219 0 ~ -3 0.10 0.10 3.0 4.0 0.088 0.091 0.384 1.430 3.246 0 ~ -4 0.20 0.20 5.0 6.0 0.162 0.175 0.519 2.433 4.029 0 ~ -5 0.50 0.50 10.0 8.0 0.507 0.567 1.129 7.026 8.045 ~ ~ + 'RTT' is two-way TOA on the identical range-noise draw as 'TOA', solved + for (x, y) alone: same anchors, points, seed and sigma, one fewer state +Level TOA(m) TDOA(m) AOA(deg) RSS(dB) TOA RTT TDOA AOA AOA unw RSS RTT fail AOA fail RSS fail +---------------------------------------------------------------------------------------------------------------------------------------- +1 0.00 0.00 0.0 0.0 0.000 0.000 0.000 0.000 0.000 1.652 ~ 0 ~ +2 0.05 0.05 1.0 2.0 0.044 0.044 0.047 0.121 0.355 2.219 ~ 0 ~ +3 0.10 0.10 3.0 4.0 0.088 0.083 0.091 0.384 1.430 3.246 ~ 0 ~ +4 0.20 0.20 5.0 6.0 0.162 0.152 0.175 0.519 2.433 4.029 ~ 0 ~ +5 0.50 0.50 10.0 8.0 0.507 0.431 0.567 1.129 7.026 8.045 ~ ~ ~ ``` -The two fail columns carry `~` because they are integer counts with no +The three fail columns carry `~` because they are integer counts with no tolerance to give: a fix "fails" partly on whether an iterative solve reached a 1e-6 m step inside its budget, and that is exactly the kind of thing a different LAPACK moves by one. The medians beside them are pinned. On this -machine the RSS column reads 21, 19, 26, 34, 38 of 50 and AOA's level 5 reads -4 — none of them the >100 m kind, which is `[0, 0, 0, 0, 0]` and printed under -the table. +machine the RSS column reads 21, 19, 26, 34, 38 of 50, AOA's level 5 reads +4, and RTT reads 0, 0, 0, 0, 1 — none of them the >100 m kind, which is +`[0, 0, 0, 0, 0]` and printed under the table. + +#### What estimating the clock costs (`TOA` against `RTT`) + +The two range columns are a **matched pair**, and the match is the experiment. +`RTT` is two-way TOA — Eqs. (4.6)-(4.9) — and it shares everything with `TOA` +except one thing: the state vector. Same anchors, same 50 test points, same +seed, the same sigma ladder, and the *same realisation* drawn from it, because +the example draws the range noise once per level and hands the array to both. +What differs is that a one-way pseudorange contains an unknown receiver clock +offset, so `TOA` solves for `(x, y, c*dt)` per Eqs. (4.24)-(4.26); a round trip +is timed on one clock and cancels that offset by construction, so `RTT` solves +for `(x, y)` and no bias is injected into it. + +So the gap between the two columns prices the third unknown, and it is +measured rather than argued: + +| σ (m) | TOA (m) | RTT (m) | TOA / RTT | +|---|---|---|---| +| 0.00 | 0.000 | 0.000 | — both exact | +| 0.05 | 0.044 | 0.044 | 1.00× | +| 0.10 | 0.088 | 0.083 | 1.06× | +| 0.20 | 0.162 | 0.152 | 1.07× | +| 0.50 | 0.507 | 0.431 | 1.18× | + +**Read that column as one number and not as a trend.** Adding the clock column +to `H` inflates the position covariance by a factor set by the *geometry alone* +— it does not grow with σ — and for this square array the analytic value is +**1.026** (median over per-point DOP ratios; 1.033 as a ratio of medians). The +spread from 1.00 to 1.18 above is a 50-point median resolving a 2.6% effect +through about ±7% of sampling noise. Over 40 repeats per level the ratio of +medians ranges 0.87 to 1.27, so a single level cannot even establish the sign. +`tests/ch4_rf_point_positioning/test_rtt_matched_comparison.py` therefore pins +the claim twice: analytically, where the inequality is exact at every one of +the 50 points, and over a 1200-point sample, where 1.026 is resolvable. + +Two things this row deliberately does **not** do. It does not model the RTT +noise through processing time and clock drift, though `simulate_rtt_measurement` +offers that and Example 5 of `example_toa_positioning` uses it — the σ here is +charged on the *converted* range of Eq. (4.7). Varying the error budget as well +as the state vector would leave the comparison unable to attribute the +difference to either. And nothing here charges RTT for the transponder or for +the processing-time calibration it needs, so the ~3% is the *best case* for +two-way ranging, not a net verdict on it. + +The noiseless row is worth its own sentence: both range methods are exact +there, at 3.5e-10 m and 9.9e-10 m, so the two-way series is in fact the larger +of the two by 0.6 nanometres. Nothing is measured at σ = 0, which is why the +example prints no ratio for level 1. An earlier draft guarded that print on +`median > 0` and duly reported "0.35×" as the cost of the clock state — a +ratio of two roundoff residues under a heading claiming to price a physical +effect. **TOA reads 0.000 m at level 1 because it now estimates the clock.** The comparison injects a shared 1.5 m receiver bias into the TOA pseudoranges, and @@ -470,18 +523,27 @@ read was a ratio between two different noise realisations. *This figure shows four subplots comparing RF positioning methods:* - **Top-Left:** median error over every fix, against the noise **level index** - 1–5. The x-axis is an index and not a distance because the four methods have - four schedules in three different units; each series carries its own in the + 1–5. The x-axis is an index and not a distance because the five methods have + five schedules in three different units; each series carries its own in the legend. They used to be drawn at TOA's metre positions under an axis labelled "Measurement Noise (m)", which put AOA's 10° at x = 0.5 m. -- **Top-Right:** error CDF at level 3 — the panel title names all four methods' - noise there, rather than only TOA's. TOA/TDOA/AOA are sub-metre; RSS spreads - past 12 m. + **Do not look for the TOA/RTT gap here.** RSS at 8 m puts the three range + series in a band near zero, and a log axis — which is how the sibling + geometry figure solves exactly this — was rendered and rejected: level 1 is + noiseless, so those medians are roundoff at 1e-10 to 1e-8 m and a log axis + spends ten of twelve decades on them, compressing every informative level + into the top fifth of the panel. A 2.6% difference between two series that + also share a schedule is a table result, not a picture. +- **Top-Right:** error CDF at level 3 — the panel title names all five methods' + noise there, rather than only TOA's. TOA/RTT/TDOA/AOA are sub-metre; RSS + spreads past 12 m. - **Bottom-Left:** error distribution at level 3 — RSS has far higher variance. - **Bottom-Right:** fixes solved, on the same four conditions as the table. - TOA and TDOA hold 100% across the sweep, AOA falls to 92% at level 5, and RSS - runs 58% down to 23%. This is where the failures live, so that the panel - above it can be an accuracy. + TOA, RTT and TDOA hold 100% across the sweep bar a single RTT fix at level 5, + AOA falls to 92% there, and RSS runs 58% down to 23%. This is where the + failures live, so that the panel above it can be an accuracy. Each method has + its own dash pattern, because four of the five coincide at 100% for most of + the sweep and solid lines would show only the last one drawn. ### Geometry is method-specific (`--compare-geometry`) @@ -1118,7 +1180,7 @@ All figures are generated by the example scripts and stored in the `figs/` direc | Figure | Source Script | Description | |--------|--------------|-------------| | `toa_positioning_example.png` | `example_toa_positioning.py` | TOA positioning geometry showing anchors, range circles, true/estimated positions, and iterative solver convergence path | -| `ch4_rf_comparison.png` | `example_comparison.py` | Comprehensive comparison of TOA, TDOA, AOA, and RSS methods: median error vs noise level, error CDF, box plots, and solved rates | +| `ch4_rf_comparison.png` | `example_comparison.py` | Comprehensive comparison of TOA, RTT (two-way TOA), TDOA, AOA, and RSS methods: median error vs noise level, error CDF, box plots, and solved rates | | `ch4_aoa_geometry.png` | `example_aoa_positioning.py` | AOA positioning geometry demonstrating ENU coordinate convention with azimuth angles measured from North | | `ch4_dop_geometry.png` | `example_dop_geometry.py` | DOP field and walk-away curve showing how anchor geometry amplifies the same range noise into different position uncertainty | | `ch4_initial_guess_basin.png` | `example_initial_guess_basin.py` | Starting-point sweep showing where raw angle residuals and book sin/tan residuals converge or fail | diff --git a/ch4_rf_point_positioning/example_comparison.py b/ch4_rf_point_positioning/example_comparison.py index de64475..73a3dd4 100644 --- a/ch4_rf_point_positioning/example_comparison.py +++ b/ch4_rf_point_positioning/example_comparison.py @@ -1,8 +1,8 @@ """ Comparison of RF Positioning Methods. -This script compares TOA, TDOA, AOA, and RSS positioning methods -under various conditions using pre-generated datasets. +This script compares TOA, RTT (two-way TOA), TDOA, AOA, and RSS positioning +methods under various conditions using pre-generated datasets. Can run with: - Inline data (default): @@ -13,7 +13,8 @@ python -m ch4_rf_point_positioning.example_comparison --compare-geometry Implements: - - TOA positioning (Eqs. 4.14-4.23) + - TOA positioning (Eqs. 4.14-4.23), with the clock as a state (Eqs. 4.24-4.26) + - Two-way TOA / RTT ranging (Eqs. 4.6-4.9) - TDOA positioning (Eqs. 4.34-4.42) - AOA positioning (Eqs. 4.63-4.67) - DOP analysis (Section 4.5) @@ -44,6 +45,7 @@ from core.eval import save_figure, show_figures_if_requested from core.rf import ( DIVERGENCE_M, + STALL_M, AOAPositioner, SolveOutcome, TDOAPositioner, @@ -53,6 +55,7 @@ solve_batch, toa_range, toa_solve_with_clock_bias, + two_way_toa_range, ) from core.utils import resolve_data_path @@ -389,11 +392,33 @@ def solve(self, ranges, initial_guess, **kwargs): INLINE_SEED = np.array([5.0, 5.0]) +def draw_range_noise(n_points, n_anchors, range_noise_std_m): + """One ``(N, K)`` draw of per-range gaussian noise, or ``None`` at zero. + + Hoisted out of :func:`toa_positioning_test` so that the one-way and + two-way range series can be handed the **same realisation**. The two + columns exist to price one difference -- the estimator's state vector -- + and drawing separately would put a full realisation of noise on each side, + which is the defect `aoa_bearings` was written to remove one series over + (and which `.cursor/rules/030-figures-and-claims.mdc` names: a paired + comparison on unpaired draws is two experiments reported as a ratio). + + Returning ``None`` rather than a zero array at ``range_noise_std_m == 0`` + is not tidiness: it keeps the global RNG untouched on the noiseless level, + exactly as the in-line ``if range_noise_std_m > 0`` it replaces did, so + every draw downstream in the sweep lands where it did before. + """ + if range_noise_std_m <= 0: + return None + return np.random.randn(n_points, n_anchors) * range_noise_std_m + + def toa_positioning_test( anchors, true_positions, range_noise_std_m=0.0, clock_bias_m=0.0, + range_noise=None, ): """Test TOA positioning (inline mode). Returns a `SolveOutcome`. @@ -404,6 +429,10 @@ def toa_positioning_test( clock_bias_m: Shared receiver clock bias in metres, added to every pseudorange. TDOA differencing cancels this term; TOA has to estimate it, which is what the state below is for. + range_noise: A pre-drawn ``(N, K)`` noise array to add instead of + drawing one. `run_inline_comparison` passes the same array to + :func:`rtt_positioning_test`, so the two range series differ in + their estimator and in nothing else. ``None`` draws as before. **The state is (x, y, c*dt), not (x, y), and that is the whole point.** A shared bias is unobservable to a position-only solver: no (x, y) @@ -430,8 +459,10 @@ def toa_positioning_test( ] ) ranges += clock_bias_m - if range_noise_std_m > 0: - ranges += np.random.randn(*ranges.shape) * range_noise_std_m + if range_noise is None: + range_noise = draw_range_noise(*ranges.shape, range_noise_std_m) + if range_noise is not None: + ranges += range_noise return solve_batch( _ClockBiasSolver(anchors), @@ -442,6 +473,67 @@ def toa_positioning_test( ) +def rtt_positioning_test( + anchors, + true_positions, + range_noise_std_m=0.0, + range_noise=None, +): + """Test RTT (two-way TOA) positioning (inline mode). Returns a `SolveOutcome`. + + Args: + anchors: Anchor positions, shape (K, 2). + true_positions: Ground-truth agent positions, shape (N, 2). + range_noise_std_m: Gaussian noise std in metres, on the **converted** + range -- the quantity Eq. (4.7) hands back after halving the round + trip -- not on the raw round-trip time. + range_noise: A pre-drawn ``(N, K)`` noise array; see + :func:`toa_positioning_test`. + + **Exactly one thing differs from the TOA row above: the state vector.** + Same anchors, same 50 test points, same seed, the same sigma ladder and -- + because `run_inline_comparison` draws once and passes the array to both -- + the same realisation of it. What changes is that a round trip is timed on + one clock, so Eq. (4.7)'s ``d = c(t_rtt - t_proc)/2`` carries no receiver + clock offset to estimate: **no bias is injected here, and the solve is the + two-state `(x, y)` `TOAPositioner`** rather than the three-state + `(x, y, c*dt)` of Eqs. (4.24)-(4.26). The difference between the two + medians is therefore the price of the third unknown, measured rather than + argued. + + **The noise is deliberately *not* modelled through processing time and + clock drift here**, though `simulate_rtt_measurement` offers exactly that + and `example_toa_positioning`'s Example 5 uses it. Doing so would change + two things at once -- the state vector *and* the error budget -- and the + row would no longer answer the question it is in the table to answer. The + cost of the RTT hardware's own uncertainty is taught in that standalone + section, on its own geometry; this one prices the clock state alone. + + Note this makes the comparison favourable to RTT by construction at equal + sigma, which is the honest framing: two-way ranging buys you the missing + unknown, and pays for it in the transponder and the processing-time + calibration that this row charges nothing for. + """ + ranges = np.array( + [ + [two_way_toa_range(anchor, true_pos) for anchor in anchors] + for true_pos in true_positions + ] + ) + if range_noise is None: + range_noise = draw_range_noise(*ranges.shape, range_noise_std_m) + if range_noise is not None: + ranges = ranges + range_noise + + return solve_batch( + TOAPositioner(anchors, method="iterative_ls"), + ranges, + INLINE_SEED, + true_positions, + progress=partial(tqdm, desc=" RTT", leave=False, unit="pt"), + ) + + def tdoa_positioning_test( anchors, true_positions, range_noise_std_m=0.0, clock_bias_m=0.0 ): @@ -640,6 +732,16 @@ def run_inline_comparison(): schedule so that the comparison is apples-to-apples. A shared receiver clock bias is injected into TOA pseudo-ranges to demonstrate that TDOA differencing cancels it. + + **TOA and RTT are a matched pair, and the match is the experiment.** They + share the anchors, the 50 test points, the seed, the sigma ladder and the + realisation drawn from it -- `draw_range_noise` is called once per level + and handed to both. The only difference is the estimator's state vector: + TOA carries `(x, y, c*dt)` because a one-way pseudorange has an unknown + receiver clock in it, RTT carries `(x, y)` because a round trip is timed + on one clock and cancels it by construction. So the gap between the two + columns is the price of the third unknown, measured instead of intuited. + See :func:`rtt_positioning_test` for what is deliberately *not* varied. """ print("=" * 70) print("RF Positioning Methods Comparison") @@ -655,6 +757,10 @@ def run_inline_comparison(): # ---- Independent noise schedules per method ---- toa_range_noise_levels_m = [0.0, 0.05, 0.1, 0.2, 0.5] + # RTT shares TOA's ladder by design, not by coincidence: the two rows are a + # matched pair and the schedule is one of the things held equal. Kept as + # its own list so the figure's legend can carry it per series. + rtt_range_noise_levels_m = list(toa_range_noise_levels_m) tdoa_range_noise_levels_m = [0.0, 0.05, 0.1, 0.2, 0.5] aoa_noise_levels_deg = [0.0, 1.0, 3.0, 5.0, 10.0] # degrees rss_fading_noise_levels_db = [0.0, 2.0, 4.0, 6.0, 8.0] @@ -666,13 +772,24 @@ def run_inline_comparison(): # "AOA_unw" is the unweighted control, reported in the table but not # plotted -- the figure compares measurement types, not solver options. - results = {"TOA": [], "TDOA": [], "AOA": [], "RSS": [], "AOA_unw": []} + results = { + "TOA": [], + "RTT": [], + "TDOA": [], + "AOA": [], + "RSS": [], + "AOA_unw": [], + } print("\nNoise configuration (independent per method):") print( f" TOA : range noise {toa_range_noise_levels_m} m " f"(+ clock bias {clock_bias_m} m)" ) + print( + f" RTT : range noise {rtt_range_noise_levels_m} m " + "(no clock bias; the round trip cancels it)" + ) print(f" TDOA: range noise {tdoa_range_noise_levels_m} m (clock bias cancels)") print(f" AOA : angle noise {aoa_noise_levels_deg} deg") print( @@ -695,17 +812,35 @@ def run_inline_comparison(): print( f"\n[{i+1}/{n_levels}] TOA: {toa_range_noise_m:.2f}m " f"(+bias {clock_bias_m}m), " + f"RTT: {toa_range_noise_m:.2f}m (no bias), " f"TDOA: {tdoa_range_noise_m:.2f}m, " f"AOA: {aoa_noise_levels_deg[i]:.1f}deg, " f"RSS: {rss_fading_db:.1f}dB" ) + # Drawn once and solved twice, which is what makes the TOA/RTT pair a + # measurement of the clock state rather than of two noise draws. It + # also keeps the sweep's global RNG stream where it was: this draw is + # the one `toa_positioning_test` used to make at exactly this point, + # and `rtt_positioning_test` consumes nothing. + shared_range_noise = draw_range_noise( + len(true_positions), len(anchors), toa_range_noise_m + ) results["TOA"].append( toa_positioning_test( anchors, true_positions, toa_range_noise_m, clock_bias_m=clock_bias_m, + range_noise=shared_range_noise, + ) + ) + results["RTT"].append( + rtt_positioning_test( + anchors, + true_positions, + toa_range_noise_m, + range_noise=shared_range_noise, ) ) results["TDOA"].append( @@ -745,7 +880,7 @@ def run_inline_comparison(): print("\n" + "=" * 70) print("Results Summary (median error in metres)") print("=" * 70) - print(f" Clock bias: {clock_bias_m} m (TOA only; cancels in TDOA)") + print(f" Clock bias: {clock_bias_m} m (TOA only; cancels in TDOA and RTT)") print( f" RSS config: Rayleigh short-term (sigma={sigma_short_linear}), " f"{n_samples_avg} samples averaged" @@ -754,11 +889,16 @@ def run_inline_comparison(): f" AOA anchor {DEGRADED_ANCHOR} is {DEGRADED_ANCHOR_SCALE:.0f}x noisier " f"than the others; 'AOA unw' solves the same bearings unweighted" ) + print( + " 'RTT' is two-way TOA on the identical range-noise draw as 'TOA', " + "solved\n for (x, y) alone: same anchors, points, seed and sigma, one " + "fewer state" + ) header = ( f"{'Level':<6} {'TOA(m)':<9} {'TDOA(m)':<9} " f"{'AOA(deg)':<9} {'RSS(dB)':<9} " - f"{'TOA':<9} {'TDOA':<9} {'AOA':<9} {'AOA unw':<9} " - f"{'RSS':<9} {'AOA fail':<9} {'RSS fail':<9}" + f"{'TOA':<9} {'RTT':<9} {'TDOA':<9} {'AOA':<9} {'AOA unw':<9} " + f"{'RSS':<9} {'RTT fail':<9} {'AOA fail':<9} {'RSS fail':<9}" ) print(header) print("-" * len(header)) @@ -777,10 +917,12 @@ def run_inline_comparison(): f"{aoa_noise_levels_deg[i]:<9.1f} " f"{rss_fading_noise_levels_db[i]:<9.1f} " f"{results['TOA'][i].median_m:<9.3f} " + f"{results['RTT'][i].median_m:<9.3f} " f"{results['TDOA'][i].median_m:<9.3f} " f"{results['AOA'][i].median_m:<9.3f} " f"{results['AOA_unw'][i].median_m:<9.3f} " f"{results['RSS'][i].median_m:<9.3f} " + f"{results['RTT'][i].n_failed:<9d} " f"{results['AOA'][i].n_failed:<9d} " f"{results['RSS'][i].n_failed:<9d}" ) @@ -790,8 +932,43 @@ def run_inline_comparison(): for i in range(n_levels) ] + # What one fewer state is worth, printed rather than asserted. Both + # columns solve the SAME range errors, so the ratio is not a comparison of + # two noise realisations -- it is the cost of carrying the clock. + # + # Level 1 is exact on both sides and must not print a ratio. `rtt > 0` is + # not the guard that gets this right: the noiseless medians are 3.5e-10 m + # and 9.9e-10 m -- both zero as far as a positioning system is concerned, + # both nonzero as far as float division is concerned -- and the first draft + # of this loop duly printed "0.35x", a ratio of two roundoff residues, + # under a heading claiming to price the clock state. The floor is + # `STALL_M`, the solver's own 1e-6 m step tolerance: nothing below it is a + # distance, which is exactly why `solve_batch` uses it to decide a fix + # never left its seed. + print() + print(" What the clock state costs, TOA / RTT on the identical draw:") + for i in range(n_levels): + toa_m = results["TOA"][i].median_m + rtt_m = results["RTT"][i].median_m + if max(toa_m, rtt_m) < STALL_M: + ratio = f"-- (both exact, under {STALL_M:g} m)" + else: + ratio = f"{toa_m / rtt_m:.2f}x" + print( + f" level {i+1} sigma {toa_range_noise_levels_m[i]:.2f} m " + f"TOA {toa_m:.3f} m RTT {rtt_m:.3f} m {ratio}" + ) + print(" Four ranges over three unknowns against four over two: one degree") + print(" of freedom instead of two, and the inflation is the price of not") + print(" having a beacon that answers back. RTT's noise is charged on the") + print(" post-conversion range (Eq. 4.7), NOT modelled through processing") + print(" time and clock drift -- that would change the error budget as well") + print(" as the state vector, and this row exists to change one thing.") + print(" example_toa_positioning's Example 5 prices the RTT hardware budget") + print(" on its own geometry; nothing here charges RTT for the transponder.") + print() - print(" The two 'fail' columns count fixes that raised, reported") + print(" The three 'fail' columns count fixes that raised, reported") print(" converged=False, never left the seed, or landed over 100 m away") print(" -- core.rf.solve_batch's four conditions. Every median above is") print(" over all 50 fixes including those, which is the point: the RSS") @@ -836,9 +1013,10 @@ def run_inline_comparison(): print(" Only the spread between anchors carries information.") # The schedules travel with the results, because the figure needs them: - # four series in three different units cannot share one x-axis in metres. + # five series in three different units cannot share one x-axis in metres. level_schedules = ( ("TOA", toa_range_noise_levels_m, "m"), + ("RTT", rtt_range_noise_levels_m, "m"), ("TDOA", tdoa_range_noise_levels_m, "m"), ("AOA", aoa_noise_levels_deg, "deg"), ("RSS", rss_fading_noise_levels_db, "dB"), @@ -1076,9 +1254,9 @@ def plot_geometry_comparison(all_results: dict): def _schedule_label(method, schedule, unit): """A legend entry that carries the series' own noise units. - Panels 1 and 4 plot four methods against a level *index*, because the four - schedules are in three different units: metres for TOA and TDOA, degrees - for AOA, dB for RSS. They used to be drawn at TOA's metre positions under + Panels 1 and 4 plot five methods against a level *index*, because the five + schedules are in three different units: metres for TOA, RTT and TDOA, + degrees for AOA, dB for RSS. They used to be drawn at TOA's metre positions under an x-axis reading "Measurement Noise (m)", which puts AOA's 10 deg at x = 0.5 m and invites exactly one reading, the wrong one. The units now travel with the line that owns them. @@ -1100,14 +1278,35 @@ def plot_inline_comparison(level_schedules, results): fig.suptitle("RF Positioning Methods Comparison", fontsize=16, fontweight="bold") methods = [name for name, _, _ in level_schedules] - colors = ["blue", "red", "green", "orange"] + # Keyed by method, not positional, so that inserting a series cannot + # silently recolour the four that were here before it -- a reader who + # learned "RSS is orange" from the committed figure keeps that. RTT's + # purple is the fifth: dark enough to read against the white panel and + # distinguishable from the blue it will sit closest to. + colors = { + "TOA": "blue", + "RTT": "purple", + "TDOA": "red", + "AOA": "green", + "RSS": "orange", + } # One dash pattern per method, used by every line panel below. TOA and - # TDOA now trace each other almost exactly -- they carry the same - # information once TOA estimates its clock, which is the point of this - # figure -- so with four solid lines only the last one drawn is visible - # and the panel reads as "TOA is missing". The success-rate panel already - # had this problem for the same reason and solved it this way. - dashes = ["-", "--", "-.", ":"] + # TDOA trace each other almost exactly -- they carry the same information + # once TOA estimates its clock, which is the point of this figure -- so + # with solid lines only the last one drawn is visible and the panel reads + # as "TOA is missing". The success-rate panel already had this problem for + # the same reason and solved it this way. RTT joins the pile-up at level 1, + # where every range method is exact, and at 100 % in panel 4, so it needs + # a pattern of its own rather than a shade of one. + dashes = { + "TOA": "-", + "RTT": (0, (3, 1, 1, 1)), + "TDOA": "--", + "AOA": "-.", + "RSS": ":", + } + series_colors = [colors[name] for name in methods] + series_dashes = [dashes[name] for name in methods] n_levels = len(results[methods[0]]) level_index = np.arange(1, n_levels + 1) @@ -1128,15 +1327,30 @@ def _level_caption(idx): # tolerance, so this panel was drawing the accuracy of a quarter of its # sample, selected by fit. The failures are in panel 4, where they can be # read as failures. + # + # **The axis stays linear, and a log axis was rendered before deciding + # that.** RSS at 8 m does crush TOA/RTT/TDOA into a band near zero, which + # is the shape `.cursor/rules/030` warns about, and the sibling + # `plot_geometry_comparison` fixes exactly that with `set_yscale("log")`. + # It does not work here: level 1 is noiseless, so the range and bearing + # medians there are roundoff -- 3.5e-10 to 1.6e-08 m -- and a log axis + # spends ten of its twelve decades on that, compressing levels 2-5 into + # the top fifth of the panel. The TOA/RTT gap is no more readable after + # the change than before it, and the panel acquires a meaningless cliff. + # + # So do not look for the cost of the clock state in this panel: it is + # 2.6% between two series that also share a noise schedule, and no scaling + # of a five-method overview will show it. The console table prints the + # TOA/RTT ratio per level, which is where that comparison lives. ax1 = axes[0, 0] for (method, schedule, unit), color, dash in zip( - level_schedules, colors, dashes, strict=True + level_schedules, series_colors, series_dashes, strict=True ): medians = [outcome.median_m for outcome in results[method]] ax1.plot( level_index, medians, - dash, + linestyle=dash, marker="o", label=_schedule_label(method, schedule, unit), color=color, @@ -1153,14 +1367,21 @@ def _level_caption(idx): ax2 = axes[0, 1] noise_idx = 2 for (method, _, _), color, dash in zip( - level_schedules, colors, dashes, strict=True + level_schedules, series_colors, series_dashes, strict=True ): errors = results[method][noise_idx].errors errors = errors[np.isfinite(errors)] if len(errors) > 0: sorted_errors = np.sort(errors) cdf = np.arange(1, len(sorted_errors) + 1) / len(sorted_errors) - ax2.plot(sorted_errors, cdf, dash, label=method, color=color, linewidth=2) + ax2.plot( + sorted_errors, + cdf, + linestyle=dash, + label=method, + color=color, + linewidth=2, + ) ax2.set_xlabel("Position Error (m)") ax2.set_ylabel("CDF") ax2.set_title(f"Error CDF at level {noise_idx + 1}\n({_level_caption(noise_idx)})") @@ -1178,7 +1399,12 @@ def _level_caption(idx): data.append(errors) labels.append(method) bp = ax3.boxplot(data, tick_labels=labels, patch_artist=True, showfliers=False) - for patch, color in zip(bp["boxes"], colors[: len(data)], strict=True): + # Keyed off the surviving labels, not sliced off the front of the colour + # list: a method dropped for having no finite errors would otherwise shift + # every box after it onto the wrong colour. `strict=True` is the guard that + # the two lists still line up. + box_colors = [colors[name] for name in labels] + for patch, color in zip(bp["boxes"], box_colors, strict=True): patch.set_facecolor(color) patch.set_alpha(0.6) ax3.set_ylabel("Position Error (m)") @@ -1189,7 +1415,7 @@ def _level_caption(idx): # 4. Success Rate # - # Three of the four methods now sit on 100 % for most of the sweep, so a + # Four of the five methods now sit on 100 % for most of the sweep, so a # solid line per method would hide all but the last one drawn -- the same # "reads as absent" failure this file warns about elsewhere, arrived at # from the opposite direction. Distinct dash patterns keep every method @@ -1197,7 +1423,7 @@ def _level_caption(idx): # position. ax4 = axes[1, 1] for (method, schedule, unit), color, dash in zip( - level_schedules, colors, dashes, strict=True + level_schedules, series_colors, series_dashes, strict=True ): rates = [ 100.0 * outcome.solved.sum() / outcome.n for outcome in results[method] @@ -1205,7 +1431,7 @@ def _level_caption(idx): ax4.plot( level_index, rates, - dash, + linestyle=dash, marker="o", label=_schedule_label(method, schedule, unit), color=color, diff --git a/ch4_rf_point_positioning/figs/ch4_rf_comparison.pdf b/ch4_rf_point_positioning/figs/ch4_rf_comparison.pdf index e622d0c..f6b9215 100644 Binary files a/ch4_rf_point_positioning/figs/ch4_rf_comparison.pdf and b/ch4_rf_point_positioning/figs/ch4_rf_comparison.pdf differ diff --git a/ch4_rf_point_positioning/figs/ch4_rf_comparison.png b/ch4_rf_point_positioning/figs/ch4_rf_comparison.png index 88127ae..be29598 100644 Binary files a/ch4_rf_point_positioning/figs/ch4_rf_comparison.png and b/ch4_rf_point_positioning/figs/ch4_rf_comparison.png differ diff --git a/ch4_rf_point_positioning/figs/ch4_rf_comparison.svg b/ch4_rf_point_positioning/figs/ch4_rf_comparison.svg index 390e7dd..656f13a 100644 --- a/ch4_rf_point_positioning/figs/ch4_rf_comparison.svg +++ b/ch4_rf_point_positioning/figs/ch4_rf_comparison.svg @@ -9,7 +9,7 @@ image/svg+xml - Matplotlib v3.10.7, https://matplotlib.org/ + Matplotlib v3.10.8, https://matplotlib.org/ @@ -1082,6 +1082,34 @@ z + + + + + + + + + + + + + - + - + - - + - + +" style="fill: none; stroke-dasharray: 6,2,2,2; stroke-dashoffset: 0; stroke: #800080; stroke-width: 2"/> - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + - + - + - + - - + - + - + - + - - + +" clip-path="url(#p328dcc48b5)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -1619,116 +1688,116 @@ L 542.353125 59.856 - - + + - + - + - + - + - - + + - + - + - + - + - - + + - + - + - + - + - - + + - + - + - + - + - - + + - + - + - + - + - - + + - + - + - + - + - + @@ -1792,17 +1861,17 @@ z - + +" clip-path="url(#p328dcc48b5)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -1812,17 +1881,17 @@ L 993.233125 321.603881 - + +" clip-path="url(#p328dcc48b5)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -1832,17 +1901,17 @@ L 993.233125 271.699614 - + +" clip-path="url(#p328dcc48b5)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -1852,17 +1921,17 @@ L 993.233125 221.795347 - + +" clip-path="url(#p328dcc48b5)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -1872,17 +1941,17 @@ L 993.233125 171.89108 - + +" clip-path="url(#p328dcc48b5)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -1892,17 +1961,17 @@ L 993.233125 121.986813 - + +" clip-path="url(#p328dcc48b5)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -1911,7 +1980,7 @@ L 993.233125 72.082545 - + @@ -1956,217 +2025,270 @@ z - - - - - - - + - + + + + + + + + + + - + @@ -2212,8 +2334,8 @@ L 993.233125 59.856 - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -2276,36 +2409,50 @@ z - - + - + - + - + + + + + + + + + + + + - + @@ -2314,13 +2461,13 @@ L 947.887813 284.904062 - + - + @@ -2328,13 +2475,13 @@ L 947.887813 299.582187 - + - + @@ -2355,14 +2502,14 @@ z - + - + - + - + @@ -2370,14 +2517,29 @@ z - + - + - + + + + + + + + + + + + + + + + - + @@ -2385,30 +2547,30 @@ z - - + + - + - + - + - - + + - + - + - + @@ -2418,115 +2580,115 @@ z - - + - + - + - + - + - - + - + - + - + - + - - + - + - + - + - + - - + - + - + - + - + - - + - + - + - + - + - - + - + - + - + - + - + @@ -2551,162 +2713,196 @@ L 491.483125 433.247221 - - - + - - + - - + - - + - +" clip-path="url(#paa34fe71a5)" style="fill: #800080; opacity: 0.6; stroke: #000000; stroke-linejoin: miter"/> - - + - - + - - + - - + - +" clip-path="url(#paa34fe71a5)" style="fill: #ff0000; opacity: 0.6; stroke: #000000; stroke-linejoin: miter"/> - - + - - + - - + - - + - + + + + + + + + + + + + + + + - - + - - + - - + - - + - - + - - + - - + - - + - + + + + - + - + - + - + @@ -2767,8 +2963,8 @@ z - - + + @@ -2781,43 +2977,54 @@ z - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - + + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + - - + + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + - - + + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + - - + + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + - - + + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + - + @@ -2976,17 +3183,17 @@ L 972.73858 401.856 - + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -2994,17 +3201,17 @@ L 993.233125 670.84 - + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -3013,17 +3220,17 @@ L 993.233125 619.604952 - + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -3032,17 +3239,17 @@ L 993.233125 568.369905 - + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -3051,17 +3258,17 @@ L 993.233125 517.134857 - + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -3070,17 +3277,17 @@ L 993.233125 465.89981 - + +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #b0b0b0; stroke-opacity: 0.3; stroke-width: 0.8; stroke-linecap: square"/> - + - + @@ -3089,7 +3296,7 @@ L 993.233125 414.664762 - + @@ -3160,14 +3367,14 @@ z - + - +" clip-path="url(#p63ad14a597)" style="fill: none; stroke: #0000ff; stroke-width: 2; stroke-linecap: square"/> + @@ -3175,14 +3382,29 @@ L 972.73858 414.664762 - + + + + + + + + + + + - +" clip-path="url(#p63ad14a597)" style="fill: none; stroke-dasharray: 7.4,3.2; stroke-dashoffset: 0; stroke: #ff0000; stroke-width: 2"/> + @@ -3190,14 +3412,14 @@ L 972.73858 414.664762 - + - +" clip-path="url(#p63ad14a597)" style="fill: none; stroke-dasharray: 12.8,3.2,2,3.2; stroke-dashoffset: 0; stroke: #008000; stroke-width: 2"/> + @@ -3205,14 +3427,14 @@ L 972.73858 435.158781 - + - +" clip-path="url(#p63ad14a597)" style="fill: none; stroke-dasharray: 2,3.3; stroke-dashoffset: 0; stroke: #ffa500; stroke-width: 2"/> + @@ -3220,27 +3442,27 @@ L 972.73858 609.357943 - + - + - + - + - + @@ -3378,31 +3600,31 @@ z - + - - + - + - + - + @@ -3430,7 +3652,46 @@ L 565.553125 623.94875 - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + @@ -3470,7 +3731,7 @@ L 565.553125 635.69125 - + - + @@ -3503,7 +3764,7 @@ L 565.553125 647.43375 - + - + @@ -3536,7 +3797,7 @@ L 565.553125 659.17625 - + @@ -4004,13 +4265,13 @@ z - + - + diff --git a/tests/ch4_rf_point_positioning/test_rtt_matched_comparison.py b/tests/ch4_rf_point_positioning/test_rtt_matched_comparison.py new file mode 100644 index 0000000..8421236 --- /dev/null +++ b/tests/ch4_rf_point_positioning/test_rtt_matched_comparison.py @@ -0,0 +1,388 @@ +"""What estimating the clock costs, measured against a beacon that answers back. + +Chapter 4's inline comparison carries a one-way TOA series whose state is +`(x, y, c*dt)` -- Eqs. (4.24)-(4.26), because a one-way pseudorange contains an +unknown receiver clock offset -- beside a two-way TOA (RTT) series whose state +is `(x, y)`, because Eq. (4.7)'s `d = c(t_rtt - t_proc)/2` is timed on one +clock and cancels the offset by construction. + +**The two rows are matched so that the gap between them means something.** Same +anchors, same 50 test points, same seed, same sigma ladder, and the same +realisation drawn from it -- `run_inline_comparison` calls `draw_range_noise` +once per level and hands the array to both. Only the state vector differs. The +repo has been bitten by the unpaired version of exactly this: `aoa_bearings` +exists because the weighted and unweighted AOA columns used to draw their own +noise, so the ratio between them carried a full realisation on each side. + +The RTT row is deliberately *not* noised through processing time and clock +drift, though `simulate_rtt_measurement` offers that and +`example_toa_positioning`'s Example 5 uses it. That would vary the error budget +as well as the state vector, and this row exists to vary one thing. + +**The size of the effect is small, and that is the finding these tests are +shaped around.** Adding the clock column to H inflates the position covariance +by a factor set by the geometry alone -- it does not grow with sigma. For this +square array and these 50 points the analytic inflation is a median 1.026 over +per-point ratios (1.033 as a ratio of medians), and a 50-point median has a +sampling spread of roughly +/-7%. So "the RTT median is below the TOA median" +is **not** a law that a single noise level can demonstrate: over 40 repeats at +each sigma the ratio of medians ranged 0.87 to 1.27. It happens to hold at +every noisy level of the shipped seed, and that is pinned below as a property +of the shipped realisation rather than as physics. The physics is pinned twice +over instead -- once analytically, where it is exact, and once over a sample +large enough to resolve 2.6%. + +Author: Li-Ta Hsu +References: Chapter 4, Eqs. (4.6)-(4.9), (4.14)-(4.23), (4.24)-(4.26) +""" + +import contextlib +import functools +import io +import unittest + +import matplotlib + +matplotlib.use("Agg") # headless: no display during tests + +import numpy as np + +from ch4_rf_point_positioning.example_comparison import ( + INLINE_SEED, + draw_range_noise, + generate_scenario, + rtt_positioning_test, + run_inline_comparison, + toa_positioning_test, +) +from core.rf import STALL_M, TOAPositioner, toa_solve_with_clock_bias + +#: The clock bias `run_inline_comparison` injects into the one-way ranges. +CLOCK_BIAS_M = 1.5 + +#: Points per repeat in the statistical arm. The effect is 2.6% and a +#: 50-point median carries ~7% of sampling noise, so the shipped sweep's own +#: sample cannot resolve it -- measuring it needs its own, larger one. +STAT_POINTS = 1200 +STAT_SEEDS = (0, 1, 2, 3) +STAT_SIGMA_M = 0.2 + + +@functools.lru_cache(maxsize=1) +def _sweep(): + """The inline sweep, run once and shared. + + Six seconds a call, and four tests want it. `.cursor/rules/030` asks for + exactly this (`tests/ch7_slam/slam_example_runner.py` is the pattern). + No assertion lives in here: `lru_cache` does not cache exceptions, so a + memoised helper that asserts re-runs its whole body for every caller once + anything goes wrong. + """ + with contextlib.redirect_stdout(io.StringIO()): + return run_inline_comparison() + + +def _paired_medians(sigma_m, n_points, seed, rtt_gets_the_clock_state=False): + """Median one-way and two-way error over `n_points`, on one noise draw. + + Both estimators see the identical ranges up to the injected bias, which is + the whole design. `rtt_gets_the_clock_state` is the mutation this file's + tolerance was justified against -- see + `test_the_measured_cost_is_the_state_vector_and_not_the_noise`. + """ + anchors, _ = generate_scenario(seed=42) + rng = np.random.default_rng(seed) + points = rng.uniform(1, 9, size=(n_points, 2)) + true_ranges = np.linalg.norm(points[:, None, :] - anchors[None, :, :], axis=2) + noise = rng.standard_normal(true_ranges.shape) * sigma_m + + seed_3 = np.concatenate([INLINE_SEED, [0.0]]) + one_way, two_way = [], [] + for i, point in enumerate(points): + est, _bias, _info = toa_solve_with_clock_bias( + anchors, true_ranges[i] + CLOCK_BIAS_M + noise[i], seed_3 + ) + one_way.append(float(np.linalg.norm(est - point))) + + if rtt_gets_the_clock_state: + est, _bias, _info = toa_solve_with_clock_bias( + anchors, true_ranges[i] + noise[i], seed_3 + ) + else: + est, _info = TOAPositioner(anchors, method="iterative_ls").solve( + true_ranges[i] + noise[i], INLINE_SEED + ) + two_way.append(float(np.linalg.norm(est - point))) + + return float(np.median(one_way)), float(np.median(two_way)) + + +def _analytic_inflation(): + """Per-point ratio of 3-state to 2-state position DOP, over the 50 points. + + Sigma factors out of both sides, which is the point: the cost of the clock + column is a property of the geometry and not of the noise. + """ + anchors, truth = generate_scenario(seed=42) + two_state, three_state = [], [] + for point in truth: + offsets = point - anchors + unit = offsets / np.linalg.norm(offsets, axis=1, keepdims=True) + h_2 = unit + # A one-way pseudorange is d + b, so the clock column is +1. + h_3 = np.hstack([unit, np.ones((len(anchors), 1))]) + two_state.append(np.sqrt(np.trace(np.linalg.inv(h_2.T @ h_2)))) + three_state.append(np.sqrt(np.trace(np.linalg.inv(h_3.T @ h_3)[:2, :2]))) + return np.array(three_state) / np.array(two_state) + + +class RttSeriesIsPresentAndMatched(unittest.TestCase): + """The series exists, and the things held equal are actually equal.""" + + def test_the_sweep_returns_a_two_way_series(self): + schedules, results = _sweep() + self.assertIn("RTT", results) + + names = [name for name, _, _ in schedules] + self.assertIn("RTT", names) + self.assertEqual(len(results["RTT"]), len(results["TOA"])) + + def test_rtt_shares_toa_s_noise_ladder(self): + """A matched pair whose sigma schedules differ is not matched. + + Pinned rather than trusted because the two schedules are separate + lists in the source, and a plausible future edit widens one of them. + """ + schedules, _ = _sweep() + ladders = {name: schedule for name, schedule, _ in schedules} + units = {name: unit for name, _, unit in schedules} + + self.assertEqual(ladders["RTT"], ladders["TOA"]) + self.assertEqual(units["RTT"], "m") + + def test_the_two_range_series_solve_the_same_noise_draw(self): + """Unpaired draws would make the gap a comparison of realisations. + + Checked by handing both functions a noise array of our own: if either + one drew its own instead, the errors below would not reproduce. + """ + anchors, truth = generate_scenario(seed=42) + np.random.seed(7) + noise = draw_range_noise(len(truth), len(anchors), 0.2) + + toa_a = toa_positioning_test( + anchors, truth, 0.2, clock_bias_m=CLOCK_BIAS_M, range_noise=noise + ) + rtt_a = rtt_positioning_test(anchors, truth, 0.2, range_noise=noise) + + # Same arrays again, from a global RNG left somewhere else entirely. + np.random.seed(999) + toa_b = toa_positioning_test( + anchors, truth, 0.2, clock_bias_m=CLOCK_BIAS_M, range_noise=noise + ) + rtt_b = rtt_positioning_test(anchors, truth, 0.2, range_noise=noise) + + np.testing.assert_array_equal(toa_a.errors, toa_b.errors) + np.testing.assert_array_equal(rtt_a.errors, rtt_b.errors) + + def test_a_zero_sigma_level_draws_nothing_at_all(self): + """The noiseless level must not consume the global stream. + + This is what keeps the four pre-existing series byte-stable: the + in-line `if range_noise_std_m > 0` this replaced did not draw either, + so every draw later in the sweep still lands where it did. + """ + self.assertIsNone(draw_range_noise(50, 4, 0.0)) + + np.random.seed(3) + before = np.random.randn(5).copy() + np.random.seed(3) + self.assertIsNone(draw_range_noise(50, 4, 0.0)) + np.testing.assert_array_equal(before, np.random.randn(5)) + + +class TheClockStateIsWhatCostsAccuracy(unittest.TestCase): + """The physics, pinned where it is exact and where it is measurable.""" + + def test_the_clock_column_inflates_position_dop_at_every_test_point(self): + """Exact, tolerance-free, and the reason the RTT row is worth having. + + Estimating a fourth quantity from the same four ranges leaves one + degree of freedom instead of two. The position block of + `(H3' H3)^-1` is the Schur complement of the clock column, so it + cannot be smaller than `(H2' H2)^-1` -- at any geometry, at any sigma. + """ + inflation = _analytic_inflation() + + self.assertTrue(np.all(inflation > 1.0)) + # Recorded so a geometry change that flattens the effect is visible + # rather than merely still-passing: this array is 4 anchors on a 10 m + # square with the agent inside, and the cost is a few percent. + self.assertAlmostEqual(float(np.median(inflation)), 1.026, places=2) + + def test_the_measured_cost_is_the_state_vector_and_not_the_noise(self): + """Measured over a sample big enough to resolve 2.6%, then mutated. + + **Both halves of the bound were measured.** Against the noise: over ten + seeds at these settings the ratio ran 1.0115 to 1.0376, mean 1.0257, + std 0.0070, so a floor of 1.010 on the mean of four seeds sits about + four standard errors below it. Against the defect: giving the two-way + arm the clock state as well -- the one mutation that makes this test + meaningless -- returns **exactly** 1.0000, because a common bias is + absorbed whole into `c*dt` and the position estimates become + bit-identical. The ceiling catches the opposite error, a two-way arm + accidentally handed less noise than the one-way arm. + """ + ratios = [ + (lambda pair: pair[0] / pair[1])( + _paired_medians(STAT_SIGMA_M, STAT_POINTS, seed) + ) + for seed in STAT_SEEDS + ] + measured = float(np.mean(ratios)) + + self.assertGreater(measured, 1.010) + self.assertLess(measured, 1.045) + + one_way, two_way = _paired_medians( + STAT_SIGMA_M, 300, STAT_SEEDS[0], rtt_gets_the_clock_state=True + ) + self.assertAlmostEqual(one_way / two_way, 1.0, places=9) + + def test_the_bias_is_unobservable_to_position_in_the_three_state_solve(self): + """Why the one-way row can carry a 1.5 m bias and still be comparable. + + The bias goes entirely into `c*dt`, so the one-way *position* is the + same whether or not it was injected, and the clock estimate moves by + exactly the injected amount. That is the claim the comparison rests + on: the two rows differ in their state vector and in nothing else, and + the 1.5 m is not a handicap loaded onto TOA. + + **What is exact here is the difference, not the estimate.** A first + draft asserted the recovered bias was 1.5 m on noisy data and read + 1.4642 -- the solve absorbs the injected bias *and* whatever common + part the range noise happens to have, which at sigma = 0.2 m over four + anchors is a few centimetres. The example's own docstring already said + "+1.500 m on **noiseless** data" and it means it; both cases are + separated below rather than sharing one tolerance. + """ + anchors, truth = generate_scenario(seed=42) + rng = np.random.default_rng(5) + clean = np.linalg.norm(truth[:, None, :] - anchors[None, :, :], axis=2) + noisy = clean + rng.standard_normal(clean.shape) * 0.2 + seed_3 = np.concatenate([INLINE_SEED, [0.0]]) + + # Noiseless: the bias comes back as itself. + for row, point in zip(clean, truth, strict=True): + est, bias, _ = toa_solve_with_clock_bias( + anchors, row + CLOCK_BIAS_M, seed_3 + ) + self.assertAlmostEqual(bias, CLOCK_BIAS_M, places=6) + self.assertLess(float(np.linalg.norm(est - point)), 1e-6) + + # Noisy: the position does not move and the clock absorbs the whole + # of the shift, both exactly. + worst_position, worst_bias = 0.0, 0.0 + for row in noisy: + with_bias, bias_hi, _ = toa_solve_with_clock_bias( + anchors, row + CLOCK_BIAS_M, seed_3 + ) + without, bias_lo, _ = toa_solve_with_clock_bias(anchors, row, seed_3) + worst_position = max( + worst_position, float(np.linalg.norm(with_bias - without)) + ) + worst_bias = max(worst_bias, abs((bias_hi - bias_lo) - CLOCK_BIAS_M)) + + self.assertLess(worst_position, 1e-9) + self.assertLess(worst_bias, 1e-9) + + +class WhatTheShippedSweepActuallyReports(unittest.TestCase): + """Pins on the committed table and figure, labelled as realisation pins.""" + + def test_rtt_is_at_or_below_toa_at_every_noisy_level_of_this_seed(self): + """True at seed 42; **not** a law, and the docstring is the point. + + The effect is 2.6% and a 50-point median carries roughly 7% of + sampling noise, so this ordering is not something one noise level can + establish -- over 40 repeats per sigma the ratio of medians ranged + 0.87 to 1.27. It holds at every noisy level here, which is what the + README table and the figure show, so it is pinned as a regression + guard on the shipped numbers. + `test_the_measured_cost_is_the_state_vector_and_not_the_noise` is + where the underlying claim is actually established. + """ + schedules, results = _sweep() + ladder = {name: schedule for name, schedule, _ in schedules}["RTT"] + + for level, sigma in enumerate(ladder): + if sigma == 0: + continue + with self.subTest(level=level + 1, sigma=sigma): + self.assertLessEqual( + results["RTT"][level].median_m, + results["TOA"][level].median_m, + ) + + def test_both_range_series_are_exact_on_noiseless_measurements(self): + """The one row with a known right answer, and neither wins it. + + At sigma = 0 both medians are roundoff -- 3.5e-10 m and 9.9e-10 m -- + so the two-way series is in fact the *larger* of the two, by 0.6 + nanometres. Nothing is measured there, which is why the example prints + no ratio for level 1 and why the ordering test above skips it. An + earlier draft of that print guarded on `median > 0` and duly reported + "0.35x" as the cost of the clock state. + """ + _, results = _sweep() + + self.assertLess(results["TOA"][0].median_m, STALL_M) + self.assertLess(results["RTT"][0].median_m, STALL_M) + + def test_failures_are_accounted_by_solve_batch(self): + """Median over every fix, failure count beside it -- not an RMSE. + + `solve_batch`'s four conditions: raised, reported `converged=False`, + never left the seed, or landed over 100 m away. The two-way series has + to go through it like the other four, so that a level where it stops + solving reports as a failure rather than as an accuracy over the + survivors. + """ + _, results = _sweep() + + for level, outcome in enumerate(results["RTT"]): + with self.subTest(level=level + 1): + self.assertEqual(outcome.n, 50) + self.assertEqual(len(outcome.errors), 50) + self.assertEqual(outcome.solved.shape, (50,)) + self.assertEqual(outcome.n_failed, int((~outcome.solved).sum())) + self.assertGreaterEqual(outcome.n - outcome.n_failed, 45) + + # The one failure in the sweep is a step-tolerance stop, not a + # divergence: its error is an ordinary sub-metre fix. `converged` here + # means "finished inside max_iters", which is why the failure count + # and the median are reported separately rather than one filtering the + # other. + for outcome in results["RTT"]: + unsolved = outcome.errors[~outcome.solved] + self.assertTrue(np.all(np.isfinite(unsolved))) + self.assertTrue(np.all(unsolved < 100.0)) + + def test_running_the_sweep_twice_gives_the_same_numbers(self): + """An unseeded series cannot regenerate its own committed figure.""" + schedules_a, results_a = _sweep() + with contextlib.redirect_stdout(io.StringIO()): + schedules_b, results_b = run_inline_comparison() + + self.assertEqual(schedules_a, schedules_b) + for method in results_a: + for level, (a, b) in enumerate( + zip(results_a[method], results_b[method], strict=True) + ): + with self.subTest(method=method, level=level + 1): + np.testing.assert_array_equal(a.errors, b.errors) + np.testing.assert_array_equal(a.solved, b.solved) + + +if __name__ == "__main__": + unittest.main()