From 52a0ff80a8b7888c6e36cc4f73feaa07cd610d3e Mon Sep 17 00:00:00 2001 From: Claude Date: Sun, 2 Aug 2026 21:43:19 +0000 Subject: [PATCH] =?UTF-8?q?fix(engine):=20only=20LOCK=20a=20trustworthy=20?= =?UTF-8?q?constellation=20solve=20=E2=80=94=20deterministic,=20repeatable?= =?UTF-8?q?=20mesh=20frame?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Field report (build already has the frame lock): the mesh reaches quorum but solves a DIFFERENT shape on every cold start and after each root bump; some runs collapse to a near-degenerate (triangular, two nodes overlapping) shape; different inter-anchor links read MISSING/DEGRADED each run. Root cause (the solver itself is deterministic in its inputs; the INPUTS and the LATCH are not): * the constellation solve is allowed to fire on an INCOMPLETE edge set (anchorConstellationMinEdges = 3, no complete-graph / per-edge-sample gate). A missing inter-anchor link makes the closed-form K4 cascade return null and the engine falls to the prior-folded fallback, whose missing-edge DOF is resolved from the noisy AoA prior — a different fold every run; * which peer cross-links have arrived over the best-effort band varies per run (arrival timing, not a data race — all engine state is serialized on one dispatcher), so the path taken (K4 vs fallback) and the edge values differ; * the frame LOCK latched the FIRST oriented solve regardless of completeness, fit, degeneracy, or stability — freezing whatever shape that tick produced, and re-freezing a new one after each bump. Fix — gate the lock on a TRUSTWORTHY solve (accuracy > speed). The frame may latch only when the solve is: * COMPLETE — a closed-form K4 solve, never the prior-folded fallback; * WELL-FIT — RMS inter-anchor residual within anchorConstellationLockMaxResidualMeters (6 cm), not the fallback's loose 0.5 m; * NON-DEGENERATE — min pairwise anchor separation >= anchorMinSeparationMeters (10 cm), rejecting the collapsed/overlapping shape; * STABLE — the pairwise-distance shape signature agrees for anchorConstellationLockStableTicks (3) consecutive solves within anchorConstellationLockStabilityEpsilonMeters (2 cm). Because the latch then always fires on a complete, converged, low-residual, stable distance set, the locked shape is repeatable cold-start to cold-start and after a bump-triggered re-solve. If a link is chronically missing, nothing locks — the frame stays CONVERGING and the missing link is surfaced, which is the honest signal to fix the geometry rather than freezing a guess. constellationFrameState() now reports CONVERGING while waiting for a trustworthy solve (never-locked) vs RESOLVING after a disruption to a previously-locked frame. New recorder statuses: "converging" (solved, not yet trusted). Verified: ConstellationFrameLockTest — locks a complete/consistent/stable solve; NEVER locks an incomplete-graph (fallback), a grossly-inconsistent (high-residual), or a not-yet-stable solve; motion release/track/recover intact. Updated the dev's holds_an_established_frame test to the new multi-tick-stable establishment contract. Full :common:jvmTest green (672 passed, 0 failed). Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../mofe/engine/MultiObserverFusionEngine.kt | 121 ++++++++++-- .../com/aether/mofe/model/ConfigTypes.kt | 19 ++ .../mofe/engine/AnchorMeshBootstrapTest.kt | 14 +- .../mofe/engine/ConstellationFrameLockTest.kt | 176 ++++++++++-------- 4 files changed, 236 insertions(+), 94 deletions(-) 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 00eed91..2a028d8 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt @@ -417,6 +417,14 @@ class MultiObserverFusionEngine( * re-lock after a motion release or an anchor drop broke a previously-locked frame. */ private var frameWasLocked = false + /** Sorted pairwise inter-anchor distances of the PREVIOUS solve, and a count of how many + * consecutive solves have AGREED with it (within the config epsilon). The frame may only lock + * once this run reaches [MofeConfig.anchorConstellationLockStableTicks] — the "wait for a + * repeatable shape before trusting it" gate (accuracy > speed). Reset whenever the lock is + * released (motion / offline / re-establish) so a re-solve must re-confirm before re-locking. */ + private var lastShapeSignature: DoubleArray? = null + private var stableSolveTicks = 0 + /** Last motion mode each peer self-asserted on the band (via [applyPeerMotionAssertion]). Read * by [anyReferenceAnchorInMotion] so a peer reference anchor moving releases the frame lock — * a peer's own IMU is the only sensor that can tell this device that peer was bumped/moved. */ @@ -425,7 +433,7 @@ class MultiObserverFusionEngine( /** Drop the locked orientation AND release the established hold, so the next maintenance tick * re-solves and re-orients from scratch. Call on a deliberate re-calibration (anchors * repositioned) — [initializeAsRoot] already does this on reset. */ - fun resetOrientationLock() { frameOrientation = null; frameEstablished = false } + fun resetOrientationLock() { frameOrientation = null; frameEstablished = false; resetLockStability() } /** * True if any current reference anchor is reporting motion — the override that releases the @@ -459,12 +467,63 @@ class MultiObserverFusionEngine( // advertise it immediately rather than a tick late. anyReferenceAnchorInMotion() -> ConstellationFrameState.RESOLVING_MOTION frameEstablished -> ConstellationFrameState.LOCKED - frameOrientation != null && - frameManager.getAllReferencePoints().size >= config.minAnchorsForFusion -> - ConstellationFrameState.RESOLVING + // Distinguish the INITIAL convergence (never locked — still gathering a complete, stable, + // trustworthy solve; the accuracy>speed wait) from RE-solving a frame that HAD been locked + // and was broken by an anchor drop / settling back after a disruption. + frameWasLocked -> ConstellationFrameState.RESOLVING else -> ConstellationFrameState.CONVERGING } + // ── Frame-lock TRUST gate helpers (see [bootstrapReferenceConstellation]) ────────── + + /** Sorted list of all pairwise distances of [positions] — a rigid-motion-invariant signature of + * the constellation SHAPE, used to test whether consecutive solves agree (stability gate). */ + private fun shapeSignature(positions: Map): DoubleArray { + val pts = positions.values.toList() + val out = ArrayList(pts.size * (pts.size - 1) / 2) + for (i in pts.indices) for (j in i + 1 until pts.size) out.add(pts[i].distanceTo(pts[j])) + return out.toDoubleArray().also { it.sort() } + } + + /** True if two shape signatures have the same size and every pairwise distance agrees within + * [MofeConfig.anchorConstellationLockStabilityEpsilonMeters]. */ + private fun signaturesAgree(a: DoubleArray, b: DoubleArray): Boolean { + if (a.size != b.size) return false + val eps = config.anchorConstellationLockStabilityEpsilonMeters + for (i in a.indices) if (kotlin.math.abs(a[i] - b[i]) > eps) return false + return true + } + + /** Smallest separation between any two anchors in [positions] — the degeneracy guard (a + * collapsed "two nodes overlapping" solve has a near-zero minimum here). ∞ if <2 anchors. */ + private fun minPairwiseSeparation(positions: Map): Double { + val pts = positions.values.toList() + if (pts.size < 2) return Double.POSITIVE_INFINITY + var min = Double.POSITIVE_INFINITY + for (i in pts.indices) for (j in i + 1 until pts.size) min = minOf(min, pts[i].distanceTo(pts[j])) + return min + } + + /** RMS of |‖p_a−p_b‖ − measured_d| over every measured edge whose endpoints are both in + * [positions] — how well the solved shape fits its own inter-anchor distances. High ⇒ a + * degenerate/wrong solve. NaN if no usable edge (treated as un-lockable by the caller). */ + private fun constellationResidualRms( + positions: Map, distances: Map, Double>, + ): Double { + var sumSq = 0.0; var n = 0 + for ((pair, d) in distances) { + if (d <= 0.0 || !d.isFinite()) continue + val pa = positions[pair.first] ?: continue + val pb = positions[pair.second] ?: continue + val r = pa.distanceTo(pb) - d + sumSq += r * r; n++ + } + return if (n == 0) Double.NaN else kotlin.math.sqrt(sumSq / n) + } + + /** Reset the stability run so a fresh convergence must re-confirm before the frame can lock. */ + private fun resetLockStability() { lastShapeSignature = null; stableSolveTicks = 0 } + // ───────────────────────────────────────────────────────────────────── // Calibration bootstrap (CALIBRATING phase only) // ───────────────────────────────────────────────────────────────────── @@ -527,6 +586,7 @@ class MultiObserverFusionEngine( frameOrientation = null // fresh frame → re-establish orientation frameEstablished = false // …and re-solve the constellation from scratch frameWasLocked = false // fresh frame → next lock is a first establishment, not recovery + resetLockStability() // …and the trust/stability run starts over listener.onQuorumChange(frameManager.quorumStatus) emitOperatingModeIfChanged(previousMode) } @@ -536,6 +596,7 @@ class MultiObserverFusionEngine( frameOrientation = null frameEstablished = false frameWasLocked = false + resetLockStability() peerAssertedMotion.clear() targets.clear() calibrationContexts.clear() @@ -1290,8 +1351,11 @@ class MultiObserverFusionEngine( } // Motion detected on a reference anchor: release the lock and fall through to re-solve. // The degradation is advertised by the re-solve's "resolving-motion" status at the end - // of this method (a single emit per tick), and by [constellationFrameState]. + // of this method (a single emit per tick), and by [constellationFrameState]. Reset the + // stability run so the frame must re-confirm a stable shape before it can re-lock — a + // real relocation must not re-lock on a single post-bump solve. frameEstablished = false + resetLockStability() } val topo = frameManager.topology ?: run { pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "no-topology", 0, edgeCount, 0, emptyList()); return 0 } @@ -1317,7 +1381,10 @@ class MultiObserverFusionEngine( var viaFallback = false val solved = AnchorMeshBootstrap.bootstrap(ids, corrected, rootId) ?: refineConstellationFromPrior(ids, rootId, corrected)?.also { viaFallback = true } - ?: run { pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "solve-null", ids.size, edgeCount, 0, emptyList()); return 0 } + ?: run { + resetLockStability() // no solve this tick — the stability run is broken + pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "solve-null", ids.size, edgeCount, 0, emptyList()); return 0 + } // ORIENT the distance-solved shape into the mesh +Z=up world frame — the one thing // distance geometry cannot supply, since ranges are invariant to rotation and reflection. @@ -1369,16 +1436,35 @@ class MultiObserverFusionEngine( } } emitOperatingModeIfChanged(previousMode) - // ESTABLISH / RE-ESTABLISH the frame lock. Latch once the frame is ORIENTED, COMPLETE, and - // NOT currently in motion. Three cases converge here: first establishment (anchors seated - // this tick, placed > 0); RECOVERY after a motion-triggered re-solve, once the moved anchor - // settles (its final position seated, or no correction needed so placed == 0 — hence the - // ref-count fallback); and healing after an anchor dropped. Never freeze a still-un-levelled - // ("placed-raw", orientation == null) frame, nor one whose anchors are still moving. + // ESTABLISH / RE-ESTABLISH the frame lock — but ONLY on a TRUSTWORTHY solve, so the locked + // shape is REPEATABLE cold-start to cold-start and after a bump re-solve, not whichever + // shape the noisy, timing-dependent edge set produced at the tick that latched first + // (accuracy > speed). The solver is deterministic in its inputs; these gates ensure the + // inputs are worth trusting before we freeze the result: + // • ORIENTED + full set + not mid-motion (as before); + // • COMPLETE — a closed-form K4 solve, NOT the prior-folded fallback (an incomplete + // graph resolves its missing-edge DOF from the noisy AoA prior → non-repeatable); + // • WELL-FIT — the solved shape fits its OWN measured edges tightly (a degenerate/wrong + // solve cannot), far below the fallback's loose 0.5 m acceptance; + // • NON-DEGENERATE — no two anchors collapsed together ("triangular, nodes overlapping"); + // • STABLE — consecutive solves AGREE for N ticks, so we lock a converged, repeatable + // shape rather than a transient from a half-arrived / still-settling edge set. val motionNow = anyReferenceAnchorInMotion() val refCount = frameManager.getAllReferencePoints().size + val residualRms = constellationResidualRms(placedPositions, corrected) + val minSep = minPairwiseSeparation(placedPositions) + // Stability run: does this solve's shape agree with the previous one? (Reset on any unlock.) + val signature = shapeSignature(placedPositions) + val prevSig = lastShapeSignature + stableSolveTicks = + if (prevSig != null && signaturesAgree(prevSig, signature)) stableSolveTicks + 1 else 0 + lastShapeSignature = signature + val trustworthy = !viaFallback && + residualRms.isFinite() && residualRms <= config.anchorConstellationLockMaxResidualMeters && + minSep >= config.anchorMinSeparationMeters && + stableSolveTicks >= config.anchorConstellationLockStableTicks val latchNow = orientation != null && !motionNow && - (placed > 0 || refCount >= config.minAnchorsForFusion) + refCount >= config.minAnchorsForFusion && trustworthy // Recovery = we are re-locking after the frame HAD been locked (a motion release or an // anchor-offline drop broke it and it has now healed) — the "subsequent recovery" signal. val recovered = latchNow && !frameEstablished && frameWasLocked @@ -1388,11 +1474,14 @@ class MultiObserverFusionEngine( // "placed-raw" orientation not yet available (still the arbitrary gauge; also flags // a missing-bearing condition) // "resolving-motion" actively re-solving to track a moving reference anchor (degradation) + // "converging" solved but NOT YET TRUSTED — waiting for a complete, well-fit, + // non-degenerate, stable solve before locking (the accuracy>speed wait) // "reconverged" settled and re-locked after a motion/offline disruption (recovery) - // "placed" first establishment / an ordinary re-seat + // "placed" locked this tick (first establishment / an ordinary re-seat) val baseStatus = when { orientation == null -> "placed-raw" motionNow -> "resolving-motion" + !latchNow && !frameEstablished -> "converging" recovered -> "reconverged" else -> "placed" } @@ -1800,8 +1889,10 @@ class MultiObserverFusionEngine( removeAnchor(anchorId) // The constellation changed — release the hold so the frame re-solves and re-seats the // remaining anchors next tick. The locked orientation is kept (it stays valid for the - // survivors). This is the offline half of the nudge/offline resilience contract. + // survivors). This is the offline half of the nudge/offline resilience contract. Reset + // the stability run: the survivor set must re-confirm a stable shape before re-locking. frameEstablished = false + resetLockStability() } } diff --git a/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt b/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt index 9a84503..0ba5a01 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt @@ -405,6 +405,25 @@ data class MofeConfig( * without resisting the distance-constrained shape. Small. */ val anchorConstellationPriorWeight: Double = 0.005, + // ── Frame-lock TRUST gate (accuracy > speed) ─────────────────────────────── + // The frame LOCK (frameEstablished) freezes the constellation to stop jitter; a + // premature lock freezes a bad shape. These gate the latch so it only ever locks a + // solve that is COMPLETE, well-fit, non-degenerate, and STABLE across ticks — so the + // locked shape is repeatable cold-start to cold-start instead of tracking the noisy, + // timing-dependent edge set at whichever tick happened to latch first. + /** Max RMS inter-anchor distance residual (metres) to accept a solve as lockable. A + * degenerate or wrong-shape solve cannot fit its own measured edges this tightly. */ + val anchorConstellationLockMaxResidualMeters: Double = 0.06, + /** Minimum separation (metres) between any two anchors in a lockable solve — rejects a + * collapsed/degenerate constellation (two nodes nearly overlapping). */ + val anchorMinSeparationMeters: Double = 0.10, + /** Consecutive maintenance ticks whose solved shape must AGREE (within the epsilon + * below) before the frame may lock — waits out transients so the lock is repeatable. */ + val anchorConstellationLockStableTicks: Int = 3, + /** Max change (metres) in any pairwise inter-anchor distance between consecutive solves + * for them to count as "agreeing" for the stability gate. */ + val anchorConstellationLockStabilityEpsilonMeters: Double = 0.02, + /** * Maximum age of a buffered UWB range before it's discarded as stale. * Cross-anchor fusion waits for ranges from multiple anchors with 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 2bc93a0..5aaa23d 100644 --- a/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt @@ -1,6 +1,7 @@ package com.aether.mofe.engine import com.aether.mofe.integration.MofeTestHarness +import com.aether.mofe.model.ConstellationFrameState import com.aether.mofe.model.DeviceId import com.aether.mofe.model.MofeConfig import com.aether.mofe.model.Vector3D @@ -289,9 +290,18 @@ class AnchorMeshBootstrapTest { } } - // ESTABLISH: distance-solve + orient + seat. + // ESTABLISH: distance-solve + orient + seat. The TRUST GATE only locks a solve that is + // complete, well-fit, non-degenerate, AND stable across several consecutive ticks (the + // accuracy>speed hardening), so a single tick seats but does not yet lock — drive + // maintenance on the (consistent) shape until the frame LOCKS. val placed = h.engine.bootstrapReferenceConstellation() - assertTrue(placed > 0, "frame must establish (oriented + seated); placed=$placed") + assertTrue(placed > 0, "frame must place (oriented + seated); placed=$placed") + repeat(12) { + if (h.engine.constellationFrameState() != ConstellationFrameState.LOCKED) + h.engine.bootstrapReferenceConstellation() + } + assertEquals(ConstellationFrameState.LOCKED, h.engine.constellationFrameState(), + "frame must lock once several consistent solves agree") val established = h.frameManager.getAllReferencePoints().associate { it.id to it.position } // HOLD: fresh 2 cm range noise on the settled frame must NOT re-seat any anchor. diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt index 459cea5..ddd4f01 100644 --- a/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt @@ -14,22 +14,14 @@ import kotlin.test.assertEquals import kotlin.test.assertTrue /** - * Regression for the anchor-position JITTER (Issue B) and the frame-lock / motion-override - * contract that fixes it. + * Regression for the anchor-position JITTER (frame lock) AND the mesh-solve NON-DETERMINISM + * hardening (the trust gate): the frame may lock ONLY a solve that is complete, well-fit, + * non-degenerate, and STABLE across ticks — so the locked shape is repeatable cold-start to + * cold-start (accuracy > speed) — and the lock is released and re-solves on anchor motion. * - * Field symptom: a settled anchor's distance oscillated continuously (~10 cm, 4.4 ↔ 4.5 m) with - * the mesh otherwise static. Cause: once the constellation was distance-solved and oriented the - * bootstrap held it, but [MultiObserverFusionEngine.refineAnchorConstellation] kept re-seating - * anchors whenever the slow EMA drift of noisy inter-anchor ranges crept past the (mm) correction - * threshold — so the "settled" frame never actually stopped moving. - * - * The fix LOCKS the converged frame (both bootstrap and refine hold it exactly in place) and - * releases the lock ONLY when a reference anchor's own motion sensor reports movement — a bump, - * jostle, or deliberate relocation — then re-solves to track it and re-locks once it settles. - * These tests drive the real engine path ([MultiObserverFusionEngine]) end to end: - * • a distance-solved, AoA-oriented 4-anchor constellation (so the lock actually latches); - * • sustained noisy inter-anchor drift that WOULD move an unlocked frame; - * • a peer reference anchor asserting motion on the band. + * These drive the real engine ([MultiObserverFusionEngine]) end to end: a distance-solved, + * AoA-oriented 4-anchor constellation with a raised anchor (so orientation, and thus the lock, + * can engage). */ class ConstellationFrameLockTest { @@ -51,25 +43,30 @@ class ConstellationFrameLockTest { private fun refs(h: MofeTestHarness): Map = h.frameManager.getAllReferencePoints().associate { it.id to it.position } - /** Feed every inter-anchor edge at the distance implied by [positions], EMA-converging. */ + /** Feed inter-anchor edges from [positions]; [skip] omits one edge (to model a missing link). */ private fun feedInterAnchor( h: MofeTestHarness, positions: Map = truth, reps: Int = 20, + skip: Pair? = null, + corrupt: Triple? = null, ) { for (i in ids.indices) for (j in i + 1 until ids.size) { - val d = positions.getValue(ids[i]).distanceTo(positions.getValue(ids[j])) - repeat(reps) { h.engine.processInterAnchorRanging(ids[i], ids[j], d) } + val a = ids[i]; val b = ids[j] + if (skip != null && (a to b == skip || b to a == skip)) continue + val d = when { + corrupt != null && (a to b == corrupt.first to corrupt.second || + b to a == corrupt.first to corrupt.second) -> corrupt.third + else -> positions.getValue(a).distanceTo(positions.getValue(b)) + } + repeat(reps) { h.engine.processInterAnchorRanging(a, b, d) } } } - /** - * Feed the ROOT's own AoA to each peer so the orientation solve has ≥2 bearings and can rank - * the raised anchor: A4 reads a clearly-higher elevation than the coplanar A2/A3. This is the - * self-authored path ([MultiObserverFusionEngine.processRanging] → maybeUpdateSelfYaw). - */ + /** Feed the ROOT's own AoA so the orientation solve has ≥2 bearings and can rank the raised + * anchor (A4 reads a clearly-higher elevation than the coplanar A2/A3). */ private fun feedSelfAoa(h: MofeTestHarness) { - val elevations = mapOf(a2 to 0.0, a3 to 0.0, a4 to 0.5) // A4 unambiguously highest + val elevations = mapOf(a2 to 0.0, a3 to 0.0, a4 to 0.5) for (peer in listOf(a2, a3, a4)) { val p = truth.getValue(peer) val az = atan2(p.y, p.x) @@ -87,16 +84,28 @@ class ConstellationFrameLockTest { } } - /** Bring the engine to a fully LOCKED frame: solved, oriented, seated, held. */ - private fun establishLockedFrame(h: MofeTestHarness) { + private fun seedAnchors(h: MofeTestHarness) { h.engine.setSelfId(a1) h.engine.initializeAsRoot(a1) h.engine.registerMeshNode(a2, truth.getValue(a2)) h.engine.registerMeshNode(a3, truth.getValue(a3)) h.engine.registerMeshNode(a4, truth.getValue(a4)) + } + + /** Drive maintenance until the trust gate locks the frame (or a tick cap is hit). */ + private fun driveUntilLocked(h: MofeTestHarness, maxTicks: Int = 16): Boolean { + repeat(maxTicks) { + if (h.engine.constellationFrameState() == ConstellationFrameState.LOCKED) return true + h.engine.bootstrapReferenceConstellation() + } + return h.engine.constellationFrameState() == ConstellationFrameState.LOCKED + } + + private fun establishLockedFrame(h: MofeTestHarness) { + seedAnchors(h) feedSelfAoa(h) feedInterAnchor(h) - repeat(3) { h.engine.bootstrapReferenceConstellation() } + assertTrue(driveUntilLocked(h), "frame should lock on a complete, consistent, stable solve") } // ───────────────────────────────────────────────────────────────────── @@ -105,29 +114,20 @@ class ConstellationFrameLockTest { fun locks_converged_frame_and_holds_it_against_range_drift() { val h = MofeTestHarness(MofeConfig()).build() establishLockedFrame(h) - assertEquals( - ConstellationFrameState.LOCKED, h.engine.constellationFrameState(), - "the frame must LOCK once it is distance-solved and oriented", - ) + assertEquals(ConstellationFrameState.LOCKED, h.engine.constellationFrameState()) val locked = refs(h) - - // Sustained drift: every inter-anchor edge now reads +3 cm (≫ the mm re-seat threshold), - // exactly the slow EMA creep that used to jitter the frame. A LOCKED frame must not move. - val drifted = truth.mapValues { (_, p) -> p * 1.015 } // scale ⇒ every edge grows ~3 cm+ + // Sustained drift (+~3 cm/edge, ≫ the mm re-seat threshold): a LOCKED frame must not move. + val drifted = truth.mapValues { (_, p) -> p * 1.015 } repeat(20) { feedInterAnchor(h, drifted, reps = 4) - assertEquals( - 0, h.engine.refineAnchorConstellation(), - "refineAnchorConstellation must HOLD (re-seat nothing) while the frame is locked", - ) - h.engine.bootstrapReferenceConstellation() // also holds + assertEquals(0, h.engine.refineAnchorConstellation(), + "refineAnchorConstellation must HOLD while the frame is locked") + h.engine.bootstrapReferenceConstellation() } - - val after = refs(h) for (id in ids) { - val moved = locked.getValue(id).distanceTo(after.getValue(id)) - assertTrue(moved < 1e-9, "$id must not move while the frame is locked; moved=${moved} m") + val moved = locked.getValue(id).distanceTo(refs(h).getValue(id)) + assertTrue(moved < 1e-9, "$id must not move while the frame is locked; moved=$moved m") } assertEquals(ConstellationFrameState.LOCKED, h.engine.constellationFrameState()) } @@ -136,48 +136,70 @@ class ConstellationFrameLockTest { fun motion_on_a_reference_anchor_releases_lock_tracks_then_recovers() { val h = MofeTestHarness(MofeConfig()).build() establishLockedFrame(h) - assertEquals(ConstellationFrameState.LOCKED, h.engine.constellationFrameState()) fun solvedA4toA2(): Double = refs(h).getValue(a4).distanceTo(refs(h).getValue(a2)) - val heldSeparation = solvedA4toA2() // frozen old geometry (A4 at 0.6 m) + val heldSeparation = solvedA4toA2() - // A peer reference anchor self-asserts motion on the band → the degradation is advertised - // immediately (before the maintenance tick that formally re-solves). h.engine.applyPeerMotionAssertion(a4, MotionMode.MOVING) - assertEquals( - ConstellationFrameState.RESOLVING_MOTION, h.engine.constellationFrameState(), - "a moving reference anchor must advertise RESOLVING_MOTION", - ) + assertEquals(ConstellationFrameState.RESOLVING_MOTION, h.engine.constellationFrameState(), + "a moving reference anchor must advertise RESOLVING_MOTION") - // A4 has actually moved (lifted 0.6 → 0.95 m); its inter-anchor ranges change to match. - // The released lock must re-solve and TRACK it. + // A4 lifted 0.6 → 0.95 m; its ranges change. The released lock must re-solve and track it. val moved = truth.toMutableMap().apply { put(a4, Vector3D(1.0, 1.0, 0.95)) } feedInterAnchor(h, moved, reps = 25) repeat(3) { h.engine.bootstrapReferenceConstellation() } + val tracked = solvedA4toA2() + assertTrue(tracked - heldSeparation > 0.10, + "the released frame must re-solve to track the moved anchor (held=$heldSeparation, tracked=$tracked)") - val trackedSeparation = solvedA4toA2() - assertTrue( - trackedSeparation - heldSeparation > 0.10, - "the released frame must re-solve to track the moved anchor " + - "(held=${heldSeparation} m, tracked=${trackedSeparation} m)", - ) - assertEquals( - ConstellationFrameState.RESOLVING_MOTION, h.engine.constellationFrameState(), - "still degrading while the anchor is asserted MOVING", - ) - - // Motion ceases → the frame settles and RE-LOCKS: the advertised recovery. + // Motion ceases → the frame settles and RE-LOCKS (the advertised recovery). h.engine.applyPeerMotionAssertion(a4, MotionMode.STATIONARY) - repeat(4) { h.engine.bootstrapReferenceConstellation() } - assertEquals( - ConstellationFrameState.LOCKED, h.engine.constellationFrameState(), - "the frame must re-lock (recover) once motion ceases and it reconverges", - ) - - // And the recovered lock holds the NEW geometry (A4 lifted), not the pre-motion one. - assertTrue( - abs(solvedA4toA2() - trackedSeparation) < 0.05, - "the recovered lock must hold the tracked (moved) geometry", - ) + assertTrue(driveUntilLocked(h), "the frame must re-lock (recover) once motion ceases and it reconverges") + assertTrue(abs(solvedA4toA2() - tracked) < 0.05, "the recovered lock must hold the tracked (moved) geometry") + } + + @Test + fun never_locks_an_incomplete_graph_solve() { + // One inter-anchor link (A2↔A4) never arrives — the closed-form K4 cascade can't run, so + // the engine falls to the prior-folded fallback whose shape is not repeatable. The trust + // gate must therefore NEVER lock it: it stays CONVERGING no matter how long we wait. + val h = MofeTestHarness(MofeConfig()).build() + seedAnchors(h) + feedSelfAoa(h) + feedInterAnchor(h, skip = a2 to a4) + + repeat(20) { h.engine.bootstrapReferenceConstellation() } + assertTrue(h.engine.constellationFrameState() != ConstellationFrameState.LOCKED, + "an incomplete-graph (fallback) solve must never lock; state=${h.engine.constellationFrameState()}") + } + + @Test + fun requires_stability_across_ticks_before_locking() { + // With a complete, consistent, well-fit solve every tick, the frame must STILL wait for + // several agreeing ticks (accuracy > speed) — it must not lock on the very first solve. + val h = MofeTestHarness(MofeConfig()).build() + seedAnchors(h) + feedSelfAoa(h) + feedInterAnchor(h) + + h.engine.bootstrapReferenceConstellation() // first solve + assertTrue(h.engine.constellationFrameState() != ConstellationFrameState.LOCKED, + "must not lock on the very first solve (stability not yet established)") + assertTrue(driveUntilLocked(h), "must lock once several consecutive solves agree") + } + + @Test + fun never_locks_a_grossly_inconsistent_solve() { + // A complete graph but one edge is physically impossible (A2↔A3 forced to 9 m when the + // geometry implies ~2.83 m). No rigid constellation fits, so the residual stays high and + // the trust gate must never lock it. + val h = MofeTestHarness(MofeConfig()).build() + seedAnchors(h) + feedSelfAoa(h) + feedInterAnchor(h, corrupt = Triple(a2, a3, 9.0)) + + repeat(20) { h.engine.bootstrapReferenceConstellation() } + assertTrue(h.engine.constellationFrameState() != ConstellationFrameState.LOCKED, + "a high-residual (inconsistent) solve must never lock; state=${h.engine.constellationFrameState()}") } } -- 2.43.0