From 262227950dd7221bcd6f600c3ad999cd39c6c592 Mon Sep 17 00:00:00 2001 From: Claude Date: Mon, 3 Aug 2026 18:03:25 +0000 Subject: [PATCH 08/10] feat: apply the self-yaw reconcile to the FPV view (F2 wiring) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit F2b — wire the F2 engine core into the live pipeline and apply it to the view. - SelfHeadingObserver.fusedYawError → public, returning a public YawConsensus (errorRad/spreadRad/anchorCount) so the client can consume it without exposing the internal SelfYawFusion consensus type. - MofeEngineHost.reconcileSelfYaw(): on the engine thread at ~10 Hz, fuse the observer's self-AoA sightings to the SOLVED reference anchors into a robust heading-error consensus and fold it slowly into a YawDriftReconciler; expose selfYawCorrectionRad(). Gated on a LOCKED frame (resets to 0 otherwise); holds the last correction when a consensus momentarily drops. The EKF's own per-bearing anchor is left intact, so this corrects only the RESIDUAL drift. - HudViewModel.latestFacing(): rotate the facing by −correction about world up at the single facing source, so the FPV camera AND the aim ray share one consistent, drift-corrected ray. Identity until the frame locks with a ≥2-anchor consensus, so it is byte-identical to today in the common case. Engine core (SelfYawFusion/SelfHeadingObserver/YawDriftReconciler) is jvmTest-verified; this wiring is a reviewed androidApp change (not compilable headless) pending on-device validation (F4). Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../aether/mofe/platform/MofeEngineHost.kt | 49 +++++++++++++++++++ .../com/aether/mofe/viewmodel/HudViewModel.kt | 21 +++++++- .../aether/mofe/engine/SelfHeadingObserver.kt | 20 ++++++-- .../aether/mofe/engine/YawDriftReconciler.kt | 5 +- .../mofe/engine/SelfHeadingObserverTest.kt | 4 +- 5 files changed, 88 insertions(+), 11 deletions(-) diff --git a/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt b/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt index 87043fc..b3ae0d9 100644 --- a/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt +++ b/androidApp/src/main/java/com/aether/mofe/platform/MofeEngineHost.kt @@ -7,6 +7,8 @@ import com.aether.mofe.engine.MofeBuilder import com.aether.mofe.engine.MofeListener import com.aether.mofe.engine.MultiObserverFusionEngine import com.aether.mofe.engine.MultilaterationSolver +import com.aether.mofe.engine.SelfHeadingObserver +import com.aether.mofe.engine.YawDriftReconciler import com.aether.mofe.model.AnchorHealth import com.aether.mofe.model.ConstellationFrameState import com.aether.mofe.model.DeviceId @@ -34,6 +36,7 @@ import kotlinx.coroutines.launch import kotlinx.coroutines.sync.Mutex import kotlinx.coroutines.sync.withLock import kotlinx.coroutines.withContext +import kotlin.math.cos import kotlin.time.Duration.Companion.milliseconds /** @@ -217,6 +220,7 @@ class MofeEngineHost( // ~60 Hz on the engine thread so the aiming ray and first-person camera // track the hand smoothly, independent of the slower UWB solve cadence. scope.launch(engineDispatcher) { + var yawTick = 0 while (true) { _selfPose = runCatching { eng.getState(selfId) }.getOrNull() // Cache the self motion mode on the engine thread so selfMotionMode() @@ -227,6 +231,9 @@ class MofeEngineHost( // Self-attitude spin diagnostic: throttled to ~4 Hz (see maybeEmitSelfDiag). // On the engine thread so it reads a coherent EKF snapshot. maybeEmitSelfDiag(selfId) + // F2: slow view-layer yaw-drift reconcile at ~10 Hz (every 6th 16 ms tick), + // on the engine thread so it reads a coherent AoA + reference snapshot. + if (yawTick++ % 6 == 0) reconcileSelfYaw(eng) kotlinx.coroutines.delay(16.milliseconds) } } @@ -258,6 +265,48 @@ class MofeEngineHost( @Volatile private var _selfPose: FusedState? = null fun latestSelfPose(): FusedState? = _selfPose + // ── F2: view-layer self-yaw drift reconcile ──────────────────────────────── + // A magnetometer-less phone has no absolute yaw reference, so its gyro-integrated heading + // drifts and the FPV view slides under motion. This walks the drift out SLOWLY at the VIEW + // layer (never the EKF — feeding AoA yaw into the EKF jittered the aim and was removed): a + // robust multi-anchor AoA consensus of the heading error, low-passed with a hard anti-jerk + // clamp (see YawDriftReconciler). Engine-thread only; the EKF's own per-bearing anchor is left + // intact, so this only corrects the RESIDUAL drift the EKF misses (chiefly during motion). + private val yawReconciler = YawDriftReconciler() + @Volatile private var _selfYawCorrectionRad = 0.0 + /** Slowly-reconciled absolute-yaw correction (rad): ROTATE the predicted self facing by −this + * about world up to counter gyro yaw drift. 0 until the frame is LOCKED with a ≥2-anchor AoA + * consensus, so it never engages before there is a trustworthy heading reference. */ + fun selfYawCorrectionRad(): Double = _selfYawCorrectionRad + + /** One ~10 Hz reconcile tick: fuse the fresh self-AoA sightings to the solved reference anchors + * into a robust heading-error consensus, and fold it slowly into [yawReconciler]. Gated on a + * LOCKED frame; resets (correction → 0) whenever the frame is not locked, so a re-solve or mesh + * switch never leaves a stale correction on the view. Runs on the engine dispatcher. */ + private fun reconcileSelfYaw(eng: MultiObserverFusionEngine) { + val self = _selfPose + val locked = runCatching { eng.constellationFrameState() }.getOrNull() == ConstellationFrameState.LOCKED + if (self == null || !locked) { + yawReconciler.reset(); _selfYawCorrectionRad = 0.0; return + } + val az = runCatching { eng.selfAoaBearings() }.getOrDefault(emptyMap()) + val el = runCatching { eng.selfAoaElevations() }.getOrDefault(emptyMap()) + val anchors = runCatching { eng.referencePointPositions() }.getOrNull().orEmpty().toMap() + val selfPos = self.position + val sightings = az.mapNotNull { (id, a) -> + val e = el[id] ?: return@mapNotNull null + val pos = anchors[id] ?: return@mapNotNull null + val dir = pos - selfPos + if (dir.magnitude < 1e-6) null + else SelfHeadingObserver.Sighting(dir, a, e, weight = (cos(a) * cos(e)).coerceAtLeast(0.05)) + } + // No ≥2-anchor consensus this tick ⇒ HOLD the last correction (don't snap to 0 — the frame + // is still locked, anchors are just momentarily un-fused). Reset only happens on unlock above. + val c = SelfHeadingObserver.fusedYawError(self.orientation, sightings) ?: return + yawReconciler.observe(c.errorRad, dtSeconds = 0.096, spreadRad = c.spreadRad) + _selfYawCorrectionRad = yawReconciler.correctionRad + } + // ── Self-attitude "spin" diagnostic ─────────────────────────────────────── // The first-person / orbit view is rendered by rotating the (stable) mesh into // THIS device's fused attitude — so an on-screen spin of a still device is a diff --git a/androidApp/src/main/java/com/aether/mofe/viewmodel/HudViewModel.kt b/androidApp/src/main/java/com/aether/mofe/viewmodel/HudViewModel.kt index 17c72a8..370baff 100644 --- a/androidApp/src/main/java/com/aether/mofe/viewmodel/HudViewModel.kt +++ b/androidApp/src/main/java/com/aether/mofe/viewmodel/HudViewModel.kt @@ -125,7 +125,8 @@ class HudViewModel(application: Application) : AndroidViewModel(application) { val fy = 2.0 * (w * x - y * z) val fz = 2.0 * (x * x + y * y) - 1.0 val m = kotlin.math.sqrt(fx * fx + fy * fy + fz * fz) - return if (m > 1e-6f) Vector3D(fx / m, fy / m, fz / m) else Vector3D(0.0, 0.0, -1.0) + val raw = if (m > 1e-6f) Vector3D(fx / m, fy / m, fz / m) else Vector3D(0.0, 0.0, -1.0) + return yawDriftCorrected(raw) } /** @@ -141,7 +142,23 @@ class HudViewModel(application: Application) : AndroidViewModel(application) { val fy = 1.0 - 2.0 * (x * x + z * z) val fz = 2.0 * (y * z + w * x) val m = kotlin.math.sqrt(fx * fx + fy * fy + fz * fz) - return if (m > 1e-6f) Vector3D(fx / m, fy / m, fz / m) else Vector3D(0.0, 1.0, 0.0) + val raw = if (m > 1e-6f) Vector3D(fx / m, fy / m, fz / m) else Vector3D(0.0, 1.0, 0.0) + return yawDriftCorrected(raw) + } + + /** + * F2: counter the EKF's RESIDUAL yaw drift by rotating a world-frame body vector by −correction + * about world up (+Z). The correction is 0 until the frame LOCKS with an anchor AoA consensus, + * so this is identity in the common case and only engages to hold a moving observer's view + * steady. Applied to BOTH the aim/lens ([latestFacing]) and the top-edge ([latestTopEdge]) — the + * whole attitude shares one yaw error — so the FPV camera and the aim basis stay consistent, not + * just the forward ray. A pure yaw rotation leaves the vertical component untouched. + */ + private fun yawDriftCorrected(v: Vector3D): Vector3D { + val c = app.mofeEngineHost.selfYawCorrectionRad() + if (c == 0.0) return v + val cs = kotlin.math.cos(c); val sn = kotlin.math.sin(c) + return Vector3D(v.x * cs + v.y * sn, -v.x * sn + v.y * cs, v.z) } /** diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt index d2a5c73..8e59283 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt @@ -35,6 +35,13 @@ object SelfHeadingObserver { val weight: Double = 1.0, ) + /** + * A robust yaw-error consensus for the caller — the public summary that hides the internal + * fusion type. Feed [errorRad] + [spreadRad] into [YawDriftReconciler.observe]; [anchorCount] + * is the number of anchors that survived outlier rejection (a confidence/gating signal). + */ + data class YawConsensus(val errorRad: Double, val spreadRad: Double, val anchorCount: Int) + /** Per-anchor yaw error e = wrapPi(measuredHeading − trueBearing) for [attitude] + [s]. */ fun yawError( attitude: Quaternion, @@ -50,17 +57,20 @@ object SelfHeadingObserver { /** * Fuse the fresh [sightings] into ONE robust absolute yaw-error consensus for [attitude], or * null when fewer than [SelfYawFusion.Params.minAnchors] agree. The caller feeds the result's - * [SelfYawFusion.Result.centerRad] + [SelfYawFusion.Result.spreadRad] into [YawDriftReconciler]. + * [YawConsensus.errorRad] + [YawConsensus.spreadRad] into [YawDriftReconciler.observe]. */ - internal fun fusedYawError( + fun fusedYawError( attitude: Quaternion, sightings: List, - params: SelfYawFusion.Params = SelfYawFusion.Params(), toBody: (Double, Double) -> Vector3D = AoaCalibration::toBodyDirection, - ): SelfYawFusion.Result? { + ): YawConsensus? { + // Default consensus params kept INTERNAL to the observer (the public API stays free of the + // internal SelfYawFusion type); the caller never needs to tune them. + val params = SelfYawFusion.Params() if (sightings.size < params.minAnchors) return null val errs = sightings.map { yawError(attitude, it, toBody) } val wts = sightings.map { it.weight } - return SelfYawFusion.combine(errs, wts, params) + val r = SelfYawFusion.combine(errs, wts, params) ?: return null + return YawConsensus(r.centerRad, r.spreadRad, r.keptIndices.size) } } diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt index eff29d2..8b3318f 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt @@ -19,8 +19,9 @@ import kotlin.math.exp * correction a fraction `alpha = 1 − exp(−dt / tau)` of the way to it, with `tau` scaled UP * (slower) by the consensus spread so a low-agreement instant barely nudges it, and a hard * per-update [maxStepRad] clamp as a belt-and-braces anti-jerk. Pure and deterministic; the - * caller owns cadence and feeds [observe]. `add correctionRad to the predicted yaw → reconciled - * yaw`, or equivalently counter-rotate the rendered world by it. + * caller owns cadence and feeds [observe]. [correctionRad] estimates how far the predicted yaw + * OVER-reads the truth, so the reconciled yaw is `predicted − correctionRad`: rotate the view (or + * the predicted facing) by −[correctionRad] about world up to cancel the drift. */ class YawDriftReconciler( /** Base time constant (s): larger = slower, steadier. ~4 s walks out drift over a few diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt index e344b55..1eedd25 100644 --- a/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt @@ -51,9 +51,9 @@ class SelfHeadingObserverTest { errs.forEach { assertEquals(drift, it, 1e-6, "each anchor reports the common drift; got $it°") } val fused = SelfHeadingObserver.fusedYawError(yaw(drift), sightings, toBody = body)!! - assertEquals(drift * deg, fused.centerRad, 1e-6, "fused consensus recovers the drift") + assertEquals(drift * deg, fused.errorRad, 1e-6, "fused consensus recovers the drift") assertTrue(fused.spreadRad < 1e-6, "noise-free anchors ⇒ ~0 spread") - assertEquals(4, fused.keptIndices.size, "all four clean anchors survive rejection") + assertEquals(4, fused.anchorCount, "all four clean anchors survive rejection") } @Test -- 2.43.0