From 21629b899b501edee4539d1cca27eac06f2bdf4a Mon Sep 17 00:00:00 2001 From: Claude Date: Sun, 2 Aug 2026 16:12:43 +0000 Subject: [PATCH] engine: remove premature self-yaw AoA fusion (SelfYawFusion) The fused multi-anchor self-yaw reconcile decodes every bearing through the AoA convention, which still defaults to the wrong boresight (X_POS). A boresight error is common-mode across anchors, so the fusion's spread/confidence stays small while its centre is ~90 deg wrong: it anchors heading to a confident-but- wrong direction and masks the boresight bug that is the actual heading fix. It also presumes per-bearing noise is the binding constraint, whereas the field diagnosis puts the dominant error on the boresight flip + frame distortion. Revert the self-yaw path to the simple per-bearing update (leaves the raw boresight behaviour visible for the next capture) and drop SelfYawFusion + its test. Robust multi-anchor fusion is re-queued in the roadmap to revisit AFTER the -Z boresight fix is validated, only if a residual jitter remains. The antenna-delay calibration and missing-edge fallback are untouched. Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../mofe/engine/MultiObserverFusionEngine.kt | 123 ++---------------- .../com/aether/mofe/engine/SelfYawFusion.kt | 109 ---------------- .../aether/mofe/engine/SelfYawFusionTest.kt | 100 -------------- 3 files changed, 10 insertions(+), 322 deletions(-) delete mode 100644 common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt delete mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/SelfYawFusionTest.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 8410946..0d05fbb 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt @@ -41,8 +41,6 @@ import kotlinx.coroutines.flow.Flow import kotlinx.coroutines.flow.MutableSharedFlow import kotlinx.coroutines.flow.asSharedFlow import kotlin.Long -import kotlin.math.atan2 -import kotlin.math.cos /** * Receives every meaningful state change emitted by the engine. @@ -251,8 +249,8 @@ class MultiObserverFusionEngine( * cadence to correct the AoA-placed constellation (see refineAnchorConstellation). */ private val interAnchorDistances = mutableMapOf, Double>() /** Maintenance-tick counter that paces constellation refinement. */ - private var maintenanceTicks = 0 + /** Per-device antenna-delay offsets (metres) solved by [calibrateAntennaDelays] and SUBTRACTED * from every measured inter-anchor range before the constellation solve. A per-device range * bias (an uncalibrated UWB antenna — field-observed at +0.39 m on one anchor) is nearly @@ -371,37 +369,6 @@ class MultiObserverFusionEngine( * is never trusted for placement; the ranking (raised reads far higher) is robust. */ private val selfElevations = mutableMapOf() - /** Latest physically-valid self-AoA bearing per anchor (az, el, time), fed by - * [maybeUpdateSelfYaw]. The absolute-yaw update is applied FUSED across these on a - * throttle ([applyFusedSelfYaw]), not per raw sample — phone AoA is ~15–30° noisy per - * bearing, so a lone bearing must not yank the heading (the aim-float symptom). */ - private data class SelfBearing(val az: Double, val el: Double, val ts: Timestamp) - private val selfBearingSamples = mutableMapOf() - // 0 (not Long.MIN_VALUE) so the first (now - last) can't overflow Long and wrongly gate. - private var lastFusedYawApplyMicros = 0L - /** Apply the fused self-yaw update at most this often (µs) — decouples the correction - * cadence from the raw AoA rate so a fast stream can't over-weight one geometry. */ - private val fuseIntervalMicros = 100_000L // ~10 Hz - /** A per-anchor bearing older than this (µs) is stale and excluded from the fuse. */ - private val freshWindowMicros = 1_500_000L // 1.5 s - /** Minimum AoA boresight-proximity weight (cos az·cos el ≈ within 60° of boresight) to - * anchor yaw off a SINGLE bearing when no multi-anchor consensus survives — i.e. only when - * the user is actually aiming an anchor near the boresight; else hold on the gyro. */ - private val aimWeightMin = 0.5 - /** Variance multiplier for the SINGLE-anchor fallback update (no consensus survived). A lone - * bearing is ~15–30° noisy, and this path fires often during motion (off-boresight anchors - * rejected) — applying it at full strength jittered the heading (~2× the volatility, the - * "aim darts around" regression). Heavier smoothing so a lone bearing NUDGES toward the - * aimed target across several ticks instead of snapping to each noisy sample. */ - private val singleAnchorNoiseScale = 6.0 - private var _lastFusedYawSpreadDeg = Double.NaN - private var _lastFusedYawAnchors = 0 - /** Cross-anchor AoA-heading spread (deg) of the last fused self-yaw update; NaN until one - * runs with ≥2 anchors. Small = anchors agree (well-conditioned); large = noisy geometry. */ - fun selfYawFusedSpreadDeg(): Double = _lastFusedYawSpreadDeg - /** How many anchors drove the last fused self-yaw update (after outlier rejection). */ - fun selfYawFusedAnchorCount(): Int = _lastFusedYawAnchors - /** Snapshot of this device's retained AoA azimuths to its peers (id → radians). */ fun selfAoaBearings(): Map = selfBearings.toMap() @@ -882,87 +849,17 @@ class MultiObserverFusionEngine( } // Feed the auto-calibrator while a capture is running: physically-valid self AoA, - // with the user holding the aim (lens) axis on the target. See [AoaAutoCalibrator]. + // with the user holding the wand axis on the target. See [AoaAutoCalibrator]. if (AoaAutoCalibrator.collecting) AoaAutoCalibrator.addSample(az, el) val selfCtx = targets[self] ?: return - // Record this anchor's latest bearing; DON'T anchor yaw off one noisy sample. The - // update is applied fused across all fresh anchors on a throttle below. - selfBearingSamples[m.targetId] = SelfBearing(az, el, m.timestamp) - val nowMicros = m.timestamp.microseconds - if (nowMicros - lastFusedYawApplyMicros < fuseIntervalMicros) return - applyFusedSelfYaw(selfCtx, m.timestamp) - } - - /** - * Fused absolute-yaw update: combine EVERY anchor with a fresh self-AoA bearing into ONE - * robust heading correction, instead of letting each ~15–30°-noisy bearing yank the EKF. - * - * For each fresh anchor the per-anchor yaw error e_i = (measured heading − true bearing) - * is computed from the CURRENT attitude + geometry; for the correct heading every e_i is - * the same (it is the attitude's yaw error, common to all anchors), so [SelfYawFusion] - * rejects gross outliers (NLOS/motion spikes), takes the circular-mean consensus, and - * scales the measurement noise up with the survivors' spread (low agreement → gentle - * update). The surviving anchors' bearings are then applied with that shared noise scale — - * sequential EKF updates with inflated variance ≈ one smoothed, averaged correction, so - * √N of the phone-AoA noise is beaten down and a single bad bearing can't swing the aim. - * A lone fresh anchor falls back to the original single-bearing update (no regression on a - * sparsely-connected mesh). Engine-thread confined (called from [maybeUpdateSelfYaw]). - */ - private fun applyFusedSelfYaw(selfCtx: TargetContext, now: Timestamp) { - val st = selfCtx.ekf.getState() ?: return - val selfPos = st.position - val q = st.orientation - val refs = frameManager.getAllReferencePoints() - val dirs = ArrayList(); val azs = ArrayList(); val els = ArrayList() - val errs = ArrayList(); val wts = ArrayList() - for ((id, sb) in selfBearingSamples) { - if (now.microseconds - sb.ts.microseconds > freshWindowMicros) continue - val refPos = refs.firstOrNull { it.id == id }?.position ?: continue - val dir = refPos - selfPos - if (dir.magnitude < 1e-6) continue - val mBody = AoaCalibration.toBodyDirection(sb.az, sb.el) - if (mBody.magnitude < 1e-9) continue - val w = q.rotate(mBody) - val measHeading = atan2(w.y, w.x) // measured world bearing - val beta = atan2(dir.y, dir.x) // true world bearing to the anchor - dirs.add(dir); azs.add(sb.az); els.add(sb.el) - errs.add(SelfYawFusion.wrapPi(measHeading - beta)) - // Conditioning weight = AoA boresight-proximity (cos az·cos el): 1 at the boresight - // (the anchor being aimed at — best-conditioned bearing) → ~0 near the ±90° FoV rail. - // Off-boresight bearings are ~30° unreliable regardless of the az/el signs, so this - // lets the AIMED target dominate instead of being outvoted into a wrong-lock. - wts.add((cos(sb.az) * cos(sb.el)).coerceAtLeast(0.05)) - } - if (errs.isEmpty()) return - lastFusedYawApplyMicros = now.microseconds - // A single fresh anchor, OR no ≥2 consensus survives the (boresight-weighted) rejection: - // anchor to the single best-conditioned bearing, but only when it is near the boresight - // (the user is aiming at that anchor); otherwise hold on the gyro rather than trust a - // far-off-boresight bearing. - val fused = if (errs.size >= 2) SelfYawFusion.combine(errs, wts) else null - if (fused == null) { - val best = wts.indices.maxByOrNull { wts[it] } ?: return - if (wts[best] < aimWeightMin) return - selfCtx.ekf.updateBearing(dirs[best], azs[best], els[best], singleAnchorNoiseScale) - _selfBearingApplied++ - _lastSelfBearingAzDeg = azs[0] * 180.0 / kotlin.math.PI - _lastSelfBearingAzDeg = azs[best] * 180.0 / kotlin.math.PI - _lastFusedYawAnchors = 1 - _selfBearingApplied++ - _lastSelfBearingAzDeg = azs[0] * 180.0 / kotlin.math.PI - return - } - // Apply each surviving anchor down-weighted by its conditioning: the boresight anchor - // (w≈1) gets the fused noise scale; off-boresight survivors get 1/w² more variance, so a - // mis-decoded off-boresight bearing can nudge but not swing the aim. - for (i in fused.keptIndices) { - val wi = wts[i] - selfCtx.ekf.updateBearing(dirs[i], azs[i], els[i], fused.noiseScale / (wi * wi)) - } - _selfBearingApplied += fused.keptIndices.size - _lastSelfBearingAzDeg = azs[fused.keptIndices.first()] * 180.0 / kotlin.math.PI - _lastFusedYawSpreadDeg = fused.spreadRad * 180.0 / kotlin.math.PI - _lastFusedYawAnchors = fused.keptIndices.size + val selfPos = selfCtx.ekf.getState()?.position ?: return + val refPos = frameManager.getAllReferencePoints() + .firstOrNull { it.id == m.targetId }?.position ?: return + val dir = refPos - selfPos + if (dir.magnitude < 1e-6) return + selfCtx.ekf.updateBearing(dir, az, el) + _selfBearingApplied++ // yaw actually anchored to the mesh frame + _lastSelfBearingAzDeg = az * 180.0 / kotlin.math.PI } // Self-yaw (AoA) anchoring instrumentation — exposes whether the absolute-yaw diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt deleted file mode 100644 index 5d04b19..0000000 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt +++ /dev/null @@ -1,109 +0,0 @@ -package com.aether.mofe.engine - -import kotlin.math.PI -import kotlin.math.atan2 -import kotlin.math.cos -import kotlin.math.ln -import kotlin.math.sin -import kotlin.math.sqrt - -/** - * Robust multi-anchor consensus for the self-yaw (AoA heading) update. - * - * WHY. A magnetometer-less phone anchors its heading ONLY with self-measured UWB AoA, and - * field captures show that AoA is ~15–30° noisy PER BEARING and, at close (≈1 m) range, - * hypersensitive to the position solve — so applying each raw bearing straight into the EKF - * (the previous behaviour) drags the heading tens of degrees and makes the aim float. When - * the device sees several anchors at once, their bearings OVER-DETERMINE one heading: the - * per-anchor "yaw error" e_i (measured heading − true bearing) should be the SAME value for - * every anchor (it equals the attitude's yaw error, common to all), so their disagreement is - * pure per-bearing noise/outliers. This fuses them: - * • a robust CENTRE (circular mean after rejecting anchors that disagree with the - * consensus by more than [Params.outlierRad] — a NLOS / motion-corrupted spike), - * • a SPREAD (circular std of the survivors) → a live confidence signal, and - * • a NOISE SCALE that grows with the spread, so a low-agreement instant damps the update - * rather than yanking the heading. - * - * Pure and side-effect-free so the (easy-to-get-wrong) circular statistics are unit-tested - * with no engine/EKF/hardware. The caller (MultiObserverFusionEngine) computes e_i from the - * live attitude + geometry, calls [combine], then applies the surviving anchors' bearings - * with the returned [Result.noiseScale]. - */ -internal object SelfYawFusion { - - /** Tuning — conservative defaults, revisit against field captures (see mofe_aim_localizer). */ - data class Params( - /** Reject an anchor whose yaw error is more than this from the consensus (rad). - * ~45°: drops gross NLOS/motion spikes, keeps the natural ~15–30° AoA spread. */ - val outlierRad: Double = 45.0 * PI / 180.0, - /** Need at least this many agreeing anchors to trust a fused correction. */ - val minAnchors: Int = 2, - /** Baseline variance multiplier applied to EVERY fused bearing — heavier smoothing - * than a raw single-bearing update, since phone AoA is noisy even at consensus. */ - val noiseBase: Double = 2.0, - /** Spread (rad) at which the damping doubles; larger spread → weaker update. */ - val spreadRefRad: Double = 20.0 * PI / 180.0, - ) - - /** - * @param keptIndices indices (into the input list) of the anchors that survived rejection. - * @param centerRad robust consensus yaw error (rad) — the heading correction implied. - * @param spreadRad circular std of the survivors (rad) — the confidence/quality signal. - * @param noiseScale variance multiplier to pass to [ImuFusionEKF.updateBearing] per anchor. - */ - data class Result( - val keptIndices: List, - val centerRad: Double, - val spreadRad: Double, - val noiseScale: Double, - ) - - /** - * Combine per-anchor yaw errors (rad) into one robust consensus. Returns null when there - * is no usable ≥[Params.minAnchors] consensus (the caller then holds on the gyro or falls - * back to its single best-conditioned bearing). - * - * [weights] (optional, same length as [errorsRad]) express how much to trust each anchor — - * the caller passes AoA boresight-proximity, so the anchor the user is AIMING at (at the - * boresight, its bearing well-conditioned) dominates the consensus while the off-boresight - * anchors — which the field data shows are ~30° unreliable regardless of the az/el signs — - * can't outvote it into a wrong-lock. Empty/mismatched → uniform (unweighted, original - * behaviour). - */ - fun combine(errorsRad: List, weights: List = emptyList(), p: Params = Params()): Result? { - if (errorsRad.size < p.minAnchors) return null - val w = if (weights.size == errorsRad.size) weights else List(errorsRad.size) { 1.0 } - val c0 = circMean(errorsRad, w) - val kept = errorsRad.indices.filter { kotlin.math.abs(wrapPi(errorsRad[it] - c0)) <= p.outlierRad } - if (kept.size < p.minAnchors) return null - val vals = kept.map { errorsRad[it] } - val center = circMean(vals, kept.map { w[it] }) - val spread = circStd(vals) - val ratio = spread / p.spreadRefRad - val noiseScale = p.noiseBase * (1.0 + ratio * ratio) - return Result(kept, center, spread, noiseScale) - } - - /** Weighted circular mean (uniform weights ⇒ ordinary circular mean). */ - fun circMean(xs: List, weights: List = emptyList()): Double { - val w = if (weights.size == xs.size) weights else List(xs.size) { 1.0 } - var s = 0.0; var c = 0.0 - for (i in xs.indices) { s += w[i] * sin(xs[i]); c += w[i] * cos(xs[i]) } - return atan2(s, c) - } - - fun circStd(xs: List): Double { - if (xs.size < 2) return 0.0 - val s = xs.sumOf { sin(it) } / xs.size - val c = xs.sumOf { cos(it) } / xs.size - val r = sqrt(s * s + c * c).coerceIn(1e-9, 1.0) - return sqrt(-2.0 * ln(r)) - } - - fun wrapPi(a: Double): Double { - var d = a - while (d > PI) d -= 2.0 * PI - while (d < -PI) d += 2.0 * PI - return d - } -} \ No newline at end of file diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfYawFusionTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfYawFusionTest.kt deleted file mode 100644 index 7c38d48..0000000 --- a/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfYawFusionTest.kt +++ /dev/null @@ -1,100 +0,0 @@ -package com.aether.mofe.engine - -import kotlin.math.PI -import kotlin.test.Test -import kotlin.test.assertEquals -import kotlin.test.assertNotNull -import kotlin.test.assertNull -import kotlin.test.assertTrue - -/** - * Coverage for the robust multi-anchor self-yaw consensus (the core of the fused AoA - * heading update). Pure circular statistics — no EKF/engine/hardware. - */ -class SelfYawFusionTest { - - private val deg = PI / 180.0 - - @Test - fun `agreeing anchors are all kept with a small spread and baseline damping`() { - // Four anchors that agree on ~10° yaw error, within normal AoA scatter. - val errs = listOf(8.0, 11.0, 9.0, 12.0).map { it * deg } - val r = SelfYawFusion.combine(errs) - assertNotNull(r) - assertEquals(4, r.keptIndices.size, "all agreeing anchors survive") - assertTrue(r.centerRad / deg in 8.0..12.0, "consensus near the cluster: ${r.centerRad / deg}") - assertTrue(r.spreadRad / deg < 10.0, "tight spread: ${r.spreadRad / deg}") - // Low spread ⇒ damping close to the baseline (2.0), not inflated. - assertTrue(r.noiseScale in 2.0..3.0, "noiseScale=${r.noiseScale}") - } - - @Test - fun `a gross outlier anchor is rejected from the consensus`() { - // Three agree near 10°, one is a 120° NLOS/motion spike. - val errs = listOf(10.0, 12.0, 8.0, 120.0).map { it * deg } - val r = SelfYawFusion.combine(errs) - assertNotNull(r) - assertEquals(3, r.keptIndices.size, "the 120° outlier is dropped") - assertTrue(3 !in r.keptIndices, "index of the outlier excluded") - assertTrue(r.centerRad / deg in 8.0..12.0, "consensus unbiased by the outlier: ${r.centerRad / deg}") - } - - @Test - fun `higher disagreement inflates the noise scale so the update is gentler`() { - val tight = SelfYawFusion.combine(listOf(0.0, 3.0, -3.0, 2.0).map { it * deg })!! - val loose = SelfYawFusion.combine(listOf(-25.0, 20.0, -18.0, 22.0).map { it * deg })!! - assertTrue(loose.noiseScale > tight.noiseScale, - "wider spread must damp more: tight=${tight.noiseScale} loose=${loose.noiseScale}") - } - - @Test - fun `fewer than two anchors yields no consensus`() { - assertNull(SelfYawFusion.combine(listOf(10.0 * deg))) - assertNull(SelfYawFusion.combine(emptyList())) - } - - @Test - fun `no agreeing pair after rejection yields null rather than chasing noise`() { - // Three bearings all far apart (>45° from any consensus) — no trustworthy heading. - val errs = listOf(-90.0, 30.0, 150.0).map { it * deg } - assertNull(SelfYawFusion.combine(errs)) - } - - @Test - fun `consensus wraps correctly across the ±180 seam`() { - // Errors clustered near ±180° must average to ~180°, not ~0°. - val errs = listOf(178.0, -179.0, 177.0).map { it * deg } - val r = SelfYawFusion.combine(errs) - assertNotNull(r) - assertTrue(kotlin.math.abs(SelfYawFusion.wrapPi(r.centerRad - PI)) < 5 * deg, - "consensus near 180°, got ${r.centerRad / deg}") - } - - @Test - fun `weighting pulls the consensus to the boresight anchor, away from an off-boresight majority`() { - // One boresight anchor at 5°, two off-boresight at ±120° that WOULD win an equal vote. - val errs = listOf(5.0, 120.0, -120.0).map { it * deg } - val uniform = SelfYawFusion.circMean(errs) / deg - val weighted = SelfYawFusion.circMean(errs, listOf(1.0, 0.3, 0.3)) / deg - assertTrue(kotlin.math.abs(weighted - 5.0) < 15.0, "weighted mean near the boresight anchor: $weighted") - assertTrue(kotlin.math.abs(uniform - 5.0) > 45.0, "unweighted mean is dragged off by the majority: $uniform") - } - - @Test - fun `combine with boresight weight rejects an off-boresight majority (caller falls back to single)`() { - // Boresight anchor at 6°, two off-boresight near 130°. Weighted centre sits by the - // boresight, so the 130° pair is >45° out → rejected → <2 kept → null (the engine then - // anchors to the single best-conditioned bearing). - val r = SelfYawFusion.combine(listOf(6.0, 128.0, 132.0).map { it * deg }, listOf(1.0, 0.3, 0.3)) - assertNull(r) - } - - @Test - fun `combine keeps two agreeing well-weighted anchors and drops the odd one`() { - val r = SelfYawFusion.combine(listOf(8.0, 11.0, 130.0).map { it * deg }, listOf(1.0, 0.9, 0.3)) - assertNotNull(r) - assertEquals(2, r.keptIndices.size) - assertTrue(2 !in r.keptIndices, "the 130° off-boresight anchor is dropped") - assertTrue(kotlin.math.abs(r.centerRad / deg - 9.5) < 5.0, "consensus by the two aligned anchors: ${r.centerRad / deg}") - } -} \ No newline at end of file -- 2.43.0