Project

General

Profile

Download (34.6 KB)

Bug #52 » 0014-fix-engine-only-LOCK-a-trustworthy-constellation-sol.patch

knight8241, 08/07/2026 18:32

View differences:

common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt
* 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. */
......
/** 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
......
// 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<DeviceId, Vector3D>): DoubleArray {
val pts = positions.values.toList()
val out = ArrayList<Double>(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<DeviceId, Vector3D>): 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<DeviceId, Vector3D>, distances: Map<Pair<DeviceId, DeviceId>, 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)
// ─────────────────────────────────────────────────────────────────────
......
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)
}
......
frameOrientation = null
frameEstablished = false
frameWasLocked = false
resetLockStability()
peerAssertedMotion.clear()
targets.clear()
calibrationContexts.clear()
......
}
// 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 }
......
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.
......
}
}
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
......
// "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"
}
......
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()
}
}
common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt
* 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
common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt
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
......
}
}
// 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.
common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt
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 {
......
private fun refs(h: MofeTestHarness): Map<DeviceId, Vector3D> =
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<DeviceId, Vector3D> = truth,
reps: Int = 20,
skip: Pair<DeviceId, DeviceId>? = null,
corrupt: Triple<DeviceId, DeviceId, Double>? = 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)
......
}
}
/** 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")
}
// ─────────────────────────────────────────────────────────────────────
......
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())
}
......
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()}")
}
}
    (1-1/1)