From aac3c48b91e72620272acffd8a1f4b3adeaa4166 Mon Sep 17 00:00:00 2001 From: Claude Date: Mon, 3 Aug 2026 14:07:22 +0000 Subject: [PATCH 07/10] feat(engine): absolute self-yaw observer + slow view-layer reconcile (F2 core) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit F2 (observer-camera drift correction), engine half. The FPV view slides under motion because a magnetometer-less phone has NO absolute yaw reference — its gyro-integrated heading drifts freely. An earlier build fed an AoA-derived absolute yaw straight into the EKF attitude; it fought the smooth gyro and jittered the aim, and was removed. This restores the absolute reference the RIGHT way: as a slow reconcile applied to the VIEW, never the predict state. - SelfYawFusion (resurrected pure math): robust multi-anchor consensus of the per-anchor yaw error, with outlier rejection + a spread/confidence signal. - SelfHeadingObserver: per-anchor yaw error from attitude + known anchor geometry + AoA; a yaw-drifted attitude makes every anchor report the same error, so anchors over-determine it and the fuse beats down AoA noise. - YawDriftReconciler: first-order low-pass toward the fused absolute error, tau scaled by consensus spread + a hard per-update clamp, so a standing drift is walked out over seconds while no single bearing can ever jerk the view. predict = gyro (feel), reconcile = this (truth). 12 tests: zero error at truth, the common-drift property, outlier rejection, convergence-to-bias, anti-jerk clamp, spread damping, +-pi wrap, and an end-to-end combine->reconcile recovery of a 25 deg drift from noisy bearings. Wiring live AoA + applying the correction to the FPV camera is the reviewed UI follow-up. Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../aether/mofe/engine/SelfHeadingObserver.kt | 66 ++++++++++ .../com/aether/mofe/engine/SelfYawFusion.kt | 108 ++++++++++++++++ .../aether/mofe/engine/YawDriftReconciler.kt | 76 +++++++++++ .../mofe/engine/SelfHeadingObserverTest.kt | 77 ++++++++++++ .../mofe/engine/YawDriftReconcilerTest.kt | 119 ++++++++++++++++++ 5 files changed, 446 insertions(+) create mode 100644 common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt create mode 100644 common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt create mode 100644 common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/YawDriftReconcilerTest.kt diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt new file mode 100644 index 0000000..d2a5c73 --- /dev/null +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfHeadingObserver.kt @@ -0,0 +1,66 @@ +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, + 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) + } +} diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt new file mode 100644 index 0000000..0547c9b --- /dev/null +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/SelfYawFusion.kt @@ -0,0 +1,108 @@ +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, + 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, 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 + } +} diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt new file mode 100644 index 0000000..eff29d2 --- /dev/null +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/YawDriftReconciler.kt @@ -0,0 +1,76 @@ +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 + } +} diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt new file mode 100644 index 0000000..e344b55 --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/SelfHeadingObserverTest.kt @@ -0,0 +1,77 @@ +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)") + } +} diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/YawDriftReconcilerTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/YawDriftReconcilerTest.kt new file mode 100644 index 0000000..ac5d102 --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/YawDriftReconcilerTest.kt @@ -0,0 +1,119 @@ +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}°") + } +} -- 2.43.0