User Story #51 » 0014-fix-engine-only-LOCK-a-trustworthy-constellation-sol.patch
| 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()}")
|
||
|
}
|
||
|
}
|
||
- « Previous
- 1
- 2
- Next »