0008-feat-apply-the-self-yaw-reconcile-to-the-FPV-view-F2.patch - Aether - Redmine

     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
-...
     import kotlinx.coroutines.sync.Mutex
     import kotlinx.coroutines.sync.withLock
     import kotlinx.coroutines.withContext
     import kotlin.math.cos
     import kotlin.time.Duration.Companion.milliseconds
     /**
-...
                 // ~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()
-...
                         // 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)
+                    }
+                }
-...
         @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

             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)
+        }
         /**
-...
             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)
+        }
         /**

             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,
-...
         /**
          * 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<Sighting>,
             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)
+        }
+    }

      * 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

             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

Project

General

Profile

Aether

User Story #62 » 0008-feat-apply-the-self-yaw-reconcile-to-the-FPV-view-F2.patch