From d005eae71476c60f81e93807f84c53f8fe90b6f2 Mon Sep 17 00:00:00 2001 From: Claude Date: Tue, 4 Aug 2026 02:29:43 +0000 Subject: [PATCH 3/5] =?UTF-8?q?feat(engine):=20YawReconcileGate=20?= =?UTF-8?q?=E2=80=94=20hold=20the=20view-yaw=20correction=20through=20moti?= =?UTF-8?q?on=20(F2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit On-device the FPV lens stopped tracking the anchors while the user aimed/walked. Root cause: the view-layer self-yaw correction was reset to 0 on EVERY non-LOCKED tick. A magnetometer-less phone spends much of the time aiming in the frame's RESOLVING_MOTION state (the motion override of a previously-locked frame) — exactly when gyro yaw drift is worst — so the correction vanished the moment the user moved and the anchors slid off the lens. YawReconcileGate.decide(frameState) makes the policy explicit and pure: - LOCKED -> RECONCILE (fold fresh AoA consensus in) - RESOLVING_MOTION -> HOLD (keep the last correction; a held ~correct offset beats snapping to 0 while moving, and the bearings are motion-corrupted anyway) - CONVERGING/RESOLVING -> RESET (frame (re)establishing; forget any stale offset) HOLD leaves YawDriftReconciler.initialized set, so a re-LOCK resumes low-passing from the held value with no re-snap jump. View-layer only — never touches the EKF, so it can't reintroduce the aim jitter that retired the old absolute-yaw-into-EKF path. Pure common, 5 jvmTests incl. a modeled host loop proving HOLD preserves the correction across a motion episode where the old reset-to-0 would have dropped it. Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../aether/mofe/engine/YawReconcileGate.kt | 41 +++++++++ .../mofe/engine/YawReconcileGateTest.kt | 83 +++++++++++++++++++ 2 files changed, 124 insertions(+) create mode 100644 common/src/commonMain/kotlin/com/aether/mofe/engine/YawReconcileGate.kt create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/YawReconcileGateTest.kt diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/YawReconcileGate.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/YawReconcileGate.kt new file mode 100644 index 0000000..1fa98d2 --- /dev/null +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/YawReconcileGate.kt @@ -0,0 +1,41 @@ +package com.aether.mofe.engine + +import com.aether.mofe.model.ConstellationFrameState + +/** What the view-layer self-yaw reconcile should do this tick (see [YawReconcileGate]). */ +enum class YawReconcileAction { RESET, HOLD, RECONCILE } + +/** + * Decides what the slow view-layer self-yaw reconcile ([YawDriftReconciler]) does for a given + * constellation frame state — pure so the policy is unit-testable away from the engine host. + * + * WHY (F2 · lens accuracy under motion). The reconcile corrects the residual absolute-yaw drift the + * gyro-integrated FPV attitude accumulates. The first cut engaged ONLY on a [ConstellationFrameState.LOCKED] + * frame and snapped the correction to 0 on every other state. But a hand-held, magnetometer-less phone + * spends much of the time AIMING/WALKING in [ConstellationFrameState.RESOLVING_MOTION] (the motion + * override of a previously-locked frame) — exactly when yaw drift is worst — so the correction kept + * vanishing to 0 the moment the user moved, and the anchors slid off the lens. That is the on-device + * "the lens doesn't track the anchors" symptom. + * + * Policy: + * - [ConstellationFrameState.LOCKED] → RECONCILE: trustworthy heading, fold fresh AoA consensus in. + * - [ConstellationFrameState.RESOLVING_MOTION] → HOLD: the frame was locked and the observer is just + * moving; the last correction is still approximately valid, so KEEP it (a held ~correct offset beats + * snapping to 0 and letting the view slide) rather than reconciling against motion-corrupted bearings. + * - [ConstellationFrameState.CONVERGING] / [ConstellationFrameState.RESOLVING] → RESET: the frame is + * (re)establishing and there is no trustworthy heading, so forget the correction — a re-solve or mesh + * switch must never leave a stale offset on the view. + * + * HOLD keeps [YawDriftReconciler.initialized] set, so when the frame re-locks the reconcile resumes + * low-passing from the held value instead of re-snapping — no visible jump across a motion episode. + * View-layer only: this never touches the EKF, so it cannot reintroduce the aim jitter that retired the + * old absolute-yaw-into-EKF path. Sustained motion escalates RESOLVING_MOTION → RESOLVING, which resets. + */ +object YawReconcileGate { + fun decide(frame: ConstellationFrameState): YawReconcileAction = when (frame) { + ConstellationFrameState.LOCKED -> YawReconcileAction.RECONCILE + ConstellationFrameState.RESOLVING_MOTION -> YawReconcileAction.HOLD + ConstellationFrameState.CONVERGING -> YawReconcileAction.RESET + ConstellationFrameState.RESOLVING -> YawReconcileAction.RESET + } +} diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/YawReconcileGateTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/YawReconcileGateTest.kt new file mode 100644 index 0000000..5658c6b --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/YawReconcileGateTest.kt @@ -0,0 +1,83 @@ +package com.aether.mofe.engine + +import com.aether.mofe.model.ConstellationFrameState +import kotlin.math.PI +import kotlin.math.abs +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.test.assertTrue + +/** + * F2 · lens accuracy under motion. [YawReconcileGate] decides whether the view-layer self-yaw + * reconcile reconciles, HOLDs, or resets for each constellation frame state. The mapping tests pin the + * policy; the modeled-loop test proves the behavioural point — HOLD keeps a good correction across a + * motion episode where the old reset-to-0 would have dropped it (the on-device "lens slides while I + * move" symptom). + */ +class YawReconcileGateTest { + + @Test + fun locked_reconciles() { + assertEquals(YawReconcileAction.RECONCILE, YawReconcileGate.decide(ConstellationFrameState.LOCKED)) + } + + @Test + fun motion_holds_not_resets() { + // The crux: moving (the motion override of a locked frame) must NOT zero the correction. + assertEquals(YawReconcileAction.HOLD, YawReconcileGate.decide(ConstellationFrameState.RESOLVING_MOTION)) + } + + @Test + fun establishing_states_reset() { + assertEquals(YawReconcileAction.RESET, YawReconcileGate.decide(ConstellationFrameState.CONVERGING)) + assertEquals(YawReconcileAction.RESET, YawReconcileGate.decide(ConstellationFrameState.RESOLVING)) + } + + @Test + fun every_frame_state_is_mapped() { + // Exhaustive: no state falls through (guards against a new enum value being silently dropped). + for (state in ConstellationFrameState.entries) { + YawReconcileGate.decide(state) // throws on an unmapped state if `when` ever loses exhaustiveness + } + assertEquals(4, ConstellationFrameState.entries.size, "new frame state? decide() must handle it") + } + + // ── modeled host loop: hold-through-motion preserves the correction ─────────────────────────── + + /** Model of MofeEngineHost.reconcileSelfYaw's per-tick decision, driven by the gate. */ + private fun tick( + rec: YawDriftReconciler, + frame: ConstellationFrameState, + errorRad: Double, + spreadRad: Double = 0.0, + dt: Double = 0.096, + ): Double = when (YawReconcileGate.decide(frame)) { + YawReconcileAction.RESET -> { rec.reset(); 0.0 } + YawReconcileAction.HOLD -> rec.correctionRad // keep the last correction untouched + YawReconcileAction.RECONCILE -> rec.observe(errorRad, dt, spreadRad) + } + + @Test + fun hold_through_motion_keeps_the_correction_reset_only_on_reestablish() { + val rec = YawDriftReconciler() + val err = 15.0 * PI / 180.0 + + // LOCKED: first consensus snaps the correction onto the ~15° error. + val afterLock = tick(rec, ConstellationFrameState.LOCKED, err) + assertTrue(abs(afterLock - err) < 1e-9, "locks onto the error (got ${afterLock * 180 / PI}°)") + + // RESOLVING_MOTION for several ticks (the user is aiming/walking): correction HELD, not zeroed. + var c = afterLock + repeat(5) { c = tick(rec, ConstellationFrameState.RESOLVING_MOTION, errorRad = 0.0) } + assertTrue(abs(c - err) < 1e-9, "held through motion (got ${c * 180 / PI}°), NOT snapped to 0") + + // Back to LOCKED: reconcile RESUMES from the held value (no re-snap jump) — still ~15°. + val afterRelock = tick(rec, ConstellationFrameState.LOCKED, err) + assertTrue(abs(afterRelock - err) < 1e-6, "resumes from held value, no jump") + + // A genuine re-establish forgets it, so a re-solve/mesh switch leaves no stale offset. + val afterResolve = tick(rec, ConstellationFrameState.RESOLVING, errorRad = err) + assertEquals(0.0, afterResolve, "reset on re-establish") + assertTrue(!rec.initialized, "reset clears initialization so the next lock re-snaps cleanly") + } +} -- 2.43.0