Aether

package com.aether.mofe.engine

import com.aether.mofe.model.Quaternion

import com.aether.mofe.model.Vector3D

import kotlin.math.atan2

/**

* Pure observer of the ABSOLUTE self-yaw error from UWB AoA to KNOWN-position anchors — the

* absolute heading reference a magnetometer-less phone otherwise lacks (its gyro-integrated yaw

* drifts with nothing to pin it, so a static anchor slowly slides across the FPV view). This is

* NOT fed into the EKF (doing so fought the smooth gyro and jittered the aim — the old self-yaw

* fusion, since removed); it drives the slow view-layer [YawDriftReconciler] instead. F2:

* predict = gyro (feel), reconcile = this (truth).

*

* For one anchor: the phone measures it at AoA (az, el), which [AoaCalibration] decodes to a

* BODY-frame direction; rotating that by the CURRENT attitude gives the anchor's direction as the

* attitude BELIEVES it — its measured world heading. The anchor's TRUE world heading is known

* from the solved constellation ([Sighting.anchorWorldDir] = anchor position − self position).

* The yaw error is `wrapPi(measured − true)`: ~0 for a correct attitude, and — the key property —

* the SAME value for every anchor when the attitude's yaw is drifted (the common offset), so

* several anchors over-determine it and [SelfYawFusion] beats down the ~15–30° per-bearing noise.

*

* [toBody] is injected (defaults to the live [AoaCalibration] convention) so the geometry is a

* pure function of its inputs and the circular math is unit-tested without touching global state.

*/

object SelfHeadingObserver {

/** One anchor's AoA sighting: its solved world direction from self, plus the raw (az, el). */

data class Sighting(

/** Anchor position − self position, world frame (only its XY heading is used). */

val anchorWorldDir: Vector3D,

val azimuthRad: Double,

val elevationRad: Double,

/** Boresight-proximity weight in [0,1]; the aimed (near-boresight) anchor is best conditioned. */

val weight: Double = 1.0,

)

/** Per-anchor yaw error e = wrapPi(measuredHeading − trueBearing) for [attitude] + [s]. */

fun yawError(

attitude: Quaternion,

s: Sighting,

toBody: (Double, Double) -> Vector3D = AoaCalibration::toBodyDirection,

): Double {

val world = attitude.rotate(toBody(s.azimuthRad, s.elevationRad))

val measured = atan2(world.y, world.x)

val bearing = atan2(s.anchorWorldDir.y, s.anchorWorldDir.x)

return SelfYawFusion.wrapPi(measured - bearing)

}

/**

* 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].

*/

internal fun fusedYawError(

attitude: Quaternion,

sightings: List<Sighting>,

params: SelfYawFusion.Params = SelfYawFusion.Params(),

toBody: (Double, Double) -> Vector3D = AoaCalibration::toBodyDirection,

): SelfYawFusion.Result? {

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)

}

}

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) error.

*

* 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 a single raw bearing must never yank the heading

* (the aim-float symptom). 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. History: an earlier build fed this consensus straight into the EKF

* attitude and it jittered the aim (~2× volatility) and was removed. The consensus math itself

* is sound; the fix (F2) is to apply it as a SLOW view-layer reconcile ([YawDriftReconciler]),

* never back into the smooth gyro-driven predict state — so this pure helper is resurrected here.

*/

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 a consumer can fold into its update weight.

*/

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

* current correction).

*

* [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).

*/

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.math.exp

/**

* The slow "reconcile" half of the observer view (F2), applied to the VIEW, never the EKF.

*

* The gyro-integrated attitude ("predict") is smooth and lag-free, but its YAW drifts with no

* absolute reference, so a static anchor slowly slides across the FPV as the observer moves.

* [SelfHeadingObserver] / [SelfYawFusion] give a noisy ABSOLUTE yaw error at ~10 Hz. Feeding that

* straight into the EKF fought the gyro and jittered the aim (why the old self-yaw fusion was

* removed). Instead this holds a slowly-decaying correction, applied as a yaw rotation at RENDER

* time: fast head-turns show up instantly (predict, untouched), while the accumulated drift is

* walked out over a time constant far longer than a bearing's noise — the view stops sliding

* WITHOUT the aim ever jittering.

*

* First-order low-pass toward the observed absolute error: each fresh observation moves the

* 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.

*/

class YawDriftReconciler(

/** Base time constant (s): larger = slower, steadier. ~4 s walks out drift over a few

* seconds while staying imperceptible per frame. */

private val tauSeconds: Double = 4.0,

/** Spread (rad) at which tau doubles — a noisy consensus is trusted half as fast. */

private val spreadRefRad: Double = 20.0 * PI / 180.0,

/** Never let one observation move the correction more than this (rad) — hard anti-jerk clamp. */

private val maxStepRad: Double = 3.0 * PI / 180.0,

) {

/** Current view yaw correction (rad): add to the predicted yaw to get the reconciled yaw. */

var correctionRad: Double = 0.0

private set

/** Whether a first absolute fix has been folded in yet. */

var initialized: Boolean = false

private set

/**

* Fold one fresh absolute yaw-ERROR consensus into the correction.

* @param errorRad the observed (predicted − true) yaw error, e.g. [SelfYawFusion.Result.centerRad].

* @param dtSeconds elapsed since the last observation (≤0 ⇒ no time passed ⇒ no move).

* @param spreadRad consensus spread (confidence); larger ⇒ slower trust.

* @return the updated [correctionRad].

*/

fun observe(errorRad: Double, dtSeconds: Double, spreadRad: Double = 0.0): Double {

if (!initialized) {

// Snap on the FIRST consensus (already multi-anchor noise-beaten) so a large standing

// drift doesn't visibly crawl in over several seconds before the reconcile catches up.

correctionRad = SelfYawFusion.wrapPi(errorRad)

initialized = true

return correctionRad

}

if (dtSeconds <= 0.0) return correctionRad

val ratio = spreadRad / spreadRefRad

val tau = tauSeconds * (1.0 + ratio * ratio)

val alpha = 1.0 - exp(-dtSeconds / tau)

val step = (SelfYawFusion.wrapPi(errorRad - correctionRad) * alpha)

.coerceIn(-maxStepRad, maxStepRad)

correctionRad = SelfYawFusion.wrapPi(correctionRad + step)

return correctionRad

}

/** Convenience: fold a [SelfYawFusion.Result] straight in (center = error, spread = confidence). */

internal fun observe(result: SelfYawFusion.Result, dtSeconds: Double): Double =

observe(result.centerRad, dtSeconds, result.spreadRad)

/** Forget the correction — frame re-established / mesh switch / lost lock. */

fun reset() {

correctionRad = 0.0

initialized = false

}

}

package com.aether.mofe.engine

import com.aether.mofe.model.Quaternion

import com.aether.mofe.model.Vector3D

import com.aether.mofe.model.WORLD_UP

import kotlin.math.PI

import kotlin.math.abs

import kotlin.test.Test

import kotlin.test.assertEquals

import kotlin.test.assertFalse

import kotlin.test.assertNull

import kotlin.test.assertTrue

/**

* The absolute self-yaw OBSERVER (F2): recover the phone's heading error from UWB AoA to

* KNOWN-position anchors, robustly fused across anchors. Geometry is exercised with an EXPLICIT

* AoA decoder (default Z_NEG convention) so the test never depends on — or mutates — the live

* [AoaCalibration] global state.

*/

class SelfHeadingObserverTest {

private val deg = PI / 180.0

/** Explicit Z_NEG decoder (shipped default): az/el → body direction, no global read. */

private val body: (Double, Double) -> Vector3D =

{ az, el -> AoaCalibration.decode(AoaBoresight.Z_NEG, 0.0, 1.0, 1.0, az, el) }

/** A pure world-yaw rotation of [deg]° about world up (+Z). */

private fun yaw(degrees: Double) = Quaternion.fromAxisAngle(WORLD_UP, degrees * deg)

@Test

fun aCorrectAttitudeGivesZeroYawErrorForAnyAnchor() {

// With the true attitude, the anchor's measured direction IS its known direction ⇒ error 0.

for ((az, el) in listOf(0.0 to 10.0, 25.0 to -8.0, -40.0 to 15.0)) {

val b = body(az * deg, el * deg)

val s = SelfHeadingObserver.Sighting(anchorWorldDir = b, azimuthRad = az * deg, elevationRad = el * deg)

val e = SelfHeadingObserver.yawError(Quaternion.IDENTITY, s, body)

assertTrue(abs(e) < 1e-9, "true attitude ⇒ ~0 yaw error, got ${e / deg}° for az=$az el=$el")

}

}

@Test

fun aYawDriftedAttitudeGivesTheSameErrorForEveryAnchor() {

// The anchors' TRUE directions are what the un-drifted phone sees; the observer runs on a

// yaw-DRIFTED attitude, so every anchor must report the identical common yaw error = drift.

val drift = 25.0

val sightings = listOf(5.0 to 6.0, -30.0 to 12.0, 45.0 to -10.0, 15.0 to 20.0).map { (az, el) ->

val b = body(az * deg, el * deg) // true direction (un-drifted attitude = identity)

SelfHeadingObserver.Sighting(anchorWorldDir = b, azimuthRad = az * deg, elevationRad = el * deg)

}

val errs = sightings.map { SelfHeadingObserver.yawError(yaw(drift), it, body) / deg }

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

assertTrue(fused.spreadRad < 1e-6, "noise-free anchors ⇒ ~0 spread")

assertEquals(4, fused.keptIndices.size, "all four clean anchors survive rejection")

}

@Test

fun aGrossOutlierBearingIsRejectedFromTheConsensus() {

// Three anchors agree on a 20° drift; one is a 90°-off NLOS spike. Robust combine drops it.

val drift = 20.0 * deg

val errors = listOf(drift + 2.0 * deg, drift - 3.0 * deg, drift + 1.0 * deg, drift + 90.0 * deg)

val r = SelfYawFusion.combine(errors)!!

assertFalse(3 in r.keptIndices, "the 90°-off outlier (index 3) is rejected")

assertEquals(3, r.keptIndices.size, "the three agreeing anchors survive")

assertTrue(abs(SelfYawFusion.wrapPi(r.centerRad - drift)) < 3.0 * deg,

"consensus sits within a few ° of the true drift, got ${r.centerRad / deg}°")

}

@Test

fun tooFewAnchorsYieldsNoConsensus() {

val s = SelfHeadingObserver.Sighting(Vector3D(1.0, 0.0, 0.0), 0.0, 0.0)

assertNull(SelfHeadingObserver.fusedYawError(Quaternion.IDENTITY, listOf(s), toBody = body),

"a lone bearing is not a consensus (minAnchors = 2)")

}

}

package com.aether.mofe.engine

import kotlin.math.PI

import kotlin.math.abs

import kotlin.math.sin

import kotlin.test.Test

import kotlin.test.assertTrue

/**

* The slow view-layer "reconcile" of the observer heading (F2). Proves the properties that make it

* safe to apply where the old EKF self-yaw fusion was not: it walks out a standing drift, but no

* single noisy observation can jerk the view, low-confidence consensus is trusted less, and the

* whole [SelfYawFusion.combine] → reconcile pipeline recovers a real drift from noisy bearings.

*/

class YawDriftReconcilerTest {

private val deg = PI / 180.0

private val maxStep = 3.0 * deg // matches YawDriftReconciler default maxStepRad

@Test

fun snapsToTheFirstFixThenIsSlow() {

val r = YawDriftReconciler()

assertTrue(!r.initialized)

r.observe(0.5, dtSeconds = 0.1)

assertTrue(r.initialized, "first observation initializes")

assertTrue(abs(r.correctionRad - 0.5) < 1e-12, "snaps to the first consensus, got ${r.correctionRad}")

// A subsequent observation moves only a small, sub-clamp fraction — no longer a snap.

val before = r.correctionRad

r.observe(0.5 + 0.4, dtSeconds = 0.1)

assertTrue(r.correctionRad - before in 1e-6..maxStep, "post-init step is small & bounded")

}

@Test

fun convergesToAConstantBias() {

val r = YawDriftReconciler()

r.observe(0.0, 0.1) // init at 0

val bias = 0.4 // ~23°

var prev = r.correctionRad

repeat(600) {

val c = r.observe(bias, 0.1)

val step = c - prev

assertTrue(step >= -1e-12, "monotone toward the bias")

assertTrue(step <= maxStep + 1e-12, "no step exceeds the hard clamp")

prev = c

}

assertTrue(abs(r.correctionRad - bias) < 1e-3, "converges to the constant bias, got ${r.correctionRad}")

}

@Test

fun aSingleLargeObservationCannotJerkTheView() {

val r = YawDriftReconciler()

r.observe(0.0, 0.1) // init at 0

r.observe(3.0, 0.1) // a ~172° absolute error in one tick

assertTrue(abs(r.correctionRad - maxStep) < 1e-9,

"one big observation moves EXACTLY the hard clamp (${maxStep / deg}°), got ${r.correctionRad / deg}°")

r.observe(3.0, 0.1)

assertTrue(abs(r.correctionRad - 2.0 * maxStep) < 1e-9, "still clamped per-update while the error is large")

}

@Test

fun aModerateErrorMovesGentlyNotInstantly() {

val r = YawDriftReconciler()

r.observe(0.0, 0.1)

r.observe(0.3, 0.1) // ~17°: under the clamp, so a gentle first-order step

assertTrue(r.correctionRad in 1e-4..(0.3 * 0.05),

"a moderate error nudges, never snaps — got ${r.correctionRad / deg}° for a 17° error")

}

@Test

fun aLowConfidenceConsensusIsTrustedLess() {

val tight = YawDriftReconciler(); tight.observe(0.0, 0.1)

val noisy = YawDriftReconciler(); noisy.observe(0.0, 0.1)

tight.observe(0.3, 0.1, spreadRad = 0.0)

noisy.observe(0.3, 0.1, spreadRad = 30.0 * deg)

assertTrue(tight.correctionRad > noisy.correctionRad,

"a high-spread (low-confidence) update moves the correction less")

}

@Test

fun takesTheShortWayAroundPlusMinusPi() {

val r = YawDriftReconciler()

r.observe(-3.1, 0.1) // snap near −π

val before = r.correctionRad

r.observe(3.0, 0.1) // target on the +π side: short path is MORE negative (wrap)

val step = r.correctionRad - before

assertTrue(step < 0.0 && abs(step) < maxStep, "moved the short way (wrapped), not +0.15 the long way")

assertTrue(r.correctionRad > -PI, "stays a valid wrapped angle, got ${r.correctionRad}")

}

@Test

fun forgetsOnReset() {

val r = YawDriftReconciler()

r.observe(0.5, 0.1)

r.reset()

assertTrue(!r.initialized && r.correctionRad == 0.0, "reset clears the lock")

}

@Test

fun recoversARealDriftThroughTheCombineReconcilePipeline() {

// Six anchors, each reporting the common 25° drift with ±10° per-bearing AoA noise. The

// robust fuse beats the noise down per tick; the reconcile smooths across ticks. End state

// must sit within a few ° of the true drift — the "view stops sliding" property.

val drift = 25.0 * deg

val r = YawDriftReconciler()

val nAnchors = 6

var maxLateErr = 0.0

for (t in 0 until 90) {

val errs = (0 until nAnchors).map { i ->

drift + 10.0 * deg * sin(2.3 * i + 0.9 * t) // deterministic bounded per-bearing noise

}

val fused = SelfYawFusion.combine(errs)!!

r.observe(fused, dtSeconds = 0.1)

if (t >= 70) maxLateErr = maxOf(maxLateErr, abs(SelfYawFusion.wrapPi(r.correctionRad - drift)))

}

assertTrue(abs(SelfYawFusion.wrapPi(r.correctionRad - drift)) < 4.0 * deg,

"reconciled correction lands within 4° of the true drift, got ${r.correctionRad / deg}°")

assertTrue(maxLateErr < 6.0 * deg, "and stays bounded near it (no runaway): late max ${maxLateErr / deg}°")

}

}