Aether

import kotlin.math.atan2

import kotlin.math.cos

/** 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<DeviceId, SelfBearing>()

// 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

// with the user holding the aim (lens) axis on the target. See [AoaAutoCalibrator].

// 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<Vector3D>(); val azs = ArrayList<Double>(); val els = ArrayList<Double>()

val errs = ArrayList<Double>(); val wts = ArrayList<Double>()

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

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<Int>,

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<Double>, weights: List<Double> = 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<Double>, weights: List<Double> = 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>): 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

}

}

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}")

}

}