diff --git a/ch4_rf_point_positioning/README.md b/ch4_rf_point_positioning/README.md
index a338c3d..fc6ca72 100644
--- a/ch4_rf_point_positioning/README.md
+++ b/ch4_rf_point_positioning/README.md
@@ -15,6 +15,9 @@ python -m ch4_rf_point_positioning.example_aoa_positioning
# Dilution of precision: walk away from the anchors (add --animate)
python -m ch4_rf_point_positioning.example_dop_geometry
+# Sweep the initial guess over the floor, under two residual parameterisations
+python -m ch4_rf_point_positioning.example_initial_guess_basin
+
# Run with pre-generated datasets
python -m ch4_rf_point_positioning.example_comparison --data ch4_rf_2d_square
python -m ch4_rf_point_positioning.example_comparison --data ch4_rf_2d_nlos
@@ -58,6 +61,46 @@ Two things are measured, not asserted:
is fixed, yet the position error grows **6×** across the walk — purely
because the anchors are in the wrong place.
+## The initial guess, and why it is usually the wrong thing to blame (Section 4.4)
+
+| Figure | Built by | Size |
+|--------|----------|------|
+| `ch4_initial_guess_basin.{svg,pdf,png}` | `example_initial_guess_basin.py` | — |
+
+When an iterative solve fails, the reflex is to blame the starting point. This example holds
+the geometry at the well-behaved square, fixes one target, sets the measurement noise to
+**zero**, and sweeps the initial guess over 1681 seeds — twice, changing nothing but the
+space the residual is formed in.
+
+| | |
+|---|---|
+| `residual="tan"` | `z = tan(ψ)`, Eq. (4.64) written literally |
+| `residual="angle"` | `wrap(ψ − atan2(ΔE, ΔN))` — the default |
+
+The tan form carries two defects no starting point repairs. `tan` has period π, so an anchor
+ahead and an anchor behind give the same measurement; and as the estimate runs to infinity
+every bearing converges, so the tan residuals *shrink* on the way out. Infinity is an
+attractor, and the iteration arrives there reporting success — one traced seed at (−4.5,
+−2.0) walked to **9.4 × 10¹¹ m in 18 iterations with `converged=True`**.
+
+Measured over the sweep:
+
+| | `tan(ψ)` | `wrap(angle)` |
+|---|---:|---:|
+| seeds that fail | 785 / 1681 | 341 / 1681 |
+| **quiet:** stalled at the seed, or stopped somewhere plausible | **263** | **0** |
+| loud: walked off past 100 m | 522 | 341 |
+| failures that still reported `converged=True` | 305 | 196 |
+
+So the honest headline is 2.3× fewer failures, and a sharper claim underneath it: the
+wrapped-angle form removes the **quiet** class completely — the failures that look like
+answers — while a seed far outside the room still walks off under either parameterisation.
+
+**Fixing the residual makes the solver honest, not safe.** The convergence flag is not a
+check either way, which is why `core.rf.solve_batch`'s four conditions are not optional. Two
+questions catch both defects, and they are worth asking of any residual: *is it bounded?* and
+*does the cost stay large when the estimate is far wrong?*
+
## 📂 Dataset Connection
| Example Script | Dataset | Description |
@@ -809,7 +852,7 @@ drift from the code.
```mermaid
flowchart TB
D["optional input data/sim/ch4_rf_2d_linear data/sim/ch4_rf_2d_nlos data/sim/ch4_rf_2d_optimal data/sim/ch4_rf_2d_square only example_comparison reads it"]
- E["ch4_rf_point_positioning/example_*.py 5 runnable demos"]
+ E["ch4_rf_point_positioning/example_*.py 6 runnable demos"]
C["the reusable library core/eval/ · core/rf/ · core/utils/"]
F["ch4_rf_point_positioning/figs/ svg + pdf + png"]
D -. "--data" .-> E
@@ -822,6 +865,7 @@ flowchart TB
| `example_aoa_positioning` | `core.eval`, `core.rf` | — |
| `example_comparison` | `core.eval`, `core.rf`, `core.utils` | `ch4_rf_2d_linear`, `ch4_rf_2d_nlos`, `ch4_rf_2d_optimal`, `ch4_rf_2d_square` |
| `example_dop_geometry` | `core.eval`, `core.rf` | — |
+| `example_initial_guess_basin` | `core.eval`, `core.rf` | — |
| `example_tdoa_positioning` | `core.eval`, `core.rf` | — |
| `example_toa_positioning` | `core.eval`, `core.rf` | — |
@@ -836,11 +880,13 @@ ch4_rf_point_positioning/
├── example_tdoa_positioning.py # TDOA positioning demo
├── example_aoa_positioning.py # AOA positioning demo
├── example_dop_geometry.py # Sec. 4.5: how anchor geometry amplifies noise
+├── example_initial_guess_basin.py # Sec. 4.4: the basin is the residual's, not the seed's
├── example_comparison.py # Compare all RF methods
└── figs/ # Generated figures
├── toa_positioning_example.png # TOA positioning geometry and convergence
├── ch4_rf_comparison.png # Comprehensive RF methods comparison
├── ch4_aoa_geometry.png # AOA positioning geometry (ENU convention)
+ ├── ch4_initial_guess_basin.png # Seed sweep under two residual parameterisations
├── tdoa_covariance_matrix.png # TDOA covariance structure (Eq. 4.42)
└── closed_form_comparison.png # Fang/Chan vs iterative solvers
diff --git a/ch4_rf_point_positioning/example_initial_guess_basin.py b/ch4_rf_point_positioning/example_initial_guess_basin.py
new file mode 100644
index 0000000..26e33de
--- /dev/null
+++ b/ch4_rf_point_positioning/example_initial_guess_basin.py
@@ -0,0 +1,331 @@
+"""Where an iterative solve starts, and why that is usually the wrong thing to blame.
+
+Chapter 4, Section 4.4. A Gauss-Newton positioning solve needs an initial guess, and when it
+fails the reflex is to blame the guess. This example sweeps the guess over the whole floor,
+twice, changing nothing but the space the residual is formed in.
+
+The measurements here are NOISELESS, so every error on this page is the solver.
+
+Two parameterisations of one measurement model:
+
+ residual="tan" z = tan(psi), Eq. (4.64) written literally
+ residual="angle" wrap(psi_measured - atan2(dE, dN)) [the default]
+
+The tan form carries two defects that no starting point repairs. `tan` has period pi, so an
+anchor ahead and an anchor behind produce the same measurement and the residual cannot tell
+them apart. And as the estimate runs to infinity every anchor tends to the same bearing, so
+the tan residuals *shrink* on the way out -- infinity is an attractor, and Gauss-Newton
+arrives there reporting success. The fourth panel traces one such run.
+
+Both questions to ask of any residual are answered by the wrapped-angle form and failed by
+the tan form: **is it bounded?** and **does the cost stay large when the estimate is far
+wrong?**
+
+WHAT THE SWEEP ACTUALLY SHOWS -- measured, and not what this example was written expecting.
+Over 1681 seeds the wrapped-angle form fails 341 times against tan's 785, so the honest
+headline is 2.3x, not "the basin disappears". What it removes is the QUIET class, and that it
+removes completely: seeds that stall at the guess (82 -> 0) and seeds that stop somewhere
+plausible but wrong (181 -> 0). What survives is the loud one -- a seed far outside the room
+still walks off under either parameterisation, and 196 of those still set converged=True.
+
+So the parameterisation makes the solver *honest*, not *safe*. Fixing the residual is worth
+doing and is not a substitute for the four-condition failure test in `core.rf.solve_batch`:
+the convergence flag is not a check, whichever residual you form.
+
+Run:
+ python -m ch4_rf_point_positioning.example_initial_guess_basin
+
+Author: Li-Ta Hsu
+References: Chapter 4, Eqs. (4.63)-(4.65), (4.66)-(4.70). The behaviour pinned here is the
+ same one asserted in tests/ch4_rf_point_positioning/test_aoa_initialisation_basin.py;
+ this example is its picture.
+"""
+
+import argparse
+import sys
+from pathlib import Path
+
+import matplotlib
+import numpy as np
+
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+from matplotlib.colors import ListedColormap
+from matplotlib.patches import Patch
+
+# Run as a script, sys.path[0] is THIS directory, so `core` resolves to whatever is
+# installed -- another clone, a stale editable install -- or fails outright on a fresh
+# one. See issue #86.
+sys.path.insert(0, str(Path(__file__).resolve().parent.parent))
+
+from core.eval import resolve_figs_dir, save_figure, show_figures_if_requested
+from core.rf import AOAPositioner, aoa_azimuth, solve_batch
+from core.rf.positioning import STALL_M
+
+FIGS_DIR = Path(__file__).parent / "figs"
+
+#: Four anchors on a square room -- a geometry that is not the problem here. The collinear
+#: array in `example_comparison` fails for reasons of geometry; this one does not.
+ANCHORS = np.array([[0.0, 0.0], [10.0, 0.0], [10.0, 10.0], [0.0, 10.0]])
+
+#: One target, off-centre AND off the seed lattice. At (3.0, 7.0) one seed of the 1681 was
+#: the answer exactly, so the solver never moved and `solve_batch` scored it -- correctly, by
+#: its own definition -- as a stall. That is a classification artifact rather than a failure,
+#: and it is also the rule against seeding a solver with the ground truth, arrived at by
+#: accident. Off-lattice makes the collision impossible instead of subtracting it later.
+TRUTH = np.array([3.2, 6.8])
+
+#: The seed grid extends well outside the room: a cold start does not know where it is.
+GRID_MIN, GRID_MAX, GRID_STEP = -5.0, 15.0, 0.5
+
+#: Noiseless measurements converge to ~1e-8 m, so a millimetre is a generous "solved".
+SOLVED_M = 1e-3
+
+#: Outcome codes, in the order they are drawn.
+SOLVED, STALLED, WRONG, DIVERGED, RAISED = 0, 1, 2, 3, 4
+LABELS = {
+ SOLVED: "solved",
+ STALLED: "stalled at the seed",
+ WRONG: "converged, wrong place",
+ DIVERGED: "diverged (>100 m)",
+ RAISED: "solver raised",
+}
+COLOURS = ["#2E7D32", "#F9A825", "#C62828", "#6A1B9A", "#455A64"]
+
+#: Separate palette for the two PARAMETERISATIONS. Reusing the outcome colours for the bars
+#: put "tan" in the green of "solved" and "angle" in the purple of "diverged" on the same
+#: figure -- two meanings for one colour, caught by looking at the render.
+SERIES = ["#37474F", "#0277BD"]
+
+
+def measurements(truth=TRUTH):
+ """Noiseless AOA azimuths from every anchor to the target."""
+ return np.array([aoa_azimuth(a, truth) for a in ANCHORS])
+
+
+def seed_grid():
+ """The initial guesses to try, as a meshgrid and as a flat (N, 2) list."""
+ axis = np.arange(GRID_MIN, GRID_MAX + GRID_STEP / 2, GRID_STEP)
+ xx, yy = np.meshgrid(axis, axis)
+ seeds = np.column_stack([xx.ravel(), yy.ravel()])
+ assert np.min(np.linalg.norm(seeds - TRUTH, axis=1)) > STALL_M, (
+ "a seed coincides with the target: it cannot move, and would be scored a stall")
+ return axis, xx, yy, seeds
+
+
+def sweep(residual, verbose=True):
+ """Solve the same fix from every seed and classify each outcome.
+
+ Classification goes through `core.rf.solve_batch` rather than being re-derived here:
+ it is the four failure conditions applied once (raised / converged=False / never left
+ the seed / landed beyond the divergence threshold), and re-implementing them is the
+ recurring bug this repository has already paid for.
+ """
+ axis, xx, yy, seeds = seed_grid()
+ meas = measurements()
+ truth = TRUTH[None, :]
+ solver = AOAPositioner(ANCHORS)
+
+ codes = np.empty(len(seeds), dtype=int)
+ errors = np.empty(len(seeds))
+ claimed = np.zeros(len(seeds), dtype=bool)
+ for i, seed in enumerate(seeds):
+ out = solve_batch(solver, meas[None, :], seed, truth, residual=residual)
+ err = float(out.errors[0])
+ errors[i] = err
+ claimed[i] = bool(out.converged[0])
+ if np.isnan(err):
+ codes[i] = RAISED
+ elif bool(out.stalled[0]):
+ codes[i] = STALLED
+ elif err > out.divergence_m:
+ codes[i] = DIVERGED
+ elif err > SOLVED_M:
+ codes[i] = WRONG
+ else:
+ codes[i] = SOLVED
+
+ result = {
+ "residual": residual,
+ "axis": axis,
+ "xx": xx,
+ "yy": yy,
+ "seeds": seeds,
+ "codes": codes.reshape(xx.shape),
+ "errors": errors.reshape(xx.shape),
+ "claimed": claimed.reshape(xx.shape),
+ # the failures that lied: wrong answer, convergence flag set
+ "silent": int(np.sum((codes != SOLVED) & (codes != RAISED) & claimed)),
+ "counts": {c: int(np.sum(codes == c)) for c in LABELS},
+ "n": len(seeds),
+ }
+ if verbose:
+ n_bad = result["n"] - result["counts"][SOLVED]
+ print(f"\n residual={residual!r}: {n_bad}/{result['n']} seeds failed to reach "
+ f"the target")
+ for c, name in LABELS.items():
+ if result["counts"][c]:
+ print(f" {name:<24} {result['counts'][c]:>5}")
+ finite = errors[np.isfinite(errors)]
+ print(f" median error {np.median(finite):>10.2e} m")
+ print(f" worst error {finite.max():>10.2e} m")
+ print(f" of those failures, {result['silent']} reported converged=True")
+ return result
+
+
+def trace_worst(result):
+ """Re-solve the worst SILENT failure and return its iterate path.
+
+ Deliberately not the worst failure outright: the largest error in this sweep reports
+ `converged=False`, which is the solver behaving correctly and is not the thing worth a
+ panel. The interesting run is the furthest one that still set the flag.
+
+ `AOAPositioner.solve` returns the iterates in `info["history"]`, so the walk out to
+ infinity can be drawn rather than described.
+ """
+ flat = result["errors"].ravel()
+ finite = np.where(np.isfinite(flat), flat, -np.inf)
+ lying = result["claimed"].ravel() & (result["codes"].ravel() == DIVERGED)
+ ranked = np.where(lying, finite, -np.inf)
+ if not np.isfinite(ranked).any(): # no silent failure: fall back to the worst
+ ranked = finite
+ seed = result["seeds"][int(np.argmax(ranked))]
+ _, info = AOAPositioner(ANCHORS).solve(
+ measurements(), initial_guess=seed, residual=result["residual"]
+ )
+ return seed, np.asarray(info["history"]), bool(info["converged"])
+
+
+def plot_basin(ax, result):
+ """One basin map: every seed coloured by what the solve did from there."""
+ cmap = ListedColormap(COLOURS)
+ ax.pcolormesh(result["xx"], result["yy"], result["codes"], cmap=cmap,
+ vmin=-0.5, vmax=len(COLOURS) - 0.5, shading="auto")
+ ax.plot(ANCHORS[:, 0], ANCHORS[:, 1], "k^", ms=9, label="anchors")
+ ax.plot(*TRUTH, "w*", ms=18, mec="k", mew=1.2, label="target")
+ solved = result["counts"][SOLVED]
+ ax.set_title(f'residual="{result["residual"]}" '
+ f'{result["n"] - solved}/{result["n"]} seeds fail',
+ fontsize=11)
+ ax.set_xlabel("initial guess x (m)")
+ ax.set_ylabel("initial guess y (m)")
+ ax.set_aspect("equal")
+
+
+def plot_failure_rates(ax, results):
+ """Failure modes side by side.
+
+ Its own panel, because an accuracy figure cannot say "this did not work" -- and a
+ method that failed everywhere must never be drawn as a zero-height bar.
+ """
+ codes = [c for c in LABELS if any(r["counts"][c] for r in results)]
+ width = 0.8 / len(results)
+ for k, r in enumerate(results):
+ pos = np.arange(len(codes)) + k * width - 0.4 + width / 2
+ pct = [100 * r["counts"][c] / r["n"] for c in codes]
+ bars = ax.bar(pos, pct, width, label=f'residual="{r["residual"]}"',
+ color=SERIES[k], edgecolor="black", linewidth=0.5)
+ for b, v, c in zip(bars, pct, codes, strict=True):
+ ax.text(b.get_x() + b.get_width() / 2, max(v, 0) + 1.5,
+ f"{r['counts'][c]}", ha="center", fontsize=9)
+ ax.set_xticks(np.arange(len(codes)))
+ ax.set_xticklabels([LABELS[c].replace(" ", "\n", 1) for c in codes], fontsize=9)
+ ax.set_ylabel("% of seeds")
+ ax.set_ylim(0, 108)
+ ax.set_title("Same measurements, same seeds, one line of difference", fontsize=11)
+ ax.legend(fontsize=9)
+
+
+def plot_trace(ax, seed, history, converged):
+ """The walk to infinity, with the flag it set on arrival."""
+ d = np.linalg.norm(history - TRUTH, axis=1)
+ ax.semilogy(np.arange(len(d)), np.maximum(d, 1e-12), "o-", color="#6A1B9A", ms=4)
+ ax.axhline(100.0, color="#C62828", ls="--", lw=1,
+ label="divergence threshold, 100 m")
+ ax.set_xlabel("Gauss-Newton iteration")
+ ax.set_ylabel("distance from the target (m)")
+ ax.set_title(f'seed ({seed[0]:.1f}, {seed[1]:.1f}) -> {d[-1]:.1e} m, '
+ f'converged={converged}', fontsize=11)
+ ax.grid(True, which="both", alpha=0.3)
+ ax.legend(fontsize=9, loc="lower right")
+
+
+def plot_summary(results, trace):
+ """The whole story on one figure."""
+ fig, axes = plt.subplots(2, 2, figsize=(13.0, 10.4))
+ plot_basin(axes[0, 0], results[0])
+ plot_basin(axes[0, 1], results[1])
+ plot_failure_rates(axes[1, 0], results)
+ plot_trace(axes[1, 1], *trace)
+
+ # only the outcomes that actually occurred: an unused "solver raised" patch sat in a
+ # grey almost identical to the tan bars, which is a second meaning for one colour
+ seen = [c for c in LABELS if any(r["counts"][c] for r in results)]
+ handles = [Patch(facecolor=COLOURS[c], edgecolor="black", label=LABELS[c])
+ for c in seen]
+ handles += [plt.Line2D([], [], color="k", marker="^", ls="", label="anchor"),
+ plt.Line2D([], [], color="w", marker="*", mec="k", ls="", ms=12,
+ label="target")]
+ fig.legend(handles=handles, loc="lower center", ncol=len(handles), frameon=False, fontsize=9)
+ fig.suptitle("An initial-guess problem that is not about the initial guess\n"
+ "AOA, four anchors, zero measurement noise", fontsize=13)
+ fig.tight_layout(rect=(0, 0.045, 1, 0.96))
+ return fig
+
+
+def main() -> None:
+ """Sweep both parameterisations and write the figure."""
+ parser = argparse.ArgumentParser(
+ description="Initial-guess basin for AOA positioning (Chapter 4)")
+ parser.add_argument("--out-dir", default=str(FIGS_DIR),
+ help="Output directory for figures")
+ args = parser.parse_args()
+
+ axis, *_ = seed_grid()
+ print("=" * 70)
+ print("Chapter 4: the initial guess is not usually the problem")
+ print("=" * 70)
+ print(f" {len(axis)}x{len(axis)} seeds over [{GRID_MIN:.0f}, {GRID_MAX:.0f}] m, "
+ f"target at ({TRUTH[0]:.1f}, {TRUTH[1]:.1f}), zero measurement noise")
+
+ tan_r = sweep("tan")
+ ang_r = sweep("angle")
+
+ bad_tan = tan_r["n"] - tan_r["counts"][SOLVED]
+ bad_ang = ang_r["n"] - ang_r["counts"][SOLVED]
+ quiet_tan = tan_r["counts"][STALLED] + tan_r["counts"][WRONG]
+ quiet_ang = ang_r["counts"][STALLED] + ang_r["counts"][WRONG]
+
+ print("\n " + "-" * 66)
+ print(f" {'':30}{'tan(psi)':>12}{'wrap(angle)':>14}")
+ print(f" {'seeds that fail':30}{bad_tan:>12}{bad_ang:>14}")
+ print(f" {' quiet: stalled or plausible':30}{quiet_tan:>12}{quiet_ang:>14}")
+ print(f" {' loud: walked off past 100 m':30}"
+ f"{tan_r['counts'][DIVERGED]:>12}{ang_r['counts'][DIVERGED]:>14}")
+ print(f" {'failures claiming converged':30}"
+ f"{tan_r['silent']:>12}{ang_r['silent']:>14}")
+ print(f"\n Overall {bad_tan / max(bad_ang, 1):.1f}x fewer failures -- but the honest "
+ f"statement is narrower:")
+ print(f" the wrapped-angle form removes the QUIET failures ({quiet_tan} -> "
+ f"{quiet_ang}), the ones")
+ print(" that look like answers. Seeds far outside the room still walk off under both,")
+ print(f" and {ang_r['silent']} of those still set converged=True. The residual fix "
+ f"makes the")
+ print(" solver honest, not safe -- the four-condition test is still what catches it.")
+
+ seed, history, converged = trace_worst(tan_r)
+ print(f"\n Worst SILENT tan failure: seed ({seed[0]:.1f}, {seed[1]:.1f}) walked to "
+ f"{np.linalg.norm(history[-1] - TRUTH):.2e} m")
+ print(f" in {len(history) - 1} iterations and reported converged={converged}.")
+
+ paths = save_figure(plot_summary([tan_r, ang_r], (seed, history, converged)),
+ args.out_dir, "ch4_initial_guess_basin")
+ print(f"\n saved ch4_initial_guess_basin: "
+ f"{', '.join(p.suffix.lstrip('.') for p in paths)}")
+ plt.close("all")
+ print(f"Figures written to {resolve_figs_dir(args.out_dir)}")
+ show_figures_if_requested()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.pdf b/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.pdf
new file mode 100644
index 0000000..a003379
Binary files /dev/null and b/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.pdf differ
diff --git a/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.png b/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.png
new file mode 100644
index 0000000..97b4014
Binary files /dev/null and b/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.png differ
diff --git a/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.svg b/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.svg
new file mode 100644
index 0000000..95af121
--- /dev/null
+++ b/ch4_rf_point_positioning/figs/ch4_initial_guess_basin.svg
@@ -0,0 +1,23321 @@
+
+
+
diff --git a/notebooks/ch4_rf_positioning.ipynb b/notebooks/ch4_rf_positioning.ipynb
index 9c125b9..ca7d3d8 100644
--- a/notebooks/ch4_rf_positioning.ipynb
+++ b/notebooks/ch4_rf_positioning.ipynb
@@ -557,7 +557,16 @@
"\n",
"## 6.1 Monte Carlo Simulation\n",
"\n",
- "Let's compare all methods under identical noise conditions across multiple trials.\n"
+ "All methods, identical noise, 100 trials — **with the failures kept.**\n",
+ "\n",
+ "An iterative solve can fail four ways and only one of them is loud: it raises, it reports\n",
+ "`converged=False`, it **never leaves the initial guess** (Gauss-Newton calls a zero step\n",
+ "convergence), or it walks off somewhere absurd and **still sets `converged=True`**. Counting\n",
+ "only the first two is how a method comes to look better than it is — the silent nonsense gets\n",
+ "averaged in and the honest refusals vanish from the denominator.\n",
+ "\n",
+ "`core.rf.solve_batch` applies all four and hands back the errors with a `solved` mask beside\n",
+ "them, so the median and the failure count can be read separately.\n"
]
},
{
@@ -566,70 +575,46 @@
"metadata": {},
"outputs": [],
"source": [
- "# Monte Carlo comparison of RF positioning methods\n",
- "print(\"=\"*70)\n",
- "print(\"Monte Carlo Comparison: 100 Trials\")\n",
- "print(\"=\"*70)\n",
+ "# Monte Carlo comparison of RF positioning methods, failures kept\n",
+ "from core.rf import solve_batch\n",
"\n",
- "np.random.seed(42)\n",
- "n_trials = 100\n",
+ "print(\"=\" * 70)\n",
+ "print(\"Monte Carlo Comparison: 100 trials\")\n",
+ "print(\"=\" * 70)\n",
"\n",
- "# Noise levels\n",
- "range_noise_std = 0.10 # 10 cm for TOA/TDOA\n",
+ "rng = np.random.default_rng(42)\n",
+ "n_trials = 100\n",
+ "range_noise_std = 0.10 # 10 cm for TOA/TDOA\n",
"aoa_noise_std = np.deg2rad(3.0) # 3 degrees for AOA\n",
"\n",
- "# Results storage\n",
- "results = {\n",
- " 'TOA': [],\n",
- " 'TDOA': [],\n",
- " 'AOA': [],\n",
+ "truth = np.tile(true_pos, (n_trials, 1))\n",
+ "seed = np.mean(anchors, axis=0) # the beacon centroid: a cold start, never the answer\n",
+ "\n",
+ "batches = {\n",
+ " 'TOA': (positioner,\n",
+ " true_ranges + rng.normal(0, range_noise_std, (n_trials, len(anchors)))),\n",
+ " 'TDOA': (tdoa_positioner,\n",
+ " tdoa_measurements\n",
+ " + rng.normal(0, range_noise_std, (n_trials, len(tdoa_measurements)))),\n",
+ " 'AOA': (aoa_positioner,\n",
+ " true_aoa + rng.normal(0, aoa_noise_std, (n_trials, len(anchors)))),\n",
"}\n",
"\n",
- "for trial in range(n_trials):\n",
- " # Generate noisy measurements\n",
- " noisy_ranges = true_ranges + np.random.randn(len(anchors)) * range_noise_std\n",
- " noisy_tdoa = tdoa_measurements + np.random.randn(len(tdoa_measurements)) * range_noise_std\n",
- " noisy_aoa = true_aoa + np.random.randn(len(anchors)) * aoa_noise_std\n",
- " \n",
- " # TOA\n",
- " try:\n",
- " est, info = positioner.solve(noisy_ranges, initial_guess=np.mean(anchors, axis=0))\n",
- " if info['converged']:\n",
- " results['TOA'].append(np.linalg.norm(est - true_pos))\n",
- " except:\n",
- " pass\n",
- " \n",
- " # TDOA\n",
- " try:\n",
- " est, info = tdoa_positioner.solve(noisy_tdoa, initial_guess=np.mean(anchors, axis=0))\n",
- " if info['converged']:\n",
- " results['TDOA'].append(np.linalg.norm(est - true_pos))\n",
- " except:\n",
- " pass\n",
- " \n",
- " # AOA\n",
- " try:\n",
- " est, info = aoa_positioner.solve(noisy_aoa, initial_guess=np.mean(anchors, axis=0))\n",
- " if info['converged']:\n",
- " results['AOA'].append(np.linalg.norm(est - true_pos))\n",
- " except:\n",
- " pass\n",
- "\n",
- "# Print summary\n",
- "print(f\"\\n📊 Results Summary (range noise={range_noise_std*100:.0f}cm, AOA noise=3°):\\n\")\n",
- "print(f\"{'Method':<8} {'RMSE (m)':<10} {'Mean (m)':<10} {'Max (m)':<10} {'Success':<10}\")\n",
- "print(\"-\" * 48)\n",
- "\n",
- "for method in ['TOA', 'TDOA', 'AOA']:\n",
- " errors = np.array(results[method])\n",
- " if len(errors) > 0:\n",
- " rmse = np.sqrt(np.mean(errors**2))\n",
- " mean_err = np.mean(errors)\n",
- " max_err = np.max(errors)\n",
- " success = len(errors) / n_trials * 100\n",
- " print(f\"{method:<8} {rmse:<10.4f} {mean_err:<10.4f} {max_err:<10.4f} {success:<10.0f}%\")\n",
- " else:\n",
- " print(f\"{method:<8} {'N/A':<10} {'N/A':<10} {'N/A':<10} 0%\")\n"
+ "outcomes = {name: solve_batch(solver, meas, seed, truth)\n",
+ " for name, (solver, meas) in batches.items()}\n",
+ "\n",
+ "print(f\"\\nrange noise {range_noise_std * 100:.0f} cm, AOA noise 3 deg\")\n",
+ "print(\"A fix has FAILED if it raised, reported converged=False, never left the\")\n",
+ "print(\"initial guess, or landed more than 100 m from the truth.\\n\")\n",
+ "print(f\"{'Method':<8}{'median (m)':>12}{'mean (m)':>11}{'worst (m)':>11}{'failed':>11}\")\n",
+ "print(\"-\" * 53)\n",
+ "for name, out in outcomes.items():\n",
+ " print(f\"{name:<8}{out.median_m:>12.3f}{out.mean_solved_m:>11.3f}\"\n",
+ " f\"{out.max_solved_m:>11.3f}{out.n_failed:>8}/{out.n}\")\n",
+ "\n",
+ "print(\"\\nThe median is over every fix that returned a number; mean and worst are\")\n",
+ "print(\"over the ones that solved. Reporting a bare mean across a divergence makes\")\n",
+ "print(\"the number a property of that one outlier rather than of the method.\")\n"
]
},
{
@@ -639,45 +624,178 @@
"outputs": [],
"source": [
"# Visualize comparison results\n",
- "fig, axes = plt.subplots(1, 2, figsize=(14, 5))\n",
- "\n",
- "# Box plot\n",
- "ax1 = axes[0]\n",
- "data_to_plot = [results['TOA'], results['TDOA'], results['AOA']]\n",
- "labels = ['TOA', 'TDOA', 'AOA']\n",
+ "fig, axes = plt.subplots(1, 3, figsize=(16, 4.6))\n",
+ "methods = ['TOA', 'TDOA', 'AOA']\n",
"colors = ['#3498db', '#e74c3c', '#2ecc71']\n",
"\n",
- "bp = ax1.boxplot(data_to_plot, tick_labels=labels, patch_artist=True)\n",
+ "# Errors of the fixes that SOLVED -- said so on the axis, because the ones that did not\n",
+ "# are not in this panel\n",
+ "solved_errors = [outcomes[m].errors[outcomes[m].solved] for m in methods]\n",
+ "\n",
+ "ax1 = axes[0]\n",
+ "bp = ax1.boxplot(solved_errors, tick_labels=methods, patch_artist=True)\n",
"for patch, color in zip(bp['boxes'], colors):\n",
" patch.set_facecolor(color)\n",
" patch.set_alpha(0.7)\n",
- "\n",
- "ax1.set_ylabel('Position Error (m)', fontsize=12)\n",
- "ax1.set_title('Error Distribution by Method', fontsize=14, fontweight='bold')\n",
+ "ax1.set_ylabel('Position error (m)', fontsize=12)\n",
+ "ax1.set_title('Error distribution — solved fixes only', fontsize=13, fontweight='bold')\n",
"ax1.grid(True, alpha=0.3, axis='y')\n",
"\n",
- "# CDF plot\n",
"ax2 = axes[1]\n",
- "for method, color in zip(['TOA', 'TDOA', 'AOA'], colors):\n",
- " errors = np.sort(results[method])\n",
+ "for method, color in zip(methods, colors):\n",
+ " errors = np.sort(outcomes[method].errors[outcomes[method].solved])\n",
" cdf = np.arange(1, len(errors) + 1) / len(errors)\n",
" ax2.plot(errors, cdf, label=method, color=color, linewidth=2)\n",
- "\n",
"ax2.axhline(y=0.95, color='gray', linestyle='--', alpha=0.7, label='95th percentile')\n",
- "ax2.set_xlabel('Position Error (m)', fontsize=12)\n",
+ "ax2.set_xlabel('Position error (m)', fontsize=12)\n",
"ax2.set_ylabel('CDF', fontsize=12)\n",
- "ax2.set_title('Cumulative Distribution Function', fontsize=14, fontweight='bold')\n",
+ "ax2.set_title('Cumulative distribution', fontsize=13, fontweight='bold')\n",
"ax2.legend(loc='lower right')\n",
"ax2.grid(True, alpha=0.3)\n",
"ax2.set_xlim(0, None)\n",
"\n",
+ "# Its own panel: an accuracy plot cannot say \"this did not work\", and a method that failed\n",
+ "# everywhere would otherwise draw as a zero-height bar, which reads as the best result here\n",
+ "ax3 = axes[2]\n",
+ "failed = [100 * outcomes[m].n_failed / outcomes[m].n for m in methods]\n",
+ "bars = ax3.bar(methods, failed, color=colors, alpha=0.7, edgecolor='black')\n",
+ "for bar, m in zip(bars, methods):\n",
+ " ax3.text(bar.get_x() + bar.get_width() / 2, bar.get_height() + 2,\n",
+ " f\"{outcomes[m].n_failed}/{outcomes[m].n}\", ha='center', fontsize=11)\n",
+ "ax3.set_ylabel('Fixes that failed (%)', fontsize=12)\n",
+ "ax3.set_ylim(0, 100)\n",
+ "ax3.set_title('Failure rate', fontsize=13, fontweight='bold')\n",
+ "ax3.grid(True, alpha=0.3, axis='y')\n",
+ "\n",
"plt.tight_layout()\n",
"plt.show()\n",
"\n",
- "print(\"\\n💡 Key Observations:\")\n",
- "print(\" - TOA and TDOA achieve similar accuracy with good geometry\")\n",
- "print(\" - AOA accuracy depends on distance to anchors\")\n",
- "print(\" - All methods benefit from symmetric anchor placement\")\n"
+ "print(\"\\nWhat this run measured:\")\n",
+ "for m in methods:\n",
+ " o = outcomes[m]\n",
+ " print(f\" {m:<5} median {o.median_m:.3f} m, {o.n_failed}/{o.n} failed\")\n",
+ "print(\"\\nEvery method solves every fix here, and that is a statement about this\")\n",
+ "print(\"GEOMETRY as much as about the methods. Part 7 keeps the code and the noise\")\n",
+ "print(\"and moves the beacons into a line.\")\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "---\n",
+ "\n",
+ "# Part 7: When the solver lies\n",
+ "\n",
+ "Everything above reports the accuracy of solves that worked. This part is the other half.\n",
+ "\n",
+ "An iterative positioning solve can fail in four ways, and only one of them is loud:\n",
+ "\n",
+ "1. it raises;\n",
+ "2. it reports `converged=False`;\n",
+ "3. it **never leaves the initial guess** — Gauss-Newton calls a zero step convergence;\n",
+ "4. it walks off to somewhere absurd and **still sets `converged=True`**.\n",
+ "\n",
+ "Counting only 1 and 2 is how a method comes to look better than it is. `core.rf.solve_batch`\n",
+ "applies all four, which is what the two sections below rely on.\n",
+ "\n",
+ "## 7.1 Geometry: the same four beacons, in a line\n",
+ "\n",
+ "Beacons down a corridor rather than around a room. Nothing changes about the measurement\n",
+ "model or the noise — only where the anchors are.\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from ch4_rf_point_positioning.example_comparison import compare_geometries\n",
+ "\n",
+ "# 100 fixes per method per geometry, with the failures kept rather than dropped\n",
+ "_ = compare_geometries(verbose=True)\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### What just happened\n",
+ "\n",
+ "TOA and TDOA failed **100 out of 100** on the collinear array — and their identical 6.770 m\n",
+ "median is the tell: it is the distance from the seed to the truth, a property of the starting\n",
+ "point rather than of the measurements. Two methods that share nothing but their\n",
+ "initialisation cannot agree to three decimal places by accident.\n",
+ "\n",
+ "Note the GDOP column. TOA's GDOP on the collinear array is 1.43 against 1.02 for the square:\n",
+ "a healthy number, for a configuration that solves nothing. DOP is a local, first-order\n",
+ "measure and the ambiguity that breaks this geometry is global. **A healthy DOP is necessary,\n",
+ "not sufficient.**\n",
+ "\n",
+ "AOA is *better* on the collinear array than on the square, because reflecting a position\n",
+ "flips every azimuth — bearings carry the side information ranges do not.\n",
+ "\n",
+ "## 7.2 The initial guess, and why it is usually the wrong thing to blame\n",
+ "\n",
+ "Now hold the geometry fixed at the well-behaved square and sweep the *initial guess* over the\n",
+ "whole floor, 1681 seeds, with **zero measurement noise** — so every failure below is the\n",
+ "solver.\n",
+ "\n",
+ "The only thing that changes between the two maps is the space the residual is formed in:\n",
+ "\n",
+ "| | |\n",
+ "|---|---|\n",
+ "| `residual=\"tan\"` | `z = tan(psi)`, Eq. (4.64) written literally |\n",
+ "| `residual=\"angle\"` | `wrap(psi - atan2(dE, dN))` — the default |\n",
+ "\n",
+ "`tan` has period pi, so an anchor ahead and one behind give the same measurement; and as the\n",
+ "estimate runs to infinity every bearing converges, so the tan residuals *shrink* on the way\n",
+ "out. Infinity is an attractor, and the iteration arrives there reporting success.\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from ch4_rf_point_positioning.example_initial_guess_basin import (\n",
+ " sweep, trace_worst, plot_summary)\n",
+ "\n",
+ "tan_result = sweep(\"tan\") # ~13 s\n",
+ "angle_result = sweep(\"angle\") # ~13 s\n",
+ "\n",
+ "fig = plot_summary([tan_result, angle_result], trace_worst(tan_result))\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Read the two maps against each other\n",
+ "\n",
+ "The wrapped-angle form does **not** make the solver globally convergent: a seed far outside\n",
+ "the room still walks off, 341 times out of 1681, and 196 of those still set `converged=True`.\n",
+ "\n",
+ "What it removes, completely, is the *quiet* class — the seeds that stall at the guess\n",
+ "(82 → 0) and the ones that stop somewhere plausible but wrong (181 → 0). Those are the\n",
+ "failures a reader cannot detect downstream, because they look like answers.\n",
+ "\n",
+ "So fixing the residual makes the solver **honest, not safe**. Two questions catch both\n",
+ "defects, and they are worth asking of any residual you write:\n",
+ "\n",
+ "- **is it bounded?**\n",
+ "- **does the cost stay large when the estimate is far wrong?**\n",
+ "\n",
+ "### Try it\n",
+ "\n",
+ "- Move the target: `example_initial_guess_basin.TRUTH = np.array([8.0, 2.0])` and re-run. The\n",
+ " basin moves with it — keep it off the 0.5 m seed lattice, or one seed *is* the answer and\n",
+ " gets scored as a stall.\n",
+ "- Widen the grid to `GRID_MIN, GRID_MAX = -20, 30`. The divergent fringe grows under both\n",
+ " parameterisations; the quiet failures stay at zero for `\"angle\"`.\n",
+ "- Add noise to `measurements()` and watch the boundary between basins fray.\n"
]
},
{
diff --git a/tests/ch4_rf_point_positioning/test_initial_guess_basin.py b/tests/ch4_rf_point_positioning/test_initial_guess_basin.py
new file mode 100644
index 0000000..d78473c
--- /dev/null
+++ b/tests/ch4_rf_point_positioning/test_initial_guess_basin.py
@@ -0,0 +1,155 @@
+"""What the initial-guess basin figure claims, asserted.
+
+The figure argues one thing and it is easy to overstate: that AOA's cold-start failures are
+a property of the residual's parameterisation rather than of the starting point. The example
+was in fact written expecting the wrapped-angle form to remove the basin outright, and the
+sweep said otherwise -- 341 of 1681 seeds still fail. What it removes is the QUIET class.
+
+So these tests pin the narrow claim, in both directions:
+
+ * the quiet failures -- stalled at the seed, or stopped somewhere plausible but wrong --
+ are what changing the residual removes;
+ * the loud ones survive, and the convergence flag is still not a check, so the figure must
+ not be re-captioned as "fixed".
+
+`test_the_ratio_is_not_one` is the "a demonstration that does not demonstrate" guard: if the
+two parameterisations ever perform the same, this example has stopped demonstrating anything
+and should be deleted rather than left to argue from a caption.
+
+The sweeps are ~13 s each, so they are computed once and shared -- see the Cost note in
+.cursor/rules/030-figures-and-claims.mdc.
+
+Author: Li-Ta Hsu
+References: Chapter 4, Eqs. (4.63)-(4.65). Companion to
+ test_aoa_initialisation_basin.py, which pins the same behaviour from a single
+ cold start; this one sweeps the whole floor.
+"""
+
+import unittest
+
+import matplotlib
+
+matplotlib.use("Agg") # headless: no display during tests
+
+import numpy as np
+
+from ch4_rf_point_positioning.example_initial_guess_basin import (
+ ANCHORS,
+ DIVERGED,
+ SOLVED,
+ STALLED,
+ TRUTH,
+ WRONG,
+ sweep,
+ trace_worst,
+)
+
+_CACHE = {}
+
+
+def sweeps():
+ """Both sweeps, computed once per session."""
+ if not _CACHE:
+ for residual in ("tan", "angle"):
+ _CACHE[residual] = sweep(residual, verbose=False)
+ return _CACHE["tan"], _CACHE["angle"]
+
+
+def quiet(result):
+ """Failures that look like answers: never moved, or stopped somewhere plausible."""
+ return result["counts"][STALLED] + result["counts"][WRONG]
+
+
+class TestInitialGuessBasin(unittest.TestCase):
+ """Zero measurement noise, so every failure counted here is the solver."""
+
+ def test_the_quiet_failures_are_what_the_residual_fix_removes(self):
+ """The claim the figure is actually allowed to make."""
+ tan, angle = sweeps()
+
+ self.assertGreater(quiet(tan), 100)
+ self.assertEqual(quiet(angle), 0)
+
+ def test_the_wrapped_form_never_stops_somewhere_plausible(self):
+ """No `converged, wrong place` outcome: the sneaky class goes to zero.
+
+ A near-miss that reports success is the one failure mode a reader cannot detect
+ downstream, which is why it gets its own assertion.
+ """
+ _, angle = sweeps()
+
+ self.assertEqual(angle["counts"][WRONG], 0)
+
+ def test_far_seeds_still_diverge_under_both(self):
+ """The honest half of the result, pinned so the caption cannot drift.
+
+ If this ever fails, the wrapped-angle form has become globally convergent on this
+ geometry and the example's "honest, not safe" paragraph is out of date.
+ """
+ tan, angle = sweeps()
+
+ self.assertGreater(tan["counts"][DIVERGED], 0)
+ self.assertGreater(angle["counts"][DIVERGED], 0)
+
+ def test_the_convergence_flag_is_not_a_check_under_either(self):
+ """Failures that set converged=True exist in both sweeps.
+
+ This is why `solve_batch`'s four conditions are not optional: fixing the residual
+ does not turn the flag into a test.
+ """
+ tan, angle = sweeps()
+
+ self.assertGreater(tan["silent"], 0)
+ self.assertGreater(angle["silent"], 0)
+
+ def test_the_ratio_is_not_one(self):
+ """A demonstration that does not demonstrate is a failing test."""
+ tan, angle = sweeps()
+ failed_tan = tan["n"] - tan["counts"][SOLVED]
+ failed_angle = angle["n"] - angle["counts"][SOLVED]
+
+ self.assertGreater(failed_tan / max(failed_angle, 1), 1.5)
+
+ def test_seeds_inside_the_room_all_solve_with_the_wrapped_form(self):
+ """The practically relevant statement: a seed anywhere in the room is fine."""
+ _, angle = sweeps()
+ inside = (
+ (angle["xx"] >= ANCHORS[:, 0].min()) & (angle["xx"] <= ANCHORS[:, 0].max())
+ & (angle["yy"] >= ANCHORS[:, 1].min()) & (angle["yy"] <= ANCHORS[:, 1].max())
+ )
+
+ self.assertGreater(int(np.sum(inside)), 100)
+ self.assertTrue(np.all(angle["codes"][inside] == SOLVED))
+
+ def test_the_traced_run_is_a_silent_divergence(self):
+ """The fourth panel must show a lie, not an honest failure.
+
+ The largest error in the tan sweep reports converged=False, which is correct
+ behaviour and not worth a panel; `trace_worst` deliberately picks the furthest run
+ that still set the flag.
+ """
+ tan, _ = sweeps()
+ seed, history, converged = trace_worst(tan)
+
+ self.assertTrue(converged)
+ self.assertGreater(np.linalg.norm(history[-1] - TRUTH), 1e6)
+ self.assertLess(np.linalg.norm(history[0] - seed), 1e-9)
+
+ def test_the_measurements_are_sufficient(self):
+ """Nothing is wrong with the data: seeded at the answer, tan solves too.
+
+ Without this the figure could be read as a geometry or an observability problem.
+ """
+ from ch4_rf_point_positioning.example_initial_guess_basin import measurements
+ from core.rf import AOAPositioner
+
+ for residual in ("tan", "angle"):
+ est, info = AOAPositioner(ANCHORS).solve(
+ measurements(), initial_guess=TRUTH + 0.5, residual=residual
+ )
+ self.assertTrue(info["converged"], residual)
+ self.assertLess(float(np.linalg.norm(est - TRUTH)), 1e-3, residual)
+
+
+if __name__ == "__main__":
+ unittest.main()