User Story #51 » 0012-fix-engine-lock-the-converged-anchor-constellation-t.patch
| common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt | ||
|---|---|---|
|
import com.aether.mofe.model.BufferedMofeTelemetryEmitter
|
||
|
import com.aether.mofe.model.CalibrationRequest
|
||
|
import com.aether.mofe.model.CalibrationResult
|
||
|
import com.aether.mofe.model.ConstellationFrameState
|
||
|
import com.aether.mofe.model.DeviceId
|
||
|
import com.aether.mofe.model.DeviceType
|
||
|
import com.aether.mofe.model.EventEvaluationResult
|
||
| ... | ... | |
|
private var frameOrientation: ConstellationOrientation.Result? = null
|
||
|
/** True once the reference frame is fully ESTABLISHED — distance-solved, oriented, and seated.
|
||
|
* While set, [bootstrapReferenceConstellation] stops re-solving the constellation from scratch
|
||
|
* every maintenance tick; that per-tick re-solve re-seated every anchor against fresh range
|
||
|
* noise and jittered the whole frame — exactly the positional instability that blocks the
|
||
|
* precision predicates require. The gentler [refineAnchorConstellation] then maintains the
|
||
|
* frame in place and still tracks a genuinely moved anchor (past its correction threshold).
|
||
|
* Cleared on a deliberate re-establish ([initializeAsRoot] / [reset] / [resetOrientationLock])
|
||
|
* or when an anchor drops out ([handleAnchorOffline]), so the frame re-solves for the new set. */
|
||
|
* This is the FRAME LOCK. While set, BOTH [bootstrapReferenceConstellation] and
|
||
|
* [refineAnchorConstellation] hold the constellation exactly in place and stop re-seating
|
||
|
* anchors: the per-tick re-solve (bootstrap) AND the incremental past-threshold repair
|
||
|
* (refine) each moved anchors against fresh range noise, jittering the whole frame — the
|
||
|
* positional instability the precision predicates cannot tolerate (observed in the field as a
|
||
|
* slow ~cm oscillation of a settled anchor).
|
||
|
* The lock is RELEASED — so the constellation re-solves for the new geometry — when:
|
||
|
* • a reference anchor reports motion ([anyReferenceAnchorInMotion]: this device's own
|
||
|
* ZUPT/IMU via [selfMotionMode], or a peer anchor's band-asserted MOVING mode), covering a
|
||
|
* bump / jostle / deliberate relocation of ANY anchor, the ROOT included; or
|
||
|
* • an anchor drops out ([handleAnchorOffline]); or
|
||
|
* • a deliberate re-establish ([initializeAsRoot] / [reset] / [resetOrientationLock]).
|
||
|
* It re-latches automatically once the frame is oriented, complete, and motion has ceased. */
|
||
|
private var frameEstablished = false
|
||
|
/** Sticky: true once the frame has EVER been locked this session (until a hard reset). Lets the
|
||
|
* diagnostics tell a first establishment ("placed") apart from a RECOVERY ("reconverged") — a
|
||
|
* re-lock after a motion release or an anchor drop broke a previously-locked frame. */
|
||
|
private var frameWasLocked = false
|
||
|
/** 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. */
|
||
|
private val peerAssertedMotion = mutableMapOf<DeviceId, MotionMode>()
|
||
|
/** 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 }
|
||
|
/**
|
||
|
* True if any current reference anchor is reporting motion — the override that releases the
|
||
|
* frame lock so the constellation re-solves to track a moved anchor. The signal is each
|
||
|
* anchor's OWN motion sensor: this device's ZUPT/IMU assessment ([selfMotionMode]) when it is
|
||
|
* itself a reference anchor (covers the root and any self-anchor being bumped), and every peer
|
||
|
* anchor's self-asserted mode received on the band ([peerAssertedMotion]). A moving CLIENT does
|
||
|
* not distort the anchor constellation, so only reference points are considered.
|
||
|
*/
|
||
|
private fun anyReferenceAnchorInMotion(): Boolean {
|
||
|
val refIds = frameManager.getAllReferencePoints().mapTo(HashSet()) { it.id }
|
||
|
if (refIds.isEmpty()) return false
|
||
|
val self = selfTargetId
|
||
|
if (self != null && self in refIds && selfMotionMode() == MotionMode.MOVING) return true
|
||
|
for (id in refIds) {
|
||
|
if (id == self) continue
|
||
|
if (peerAssertedMotion[id] == MotionMode.MOVING) return true
|
||
|
}
|
||
|
return false
|
||
|
}
|
||
|
/**
|
||
|
* The current lifecycle state of the anchor reference constellation, for diagnostics. Pure
|
||
|
* function of the live lock + motion state; advertises frame stability and its degradation
|
||
|
* (LOCKED → RESOLVING_MOTION / RESOLVING) and recovery (→ LOCKED) to the app. See
|
||
|
* [ConstellationFrameState].
|
||
|
*/
|
||
|
fun constellationFrameState(): ConstellationFrameState = when {
|
||
|
// Motion takes precedence over the lock: the instant a reference anchor reports movement
|
||
|
// the frame is degrading (the next maintenance tick releases the lock and re-solves), so
|
||
|
// advertise it immediately rather than a tick late.
|
||
|
anyReferenceAnchorInMotion() -> ConstellationFrameState.RESOLVING_MOTION
|
||
|
frameEstablished -> ConstellationFrameState.LOCKED
|
||
|
frameOrientation != null &&
|
||
|
frameManager.getAllReferencePoints().size >= config.minAnchorsForFusion ->
|
||
|
ConstellationFrameState.RESOLVING
|
||
|
else -> ConstellationFrameState.CONVERGING
|
||
|
}
|
||
|
// ─────────────────────────────────────────────────────────────────────
|
||
|
// 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
|
||
|
listener.onQuorumChange(frameManager.quorumStatus)
|
||
|
emitOperatingModeIfChanged(previousMode)
|
||
|
}
|
||
| ... | ... | |
|
fun reset() {
|
||
|
frameOrientation = null
|
||
|
frameEstablished = false
|
||
|
frameWasLocked = false
|
||
|
peerAssertedMotion.clear()
|
||
|
targets.clear()
|
||
|
calibrationContexts.clear()
|
||
|
observationCollector.reset()
|
||
| ... | ... | |
|
/** Stop tracking [targetId] and release its filters. */
|
||
|
fun deregisterTarget(targetId: DeviceId) {
|
||
|
peerAssertedMotion.remove(targetId)
|
||
|
targets.remove(targetId) ?: return
|
||
|
observationCollector.removeTarget(targetId)
|
||
|
}
|
||
| ... | ... | |
|
frameManager.removeAnchor(anchorId)
|
||
|
observationCollector.removeReferencePoint(anchorId)
|
||
|
observationCollector.removeAnchor(anchorId)
|
||
|
peerAssertedMotion.remove(anchorId)
|
||
|
listener.onAnchorDeparture(anchorId, frameManager.anchorCount)
|
||
|
emitOperatingModeIfChanged(previousMode)
|
||
| ... | ... | |
|
* No-op if [targetId] is not tracked here.
|
||
|
*/
|
||
|
fun applyPeerMotionAssertion(targetId: DeviceId, mode: MotionMode) {
|
||
|
// Retain the asserted mode regardless of whether the peer is tracked as a target yet: a
|
||
|
// reference anchor may be seeded (mesh point) before it becomes an EKF target, and the
|
||
|
// frame-lock override ([anyReferenceAnchorInMotion]) must still see its MOVING assertion.
|
||
|
peerAssertedMotion[targetId] = mode
|
||
|
val ctx = targets[targetId] ?: return
|
||
|
ctx.ekf.motionAssertedStationary = (mode == MotionMode.STATIONARY)
|
||
|
}
|
||
| ... | ... | |
|
* frame version doesn't churn on sub-noise nudges). No-op unless there are enough
|
||
|
* anchors and distinct distance edges.
|
||
|
*
|
||
|
* HELD WHILE THE FRAME IS LOCKED. Once [frameEstablished], this returns immediately without
|
||
|
* re-seating anything: the past-threshold repair was intended to hold a settled frame in place,
|
||
|
* but the slow EMA drift of noisy inter-anchor ranges still crept individual anchors past the
|
||
|
* (millimetre) correction threshold tick after tick, so the "settled" frame never actually
|
||
|
* stopped — the field saw a converged anchor oscillate ~cm continuously. With the lock in force,
|
||
|
* a locked frame is truly still; the ONLY thing that moves it is a genuine anchor motion, which
|
||
|
* releases the lock in [bootstrapReferenceConstellation] (which then owns the re-solve). This
|
||
|
* method therefore runs only BELOW the lock — initial convergence and motion/offline re-solves.
|
||
|
*
|
||
|
* @return the number of anchors whose position was corrected.
|
||
|
*/
|
||
|
fun refineAnchorConstellation(): Int {
|
||
|
if (!config.anchorConstellationRefinementEnabled) return 0
|
||
|
if (frameEstablished) return 0
|
||
|
val topo = frameManager.topology ?: return 0
|
||
|
val rootId = topo.frame.originDeviceId
|
||
|
// The anchor constellation is the FULL reference-point set (the root plus every
|
||
| ... | ... | |
|
// was indistinguishable from "bootstrap ran and did nothing". Emit the decision (and the
|
||
|
// distance solution when it places) at every exit so the recorder proves which branch ran.
|
||
|
val edgeCount = interAnchorDistances.size
|
||
|
// ESTABLISHED → HOLD. The frame is already distance-solved, oriented and seated; re-solving
|
||
|
// the constellation from scratch on every ~2 s tick re-seated the anchors against fresh
|
||
|
// range noise and jittered the whole frame — the instability that blocks predicate
|
||
|
// precision. Stop here and let [refineAnchorConstellation] maintain it in place: it moves an
|
||
|
// anchor only past the correction threshold, so a settled frame stops moving yet a real
|
||
|
// nudge is still tracked. Released on a deliberate re-establish or an anchor dropping (see
|
||
|
// [frameEstablished]). Emit the held frame so a capture still shows the live positions.
|
||
|
// ESTABLISHED → HOLD (unless a reference anchor is moving). The frame is already distance-
|
||
|
// solved, oriented and seated; re-solving the constellation from scratch on every ~2 s tick
|
||
|
// re-seated the anchors against fresh range noise and jittered the whole frame — the
|
||
|
// instability that blocks predicate precision. While locked and STILL, hold exactly in
|
||
|
// place (both this and [refineAnchorConstellation] no-op), so a settled frame stops moving.
|
||
|
// MOTION OVERRIDE: the moment a reference anchor's own sensor reports movement — a bump,
|
||
|
// jostle, or deliberate relocation, the ROOT included — release the lock and fall through
|
||
|
// to re-solve, so the frame tracks the moved anchor and heals. The lock re-latches once the
|
||
|
// frame reconverges and motion ceases (see the latch at the end of this method). Also
|
||
|
// released on an anchor dropping / a deliberate re-establish (see [frameEstablished]).
|
||
|
if (frameEstablished) {
|
||
|
val held = frameManager.getAllReferencePoints()
|
||
|
pipelineTrace?.constellationBootstrapped(
|
||
|
clock.now().microseconds, "held", held.size, edgeCount, 0,
|
||
|
held.map { MofePipelineTrace.RefPoint(it.id, it.position) },
|
||
|
)
|
||
|
return 0
|
||
|
if (!anyReferenceAnchorInMotion()) {
|
||
|
val held = frameManager.getAllReferencePoints()
|
||
|
pipelineTrace?.constellationBootstrapped(
|
||
|
clock.now().microseconds, "held", held.size, edgeCount, 0,
|
||
|
held.map { MofePipelineTrace.RefPoint(it.id, it.position) },
|
||
|
)
|
||
|
return 0
|
||
|
}
|
||
|
// 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].
|
||
|
frameEstablished = false
|
||
|
}
|
||
|
val topo = frameManager.topology
|
||
|
?: run { pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "no-topology", 0, edgeCount, 0, emptyList()); return 0 }
|
||
| ... | ... | |
|
}
|
||
|
}
|
||
|
emitOperatingModeIfChanged(previousMode)
|
||
|
// ESTABLISH the hold once the frame is both ORIENTED and seated: from here it is held and
|
||
|
// handed to [refineAnchorConstellation] (see the guard at the top of this method). Latch
|
||
|
// only with a real orientation — never freeze a still-un-levelled ("placed-raw") frame —
|
||
|
// and at least one seated anchor, so the applied transform has actually taken.
|
||
|
if (orientation != null && placed > 0) frameEstablished = true
|
||
|
// 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.
|
||
|
val motionNow = anyReferenceAnchorInMotion()
|
||
|
val refCount = frameManager.getAllReferencePoints().size
|
||
|
val latchNow = orientation != null && !motionNow &&
|
||
|
(placed > 0 || refCount >= config.minAnchorsForFusion)
|
||
|
// 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
|
||
|
if (latchNow) { frameEstablished = true; frameWasLocked = true }
|
||
|
// Emit the ORIENTED positions so a capture verifies the levelling directly (raised anchor
|
||
|
// at +z, base at ~0). "placed" = levelled + headed; "placed-raw" = orientation not yet
|
||
|
// available (still the arbitrary gauge), which itself flags a missing-bearing condition.
|
||
|
val baseStatus = if (orientation != null) "placed" else "placed-raw"
|
||
|
// at +z, base at ~0). Status advertises the frame-lifecycle transition to the recorder:
|
||
|
// "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)
|
||
|
// "reconverged" settled and re-locked after a motion/offline disruption (recovery)
|
||
|
// "placed" first establishment / an ordinary re-seat
|
||
|
val baseStatus = when {
|
||
|
orientation == null -> "placed-raw"
|
||
|
motionNow -> "resolving-motion"
|
||
|
recovered -> "reconverged"
|
||
|
else -> "placed"
|
||
|
}
|
||
|
pipelineTrace?.constellationBootstrapped(
|
||
|
clock.now().microseconds, if (viaFallback) "$baseStatus-degen" else baseStatus,
|
||
|
ids.size, edgeCount, placed,
|
||
| common/src/commonMain/kotlin/com/aether/mofe/model/StateTypes.kt | ||
|---|---|---|
|
POOR
|
||
|
}
|
||
|
/**
|
||
|
* Lifecycle of the anchor reference constellation (the frame the whole mesh solves against),
|
||
|
* surfaced as a diagnostic so the app can advertise frame stability and its degradation/recovery.
|
||
|
*
|
||
|
* [CONVERGING] the frame is still being solved from inter-anchor geometry — no stable lock yet.
|
||
|
* [LOCKED] solved, oriented, seated, and HELD in place; the steady state (no per-tick re-seat,
|
||
|
* so anchor positions stop jittering on range noise).
|
||
|
* [RESOLVING_MOTION] a reference anchor's own motion sensor (ZUPT/IMU, self OR a peer's band-
|
||
|
* asserted mode) reports MOVING — a bump, jostle, or deliberate relocation, the ROOT
|
||
|
* included. The lock is released and the constellation actively re-solves to track it.
|
||
|
* [RESOLVING] the reference set changed for another reason (an anchor dropped out) or the frame is
|
||
|
* settling back toward a lock after motion; re-solving the survivors.
|
||
|
*
|
||
|
* A transition LOCKED → RESOLVING_MOTION/RESOLVING is the advertised degradation; the return to
|
||
|
* LOCKED is the advertised recovery.
|
||
|
*/
|
||
|
enum class ConstellationFrameState {
|
||
|
CONVERGING,
|
||
|
LOCKED,
|
||
|
RESOLVING_MOTION,
|
||
|
RESOLVING,
|
||
|
}
|
||
|
/** Inter-anchor ranging measurement for drift detection. */
|
||
|
data class InterAnchorRanging(
|
||
|
val anchor1Id: DeviceId,
|
||
| common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.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.MotionMode
|
||
|
import com.aether.mofe.model.RawRangingMeasurement
|
||
|
import com.aether.mofe.model.Vector3D
|
||
|
import kotlin.math.abs
|
||
|
import kotlin.math.atan2
|
||
|
import kotlin.test.Test
|
||
|
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.
|
||
|
*
|
||
|
* 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.
|
||
|
*/
|
||
|
class ConstellationFrameLockTest {
|
||
|
private val a1 = DeviceId("A1") // root / self (origin)
|
||
|
private val a2 = DeviceId("A2")
|
||
|
private val a3 = DeviceId("A3")
|
||
|
private val a4 = DeviceId("A4") // raised on a stand (gives the frame an unambiguous "up")
|
||
|
/** Ground truth (metres): A1..A3 coplanar on z = 0, A4 raised 0.6 m. */
|
||
|
private val truth = mapOf(
|
||
|
a1 to Vector3D(0.0, 0.0, 0.0),
|
||
|
a2 to Vector3D(2.0, 0.0, 0.0),
|
||
|
a3 to Vector3D(0.0, 2.0, 0.0),
|
||
|
a4 to Vector3D(1.0, 1.0, 0.6),
|
||
|
)
|
||
|
private val ids = listOf(a1, a2, a3, a4)
|
||
|
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. */
|
||
|
private fun feedInterAnchor(
|
||
|
h: MofeTestHarness,
|
||
|
positions: Map<DeviceId, Vector3D> = truth,
|
||
|
reps: Int = 20,
|
||
|
) {
|
||
|
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) }
|
||
|
}
|
||
|
}
|
||
|
/**
|
||
|
* 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).
|
||
|
*/
|
||
|
private fun feedSelfAoa(h: MofeTestHarness) {
|
||
|
val elevations = mapOf(a2 to 0.0, a3 to 0.0, a4 to 0.5) // A4 unambiguously highest
|
||
|
for (peer in listOf(a2, a3, a4)) {
|
||
|
val p = truth.getValue(peer)
|
||
|
val az = atan2(p.y, p.x)
|
||
|
repeat(3) {
|
||
|
h.clock.advance(10_000)
|
||
|
h.engine.processRanging(
|
||
|
RawRangingMeasurement(
|
||
|
anchorId = a1, targetId = peer, timestamp = h.clock.now(),
|
||
|
distance = truth.getValue(a1).distanceTo(p),
|
||
|
azimuth = az, elevation = elevations.getValue(peer),
|
||
|
signalQuality = 1.0, rssi = -55.0,
|
||
|
),
|
||
|
)
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
/** Bring the engine to a fully LOCKED frame: solved, oriented, seated, held. */
|
||
|
private fun establishLockedFrame(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))
|
||
|
feedSelfAoa(h)
|
||
|
feedInterAnchor(h)
|
||
|
repeat(3) { h.engine.bootstrapReferenceConstellation() }
|
||
|
}
|
||
|
// ─────────────────────────────────────────────────────────────────────
|
||
|
@Test
|
||
|
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",
|
||
|
)
|
||
|
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+
|
||
|
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
|
||
|
}
|
||
|
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")
|
||
|
}
|
||
|
assertEquals(ConstellationFrameState.LOCKED, h.engine.constellationFrameState())
|
||
|
}
|
||
|
@Test
|
||
|
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)
|
||
|
// 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",
|
||
|
)
|
||
|
// 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.
|
||
|
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 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.
|
||
|
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",
|
||
|
)
|
||
|
}
|
||
|
}
|
||