From 8ab396e0f7e803efad85bc3a484f000d58af7149 Mon Sep 17 00:00:00 2001 From: Claude Date: Sun, 2 Aug 2026 19:52:16 +0000 Subject: [PATCH] fix(engine): lock the converged anchor constellation to stop position jitter, with a motion override MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Field report (Issue B): a settled anchor's inter-node distance oscillated continuously (~10 cm, 4.4 <-> 4.5 m) with the mesh otherwise static — not sub-centimetre, so it needed pinning. Root cause: once the constellation was distance-solved and oriented, the bootstrap held it, but refineAnchorConstellation kept re-seating anchors whenever the slow EMA drift of noisy inter-anchor ranges crept past the millimetre correction threshold — so the "settled" frame never actually stopped moving. The threshold bounds a single step, not the accumulated drift, so it tracked the noise indefinitely. Fix — lock the frame once solved, with an on-device motion override: * refineAnchorConstellation() now HOLDS while the frame is established (returns 0, re-seats nothing) — this was the residual jitter source. Bootstrap already held; both now hold a locked frame exactly in place. * MOTION OVERRIDE: the lock is released the instant a reference anchor's own motion sensor reports movement — this device's ZUPT/IMU (selfMotionMode) when it is itself a reference anchor (the ROOT included), or a peer anchor's self-asserted MOVING mode received on the band. On release the constellation re-solves to track the moved anchor (bump / jostle / deliberate relocation) and re-latches once it settles. * Resilience: the existing anchor-offline path already releases and re-solves the survivors; the re-latch heals both cases. Diagnostics (advertise degradation + recovery): * New ConstellationFrameState { CONVERGING, LOCKED, RESOLVING_MOTION, RESOLVING } exposed via MultiObserverFusionEngine.constellationFrameState(). * Pipeline-recorder statuses: "resolving-motion" (tracking a moving anchor) and "reconverged" (settled + re-locked after a disruption). Scope: this is the steady-state (oriented, locked) jitter the field reported. A frame that never achieves a confident level orientation stays "placed-raw" and continues to re-solve — a separate missing-bearing condition already surfaced by that status. Verified: new ConstellationFrameLockTest (frame locks and holds against sustained range drift; a moving reference anchor releases the lock, the frame tracks the move, then recovers to LOCKED). Full :common:jvmTest green (669 passed, 0 failed). Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../mofe/engine/MultiObserverFusionEngine.kt | 157 ++++++++++++--- .../com/aether/mofe/model/StateTypes.kt | 23 +++ .../mofe/engine/ConstellationFrameLockTest.kt | 183 ++++++++++++++++++ 3 files changed, 335 insertions(+), 28 deletions(-) create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt 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 fcb05ac..00eed91 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt @@ -8,6 +8,7 @@ import com.aether.mofe.model.mesh.AnchorStatus 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 @@ -396,20 +397,74 @@ class MultiObserverFusionEngine( 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() + /** 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) // ───────────────────────────────────────────────────────────────────── @@ -471,6 +526,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 listener.onQuorumChange(frameManager.quorumStatus) emitOperatingModeIfChanged(previousMode) } @@ -479,6 +535,8 @@ class MultiObserverFusionEngine( fun reset() { frameOrientation = null frameEstablished = false + frameWasLocked = false + peerAssertedMotion.clear() targets.clear() calibrationContexts.clear() observationCollector.reset() @@ -519,6 +577,7 @@ class MultiObserverFusionEngine( /** Stop tracking [targetId] and release its filters. */ fun deregisterTarget(targetId: DeviceId) { + peerAssertedMotion.remove(targetId) targets.remove(targetId) ?: return observationCollector.removeTarget(targetId) } @@ -749,6 +808,7 @@ class MultiObserverFusionEngine( frameManager.removeAnchor(anchorId) observationCollector.removeReferencePoint(anchorId) observationCollector.removeAnchor(anchorId) + peerAssertedMotion.remove(anchorId) listener.onAnchorDeparture(anchorId, frameManager.anchorCount) emitOperatingModeIfChanged(previousMode) @@ -1008,6 +1068,10 @@ class MultiObserverFusionEngine( * 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) } @@ -1071,10 +1135,20 @@ class MultiObserverFusionEngine( * 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 @@ -1195,20 +1269,29 @@ class MultiObserverFusionEngine( // 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 } @@ -1286,15 +1369,33 @@ class MultiObserverFusionEngine( } } 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, diff --git a/common/src/commonMain/kotlin/com/aether/mofe/model/StateTypes.kt b/common/src/commonMain/kotlin/com/aether/mofe/model/StateTypes.kt index 48d1bd7..deb6fc4 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/model/StateTypes.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/model/StateTypes.kt @@ -63,6 +63,29 @@ enum class GdopQuality { 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, diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt new file mode 100644 index 0000000..459cea5 --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationFrameLockTest.kt @@ -0,0 +1,183 @@ +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 = + 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 = 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", + ) + } +} -- 2.43.0