From d39f528c81d1658c4148deb8923094f0a083594f Mon Sep 17 00:00:00 2001 From: Claude Date: Sun, 2 Aug 2026 11:59:46 +0000 Subject: [PATCH 3/3] feat(engine): antenna-delay calibration for inter-anchor ranging MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A ground-truth check of a field capture showed the inter-anchor ranges biased long by ~0.28 m mean — up to +0.54 m on the low anchor's links — which a per-device additive model (antenna delay) fits to ~9 cm RMS, dominated by that one anchor (+0.39 m). A per-device bias is nearly invisible to the distance solver (it inflates all of a device's edges together) yet pushes the anchor ~0.4 m out of place and skews the heading of every client that AoA-anchors to it. AntennaDelayCalibrator solves per-device delays b_i from the user-surveyed layout (MeshGroundTruth) and the measured inter-anchor distances by least squares on err_ij = measured - true = b_i + b_j (uniquely determined once a triangle of mutually-ranging anchors exists). The engine subtracts b_a + b_b from every inter-anchor range before the constellation solve (drift detection still sees the raw ranges — it tracks change, not bias), auto-calibrating once the survey is entered and enough edges accumulate. Tests: AntennaDelayCalibratorTest recovers injected delays exactly and isolates NLOS in the residual; antenna_delay_calibration_corrects_a_biased_constellation proves a biased constellation is corrected back to truth end-to-end. Verified via :common:jvmTest. Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../mofe/engine/AntennaDelayCalibrator.kt | 120 ++++++++++++++++++ .../mofe/engine/MultiObserverFusionEngine.kt | 81 +++++++++++- .../mofe/engine/AnchorMeshBootstrapTest.kt | 49 +++++++ .../mofe/engine/AntennaDelayCalibratorTest.kt | 74 +++++++++++ 4 files changed, 318 insertions(+), 6 deletions(-) create mode 100644 common/src/commonMain/kotlin/com/aether/mofe/engine/AntennaDelayCalibrator.kt create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/AntennaDelayCalibratorTest.kt diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/AntennaDelayCalibrator.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/AntennaDelayCalibrator.kt new file mode 100644 index 0000000..5b766b6 --- /dev/null +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/AntennaDelayCalibrator.kt @@ -0,0 +1,120 @@ +package com.aether.mofe.engine + +import com.aether.mofe.model.DeviceId +import com.aether.mofe.model.Vector3D +import kotlin.math.abs +import kotlin.math.sqrt + +/** + * Per-device antenna-delay calibration for inter-anchor ranging. + * + * An uncalibrated UWB radio adds a fixed per-device offset to every range it takes (its "antenna + * delay"), so a measured inter-anchor distance is + * + * d_measured(i, j) ≈ ‖p_i − p_j‖ + b_i + b_j + * + * with b_i the device's delay in metres. A per-device bias is nearly invisible to the constellation + * solver — it inflates ALL of a device's edges together, which the distance fit can largely absorb + * into position — yet it distorts the solved frame: field data (a surveyed 2.3 m × 3.7 m room) shows + * one low anchor reading +0.39 m long on every link, which pushes it ~0.4 m out of place and skews + * the heading of every client that AoA-anchors to it. + * + * Given the user-surveyed layout ([MeshGroundTruth]) the true pairwise distances are known, so the + * per-edge error e_ij = d_measured − ‖p_i − p_j‖ = b_i + b_j is a LINEAR system in the delays. This + * solves it by least squares so the engine can subtract b_i + b_j from every measured range and hand + * the constellation solver bias-corrected distances. + * + * Solvability: the b_i + b_j system is uniquely determined when the ranged graph is connected AND + * non-bipartite — i.e. it contains an odd cycle, which any triangle of three mutually-ranging anchors + * provides. A purely bipartite graph leaves a 1-DOF gauge in the ABSOLUTE delays, but the per-edge + * correction b_i + b_j is gauge-invariant on every MEASURED edge, so the ranges still correct + * correctly; only the reported absolute offsets would be ambiguous. + */ +object AntennaDelayCalibrator { + + data class Result( + /** Per-device antenna-delay offset b_i (metres) to SUBTRACT from each of its ranges. */ + val delays: Map, + /** RMS of the fit residual e_ij − (b_i + b_j) over the used edges (metres). Low ⇒ the errors + * really are a per-device bias the correction removes; high ⇒ NLOS/per-link error it can't. */ + val residualRmsMeters: Double, + val edgesUsed: Int, + /** True when enough edges over ≥2 surveyed devices produced a delay estimate. */ + val ok: Boolean, + ) { + companion object { val EMPTY = Result(emptyMap(), 0.0, 0, false) } + } + + /** + * @param truth surveyed positions (any origin/axes — only pairwise distances are used). + * @param measured measured inter-anchor distances keyed by an unordered device pair. + * @param minEdges require at least this many usable edges, else an empty (no-op) result. + */ + fun solve( + truth: Map, + measured: Map, Double>, + minEdges: Int = 3, + ): Result { + // Usable edge = both endpoints surveyed, distinct, finite error. + val ei = ArrayList(); val ej = ArrayList(); val ee = ArrayList() + val order = ArrayList(); val idx = HashMap() + fun index(id: DeviceId): Int = idx.getOrPut(id.value) { order.add(id); order.size - 1 } + for ((pair, d) in measured) { + if (pair.first == pair.second) continue + val pi = truth[pair.first] ?: continue + val pj = truth[pair.second] ?: continue + val err = d - pi.distanceTo(pj) + if (!err.isFinite()) continue + ei.add(index(pair.first)); ej.add(index(pair.second)); ee.add(err) + } + val m = ee.size + val n = order.size + if (m < minEdges || n < 2) return Result.EMPTY + + // Normal equations AᵀA·b = Aᵀe for e_k = b_{i_k} + b_{j_k}. AᵀA = deg·I + adjacency. + val ata = Array(n) { DoubleArray(n) } + val ate = DoubleArray(n) + for (k in 0 until m) { + val a = ei[k]; val b = ej[k] + ata[a][a] += 1.0; ata[b][b] += 1.0 + ata[a][b] += 1.0; ata[b][a] += 1.0 + ate[a] += ee[k]; ate[b] += ee[k] + } + // Tiny ridge: negligible when a triangle makes the system full rank; picks the min-norm + // solution (per-edge corrections still exact) in the degenerate bipartite case. + for (k in 0 until n) ata[k][k] += 1e-9 + val b = solveLinear(ata, ate) ?: return Result.EMPTY + + var ss = 0.0 + for (k in 0 until m) { + val r = ee[k] - (b[ei[k]] + b[ej[k]]) + ss += r * r + } + val delays = order.withIndex().associate { (k, id) -> id to b[k] } + return Result(delays, sqrt(ss / m), m, true) + } + + /** Gaussian elimination with partial pivoting; null if singular. */ + private fun solveLinear(a: Array, rhs: DoubleArray): DoubleArray? { + val n = rhs.size + val mm = Array(n) { i -> DoubleArray(n + 1).also { row -> + for (j in 0 until n) row[j] = a[i][j]; row[n] = rhs[i] } } + for (col in 0 until n) { + var piv = col + for (r in col + 1 until n) if (abs(mm[r][col]) > abs(mm[piv][col])) piv = r + val t = mm[col]; mm[col] = mm[piv]; mm[piv] = t + if (abs(mm[col][col]) < 1e-15) return null + for (r in col + 1 until n) { + val f = mm[r][col] / mm[col][col] + for (c in col..n) mm[r][c] -= f * mm[col][c] + } + } + val x = DoubleArray(n) + for (i in n - 1 downTo 0) { + var s = mm[i][n] + for (j in i + 1 until n) s -= mm[i][j] * x[j] + x[i] = s / mm[i][i] + } + return x + } +} diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt index 17f3237..a4f1617 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt @@ -253,6 +253,14 @@ class MultiObserverFusionEngine( /** Maintenance-tick counter that paces constellation refinement. */ private var maintenanceTicks = 0 + /** Per-device antenna-delay offsets (metres) solved by [calibrateAntennaDelays] and SUBTRACTED + * from every measured inter-anchor range before the constellation solve. A per-device range + * bias (an uncalibrated UWB antenna — field-observed at +0.39 m on one anchor) is nearly + * invisible to the distance solver yet pushes that anchor ~0.4 m out of place; removing + * b_a + b_b un-distorts the frame. Empty (identity) until the surveyed layout is entered. */ + private var antennaDelays: Map = emptyMap() + private var _antennaDelayCalibration: AntennaDelayCalibrator.Result? = null + /** Single mesh-wide event detector — predicates apply to every target. */ private val eventDetector = EventDetector() @@ -1140,8 +1148,9 @@ class MultiObserverFusionEngine( // the static anchor constellation we want to refine. val anchors = frameManager.getAllReferencePoints().associate { it.id to it.position } if (anchors.size < config.minAnchorsForFusion) return 0 - // Keep only edges whose BOTH endpoints are current reference anchors. - val edges = interAnchorDistances.filterKeys { it.first in anchors && it.second in anchors } + // Keep only edges whose BOTH endpoints are current reference anchors, with per-device + // antenna delays removed (see [correctedInterAnchorDistances]). + val edges = correctedInterAnchorDistances().filterKeys { it.first in anchors && it.second in anchors } if (edges.size < config.anchorConstellationMinEdges) return 0 val result = AnchorConstellationSolver.refine( @@ -1209,6 +1218,9 @@ class MultiObserverFusionEngine( // may never place references (unusable phone AoA, or an ANCHOR role that never reached // the band roster), leaving the engine stuck below canSolvePositions with the whole // pipeline dead at Stage 1. A distance-only bootstrap needs neither bearings nor roles. + // Solve per-device antenna delays from the surveyed layout once enough ranges exist, so the + // bootstrap below consumes bias-corrected inter-anchor distances. + maybeCalibrateAntennaDelays() bootstrapReferenceConstellation() // Rigid-body anchor refinement on its own (slower) cadence. maintenanceTicks++ @@ -1269,6 +1281,9 @@ class MultiObserverFusionEngine( pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "edges, rootId: DeviceId): Map? { + private fun refineConstellationFromPrior( + ids: Set, rootId: DeviceId, distances: Map, Double>, + ): Map? { val known = frameManager.getAllReferencePoints().associate { it.id to it.position } val prior = LinkedHashMap() for (id in ids) prior[id] = if (id == rootId) Vector3D.ZERO else (known[id] ?: return null) if (prior.size < config.minAnchorsForFusion) return null - val refined = AnchorConstellationSolver.refine(prior, interAnchorDistances, rootId) + val refined = AnchorConstellationSolver.refine(prior, distances, rootId) // Reject a fit that cannot satisfy the edges we have — inconsistent/garbage ranging. val maxSeedResidualMeters = 0.5 return if (refined.converged && refined.rmsResidualMeters <= maxSeedResidualMeters) refined.positions else null } + // ═════════════════════════════════════════════════════════════════════ + // Antenna-delay calibration (per-device inter-anchor ranging bias) + // ═════════════════════════════════════════════════════════════════════ + + /** + * Solve per-device antenna delays from the user-surveyed layout ([MeshGroundTruth]) and the + * accumulated inter-anchor ranges, then apply them to every subsequent constellation solve. + * An uncalibrated UWB antenna adds a fixed per-device offset to every range it takes; that + * per-device bias barely moves the distance-fit residual yet pushes the device ~0.4 m out of + * position and skews the heading of every client that AoA-anchors to it (field-observed at + * +0.39 m). With the survey the true distances are known, so err_ij = measured − true = b_i + b_j + * is a linear system in the delays. No-op (identity) until the survey is entered and enough + * inter-anchor edges exist. Idempotent — always solves from the RAW ranges, so re-running as + * more edges arrive only sharpens it. Confined to the engine thread. + */ + fun calibrateAntennaDelays(): AntennaDelayCalibrator.Result { + if (MeshGroundTruth.isEmpty()) return AntennaDelayCalibrator.Result.EMPTY + val truth = MeshGroundTruth.positions().mapKeys { DeviceId(it.key) } + val result = AntennaDelayCalibrator.solve(truth, interAnchorDistances, config.anchorConstellationMinEdges) + if (result.ok) { antennaDelays = result.delays; _antennaDelayCalibration = result } + return result + } + + /** Latest antenna-delay calibration (per-device delays + fit residual), or null if never run. + * The residual is the un-removable (NLOS/per-link) part of the ranging error. Diagnostic/UI. */ + fun antennaDelayCalibration(): AntennaDelayCalibrator.Result? = _antennaDelayCalibration + + /** Solved per-device antenna-delay offsets (metres); empty until [calibrateAntennaDelays]. */ + fun antennaDelays(): Map = antennaDelays + + /** + * Inter-anchor distances with per-device antenna delays removed (raw − b_a − b_b). Identity + * until [calibrateAntennaDelays] has solved delays. The constellation solve consumes THESE so a + * per-device antenna bias does not distort the frame; drift detection still sees the raw ranges + * (it tracks CHANGE, not absolute bias, so a constant delay is harmless there). + */ + private fun correctedInterAnchorDistances(): Map, Double> { + if (antennaDelays.isEmpty()) return interAnchorDistances + return interAnchorDistances.mapValues { (pair, d) -> + d - (antennaDelays[pair.first] ?: 0.0) - (antennaDelays[pair.second] ?: 0.0) + } + } + + /** Auto-calibrate once the surveyed layout is present and enough edges have accumulated; retries + * each maintenance tick until it succeeds, then holds (re-run [calibrateAntennaDelays] to redo). */ + private fun maybeCalibrateAntennaDelays() { + if (antennaDelays.isNotEmpty()) return + if (MeshGroundTruth.isEmpty()) return + if (interAnchorDistances.size < config.anchorConstellationMinEdges) return + calibrateAntennaDelays() + } + // ═════════════════════════════════════════════════════════════════════ // Predicate registration // ═════════════════════════════════════════════════════════════════════ diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt index 033e180..2bc93a0 100644 --- a/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt @@ -185,6 +185,55 @@ class AnchorMeshBootstrapTest { assertTrue(maxErr < 0.03, "distance-solved constellation matches truth; maxErr=${maxErr}m") } + @Test + fun antenna_delay_calibration_corrects_a_biased_constellation() { + // A per-device antenna delay (A4 the worst — field-observed +0.39 m) inflates every range + // that device takes, distorting the distance-solved frame almost invisibly (a per-device + // bias barely moves the fit residual). Given the surveyed layout, calibrateAntennaDelays() + // solves the delays and the constellation solve then consumes bias-corrected distances → + // the frame matches truth. Uncorrected, A4's edges are ~0.5 m long and the frame is skewed. + val delays = mapOf(a1 to 0.14, a2 to -0.01, a3 to -0.03, a4 to 0.39) + MeshGroundTruth.clear() + try { + for ((id, p) in truth) MeshGroundTruth.set(id.value, MeshGroundTruth.Pose(p)) + val h = MofeTestHarness(MofeConfig()).build() + h.engine.initializeAsRoot(a1) + // AoA prior (garbage z), as on-device, so a frame exists to be corrected. + h.engine.registerMeshNode(a2, Vector3D(2.2, 0.0, -1.3)) + h.engine.registerMeshNode(a3, Vector3D(0.0, 3.6, -1.1)) + h.engine.registerMeshNode(a4, Vector3D(2.2, 2.9, -1.5)) + // Feed BIASED inter-anchor ranges: true + b_i + b_j on all six edges. + val ids = listOf(a1, a2, a3, a4) + repeat(6) { + for (i in ids.indices) for (j in i + 1 until ids.size) { + val d = truth.getValue(ids[i]).distanceTo(truth.getValue(ids[j])) + + delays.getValue(ids[i]) + delays.getValue(ids[j]) + h.engine.processInterAnchorRanging(ids[i], ids[j], d) + } + } + // Calibrate from the survey → recovers the per-device delays. + val cal = h.engine.calibrateAntennaDelays() + assertTrue(cal.ok, "calibration must solve") + assertTrue(cal.residualRmsMeters < 0.02, "a clean per-device bias fits tightly; got ${cal.residualRmsMeters}m") + for (id in ids) assertTrue(abs(cal.delays.getValue(id) - delays.getValue(id)) < 0.03, + "delay for ${id.value} recovered (~${delays.getValue(id)}); got ${cal.delays[id]}") + + // The constellation solve now uses corrected distances → matches truth (pairwise). + val placed = h.engine.bootstrapReferenceConstellation() + assertTrue(placed > 0, "frame must (re)establish on corrected distances; placed=$placed") + val refs = h.frameManager.getAllReferencePoints().associate { it.id to it.position } + var maxErr = 0.0 + for (i in ids.indices) for (j in i + 1 until ids.size) { + val got = refs.getValue(ids[i]).distanceTo(refs.getValue(ids[j])) + val want = truth.getValue(ids[i]).distanceTo(truth.getValue(ids[j])) + maxErr = maxOf(maxErr, abs(got - want)) + } + assertTrue(maxErr < 0.05, "corrected constellation must match true distances; maxErr=${maxErr}m") + } finally { + MeshGroundTruth.clear() + } + } + @Test fun gauge_is_deterministic_regardless_of_input_order() { // The solve must pick its axes (a1→+X, a2→XY, a3→+Z) from a STABLE id order, so the diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/AntennaDelayCalibratorTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/AntennaDelayCalibratorTest.kt new file mode 100644 index 0000000..3e1ba02 --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/AntennaDelayCalibratorTest.kt @@ -0,0 +1,74 @@ +package com.aether.mofe.engine + +import com.aether.mofe.model.DeviceId +import com.aether.mofe.model.Vector3D +import kotlin.math.abs +import kotlin.test.Test +import kotlin.test.assertTrue + +/** + * Per-device antenna-delay calibration. Field data (a surveyed 2.3 m × 3.7 m room) showed the + * inter-anchor ranges biased long — up to +0.54 m on the low anchor's links — which a per-device + * additive model (antenna delay) fit to ~9 cm RMS, dominated by that one anchor (+0.39 m). Left in, + * it pushes the anchor ~0.4 m out of place and skews every client's heading. These tests exercise + * the solver that recovers those per-device offsets from the surveyed layout. + */ +class AntennaDelayCalibratorTest { + private val a1 = DeviceId("A1"); private val a2 = DeviceId("A2") + private val a3 = DeviceId("A3"); private val a4 = DeviceId("A4") + + // Surveyed layout (xy rel to A1, z rel; A4 the low corner) — the real field geometry. + private val truth = mapOf( + a1 to Vector3D(0.0, 0.0, 0.0), + a2 to Vector3D(2.286, 0.0, 0.1525), + a3 to Vector3D(0.0, 3.6957, 0.216), + a4 to Vector3D(2.286, 2.921, -0.3428), + ) + private val ids = listOf(a1, a2, a3, a4) + + private fun measuredWith( + delays: Map, + nlos: Map, Double> = emptyMap(), + ): Map, Double> { + val m = HashMap, Double>() + for (i in ids.indices) for (j in i + 1 until ids.size) { + val a = ids[i]; val b = ids[j] + var d = truth.getValue(a).distanceTo(truth.getValue(b)) + delays.getValue(a) + delays.getValue(b) + d += nlos[a to b] ?: 0.0 + m[a to b] = d + } + return m + } + + @Test + fun recovers_injected_per_device_delays_exactly() { + val delays = mapOf(a1 to 0.142, a2 to -0.014, a3 to -0.082, a4 to 0.393) + val r = AntennaDelayCalibrator.solve(truth, measuredWith(delays)) + assertTrue(r.ok, "must solve") + assertTrue(r.residualRmsMeters < 1e-4, "a clean per-device bias fits exactly; got ${r.residualRmsMeters}") + for (id in ids) assertTrue(abs(r.delays.getValue(id) - delays.getValue(id)) < 1e-3, + "recover ${id.value}: want ${delays.getValue(id)} got ${r.delays[id]}") + } + + @Test + fun absorbs_per_device_bias_and_leaves_nlos_in_the_residual() { + // Per-device delays + a single-link NLOS bump: the delays soak up the per-device part; the + // NLOS survives as residual — the signal the calibration UI should surface as a bad link. + val delays = mapOf(a1 to 0.10, a2 to 0.0, a3 to 0.0, a4 to 0.40) + val nlos = mapOf((a1 to a3) to 0.15) + val r = AntennaDelayCalibrator.solve(truth, measuredWith(delays, nlos)) + assertTrue(r.ok && r.residualRmsMeters > 0.02, + "an NLOS bump on one link must leave a residual; got ${r.residualRmsMeters}") + // A clean edge (A1-A2) still corrects back to ~truth. + val meas = measuredWith(delays, nlos) + val corr = meas.getValue(a1 to a2) - r.delays.getValue(a1) - r.delays.getValue(a2) + assertTrue(abs(corr - truth.getValue(a1).distanceTo(truth.getValue(a2))) < 0.08, + "a clean edge must correct to ~truth; got $corr") + } + + @Test + fun declines_when_too_few_edges() { + val r = AntennaDelayCalibrator.solve(truth, mapOf((a1 to a2) to 2.3), minEdges = 3) + assertTrue(!r.ok, "one edge cannot calibrate the constellation") + } +} -- 2.43.0