From 4b9c99f4bc7bc042da7ce3bb9fe1c01b828cd2d0 Mon Sep 17 00:00:00 2001 From: Claude Date: Sun, 2 Aug 2026 02:28:30 +0000 Subject: [PATCH 2/3] fix(engine): recover the anchor constellation when one inter-anchor link is missing MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit AnchorMeshBootstrap's closed-form cascade needs the COMPLETE inter-anchor graph — the 4th anchor is trilaterated against the other three — so a single permanently missing link returns null and the frame stays pinned to the raw (garbage-z) AoA seed forever (solve-null every maintenance tick). This is not rare: a fixed obstruction between two anchors (e.g. a TV) blocks their UWB range for good, leaving K4 minus one edge. When the cascade returns null, fall back to refining the AoA-placed prior against the edges that DO exist (AnchorConstellationSolver): it honours every available distance, escapes a coplanar AoA seed, and leans on the prior only for the one gauge/fold DOF the missing edge leaves free. The mesh crosses quorum and the well-constrained geometry is corrected; the two anchors either side of the gap remain prior-limited until the link is restored. Recorded in the pipeline trace as "placed(-raw)-degen" so a capture shows the degenerate path ran. Test: recovers_from_aoa_prior_when_one_link_is_missing (5 of 6 edges, A2<->A3 dropped) reaches quorum and satisfies the measured distances; the existing waits_when_a_link_is_missing (no prior available) still declines, unchanged. Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../mofe/engine/MultiObserverFusionEngine.kt | 39 ++++++++++++++++- .../mofe/engine/AnchorMeshBootstrapTest.kt | 43 +++++++++++++++++++ 2 files changed, 81 insertions(+), 1 deletion(-) 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 5652901..17f3237 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt @@ -1269,7 +1269,15 @@ class MultiObserverFusionEngine( pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "edges MofePipelineTrace.RefPoint(id, p) }, ) return placed } + /** + * Fallback for an INCOMPLETE inter-anchor distance graph. [AnchorMeshBootstrap]'s closed-form + * cascade needs the COMPLETE graph (every anchor pair ranged) to place four anchors, because + * the fourth is trilaterated against the other three; a permanently-missing link — e.g. a TV + * between two anchors, which NLOS UWB never ranges through — leaves it one edge short and it + * returns null. Rather than freeze the frame on the raw (garbage-z) AoA seed forever, refine + * that seed with [AnchorConstellationSolver]: it honours every AVAILABLE inter-anchor distance, + * escapes a coplanar AoA seed, and leans on the prior only for the one gauge/fold DOF the + * missing edge leaves free. Returns the refined positions, or null when there is no usable + * prior yet (too few anchors seeded) or the fit is too poor to trust. + * + * Caveat: the free DOF (the two anchors either side of the gap fold about the line through the + * anchors that DO range them both) is resolved only by the prior, so those two anchors' shared + * placement is no better than the AoA seed until the link is restored — but every + * well-constrained anchor is corrected and, crucially, the frame reaches quorum, which the + * missing edge otherwise blocks entirely. + */ + private fun refineConstellationFromPrior(ids: Set, rootId: DeviceId): 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) + // 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 + } + // ═════════════════════════════════════════════════════════════════════ // 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 0abdafe..033e180 100644 --- a/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt @@ -98,6 +98,49 @@ class AnchorMeshBootstrapTest { } } + @Test + fun recovers_from_aoa_prior_when_one_link_is_missing() { + // A TV between two anchors permanently blocks their mutual UWB range (A2<->A3 here — NLOS), + // so the inter-anchor graph is K4 minus one edge. The closed-form cascade cannot place the + // 4th anchor from that (it needs all three edges to the placed set), so the frame used to + // stay pinned to the raw (garbage-z) AoA seed forever (solve-null every tick). The bootstrap + // now falls back to refining that AoA prior against the 5 edges it DOES have — reaching + // quorum and correcting the well-constrained geometry. The unranged fold (A2/A3 relative) + // is left to the prior and deliberately NOT asserted here. + val h = MofeTestHarness(MofeConfig()).build() + h.engine.initializeAsRoot(a1) + // AoA prior: correct-ish xy, GARBAGE z (what phone AoA produces) — the seed the fallback refines. + h.engine.registerMeshNode(a2, Vector3D(0.0, 0.5, -1.4)) + h.engine.registerMeshNode(a3, Vector3D(1.3, 0.0, -1.1)) + h.engine.registerMeshNode(a4, Vector3D(1.0, 0.3, 0.0)) + assertTrue(h.frameManager.canSolvePositions, "precondition: AoA path already reached quorum") + + // Feed every inter-anchor distance EXCEPT the blocked pair a2<->a3. + val ids = listOf(a1, a2, a3, a4) + repeat(6) { + for (i in ids.indices) for (j in i + 1 until ids.size) { + if (ids[i] == a2 && ids[j] == a3) continue // TV between A2 and A3: never ranges + val d = truth.getValue(ids[i]).distanceTo(truth.getValue(ids[j])) + h.engine.processInterAnchorRanging(ids[i], ids[j], d) + } + } + + val placed = h.engine.bootstrapReferenceConstellation() + assertTrue(placed > 0, "fallback must re-seat anchors from the prior + available edges (got $placed)") + + val refs = h.frameManager.getAllReferencePoints().associate { it.id to it.position } + // The 5 MEASURED edges must be honoured (the unranged a2-a3 fold is free, so not asserted). + var maxErr = 0.0 + for (i in ids.indices) for (j in i + 1 until ids.size) { + if (ids[i] == a2 && ids[j] == a3) continue + 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, + "the fallback must satisfy the 5 measured inter-anchor distances; maxErr=${maxErr}m") + } + @Test fun overwrites_aoa_seeded_frame_from_distances() { // The field failure in the flesh: the frame is FIRST seeded from raw AoA (garbage -- 2.43.0