From 5d06960bad9b8f016abd8c493d32aa09b3ebed1c Mon Sep 17 00:00:00 2001 From: Claude Date: Mon, 3 Aug 2026 03:01:42 +0000 Subject: [PATCH 02/10] fix(engine): robust frame orientation for near-horizontal anchor slabs (kills the intermittent twist) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Root cause (#44): a room-scale layout with anchors at similar heights (the field survey: 4 anchors within 0.56 m of height, best-fit plane only 5.8 deg off level) has NO physically-raised anchor. upFromBasePlane picks "up" from the highest AoA elevation, but the true peer-to-peer elevation separations (0.5-4.6 deg) are far below the gate (11 deg) AND the +/-15 deg elevation noise floor. So on most ticks the gate fails (orientation null -> perpetual CONVERGING, never latches), and on the noise-crowned minority the crowned peer sets the leveling up-SIGN: crowning the wrong peer flips "up" to point down -> ~180 deg roll -> the twisted/mirrored frame. It is intermittent because each cold start redraws the elevation noise. (The azimuth mirror is NOT the culprit — its chirality residuals are cleanly separated.) Fix — two-tier, confidence-gated leveling in the pure ConstellationOrientation: • TIER 1 (table): the raised-anchor pick (unchanged math) now must PERSIST for constellationRaisedPersistTicks consecutive ticks, so one noise tick can't crown a flat anchor. • TIER 2 (slab): level off the best-fit plane NORMAL (deterministic covariance PCA via a fixed-sweep Jacobi — id-sorted accumulation so the normal is bit-reproducible), and resolve the one bit distances+azimuth can't (up vs down) with UpSignAccumulator — a sequential significance test on an elevation-sign matched filter. It converges to the CORRECT sign on a real slab and provably NEVER reaches confidence on a truly-flat set (refuses to coin-flip). • Commit an orientation only when leveling + up-sign + a decisive azimuth mirror (|resR-resN| >= constellationMirrorMarginRad) are all confident; otherwise the frame stays honestly CONVERGING instead of latching a twist. Design produced + adversarially verified by an 8-agent workflow (it corrected the initial hypothesis: the twist is the up-SIGN, not the azimuth mirror). Residual: plane-leveling carries an inherent ~6 deg tilt for a non-coplanar slab (swaps two near-equal-height anchors, not a twist); refining it with the root IMU gravity is a deferred follow-on (hook stubbed at Tier-2). jvmTests (real survey as fixture): 60-seed no-inversion regression, flat-slab refuses to orient, plane-normal accuracy + determinism, accumulator significance, mirror margin. Full :common:jvmTest green (no regression to the raised-anchor case). Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../mofe/engine/ConstellationOrientation.kt | 170 ++++++++++++++++-- .../mofe/engine/MultiObserverFusionEngine.kt | 53 +++++- .../aether/mofe/engine/UpSignAccumulator.kt | 56 ++++++ .../com/aether/mofe/model/ConfigTypes.kt | 15 ++ .../ConstellationSlabOrientationTest.kt | 163 +++++++++++++++++ 5 files changed, 442 insertions(+), 15 deletions(-) create mode 100644 common/src/commonMain/kotlin/com/aether/mofe/engine/UpSignAccumulator.kt create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationSlabOrientationTest.kt diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/ConstellationOrientation.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/ConstellationOrientation.kt index 1f0008d..7d2818a 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/ConstellationOrientation.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/ConstellationOrientation.kt @@ -52,6 +52,10 @@ object ConstellationOrientation { val headingResidualRad: Double, /** How many peer bearings drove the yaw/mirror fit. */ val bearingsUsed: Int, + /** |resR − resN|: how DECISIVELY azimuth picked the chirality. A near-zero margin means the + * bearing set is reflection-ambiguous and the mirror is a coin-flip — the caller should + * refuse to latch. ∞ default so the raised-anchor path (which never sets it) is unaffected. */ + val mirrorMarginRad: Double = Double.POSITIVE_INFINITY, ) { /** Map a gauge-frame position into the oriented world frame (+Z up, AoA-headed). */ fun apply(p: Vector3D): Vector3D { @@ -112,7 +116,126 @@ object ConstellationOrientation { val reflect = resR < resN val yaw = if (reflect) yawR else yawN val res = if (reflect) resR else resN - return Result(level, reflect, Quaternion.fromAxisAngle(Vector3D.UNIT_Z, yaw), res, azList.size) + val mirrorMargin = kotlin.math.abs(resR - resN) + return Result( + level, reflect, Quaternion.fromAxisAngle(Vector3D.UNIT_Z, yaw), res, azList.size, mirrorMargin, + ) + } + + /** A best-fit plane through the anchor cloud: its unit [normal] (sign canonicalized so it is the + * SAME direction every call for a fixed input) and its [planarityRatio] = λ1/λ0 (how planar the + * cloud is — large ⇒ a genuine near-2-D slab whose normal is a trustworthy "up" axis). */ + data class PlaneAxis(val normal: Vector3D, val planarityRatio: Double) + + /** + * Leveling axis for a near-horizontal anchor SLAB (the room-scale case with no physically-raised + * anchor): the least-variance principal axis of the anchor point cloud = the best-fit plane + * normal. Robust where [upFromBasePlane]'s single-raised-anchor cue has no signal. Returns the + * axis with an ARBITRARY-BUT-DETERMINISTIC sign (a canonical +direction) — resolving up-vs-down is + * the caller's job (it is the one bit distance geometry can never supply). Null if fewer than 3 + * anchors or the cloud is collinear (no plane). Pure + deterministic (fixed-sweep Jacobi). + */ + fun bestFitPlaneNormal(positions: Map): PlaneAxis? { + // Accumulate in a STABLE (id-sorted) order: floating-point addition is not associative, so a + // different map-iteration order would give bit-different sums and reintroduce run-to-run normal + // variation — the exact thing the anti-twist invariant forbids. + val pts = positions.entries.sortedBy { it.key.value }.map { it.value } + if (pts.size < 3) return null + val c = pts.reduce { a, b -> a + b } / pts.size.toDouble() + var xx = 0.0; var xy = 0.0; var xz = 0.0; var yy = 0.0; var yz = 0.0; var zz = 0.0 + for (p in pts) { + val d = p - c + xx += d.x * d.x; xy += d.x * d.y; xz += d.x * d.z + yy += d.y * d.y; yz += d.y * d.z; zz += d.z * d.z + } + val (evals, evecs) = jacobiEigenSymmetric3(xx, xy, xz, yy, yz, zz) // ascending λ + val l0 = evals[0]; val l1 = evals[1] + if (l1 < 1e-9) return null // collinear → no plane + var n = evecs[0].normalize() // least-variance axis + // Canonicalize the sign so accumulation across ticks references the SAME +normal every tick + // (the Jacobi eigenvector sign is arbitrary; this makes it reproducible). + if (n.z < 0.0 || (n.z == 0.0 && (n.x < 0.0 || (n.x == 0.0 && n.y < 0.0)))) n = -n + return PlaneAxis(n, l1 / kotlin.math.max(l0, 1e-12)) + } + + /** + * One per-tick VOTE on whether [normal] points physically up: correlate each peer's measured AoA + * elevation sign with the sign of its gauge height along +[normal] (the root is at the gauge + * origin, so `p·normal` IS the peer's signed height-above-root). E[vote] > 0 iff +normal is up. + * Weighted by |elevation| as a soft confidence. Elevation is used ONLY for this one sign bit, + * never for placement. Accumulate votes over ticks ([UpSignAccumulator]) — one tick is too noisy. + */ + fun elevationUpSignVote( + positions: Map, + elevations: Map, + normal: Vector3D, + rootId: DeviceId, + ): Double { + var vote = 0.0 + for ((id, el) in elevations) { + if (id == rootId || !el.isFinite()) continue + val h = (positions[id] ?: continue).dot(normal) + if (kotlin.math.abs(h) < 1e-6) continue + vote += (if (h >= 0.0) 1.0 else -1.0) * el + } + return vote + } + + /** + * Deterministic symmetric-3×3 eigensolver (cyclic Jacobi, fixed sweep count). Input is the upper + * triangle of the covariance. Returns (eigenvalues ASCENDING, matching eigenvectors). Determinism + * — a fixed sweep count, a fixed pair order, and the stable-rotation formula — is load-bearing: + * it is what makes [bestFitPlaneNormal] return a bit-identical normal run-to-run (the anti-twist + * invariant), so the caller's sign accumulation always references the same axis. + */ + private fun jacobiEigenSymmetric3( + xx: Double, xy: Double, xz: Double, yy: Double, yz: Double, zz: Double, + ): Pair> { + val a = arrayOf( + doubleArrayOf(xx, xy, xz), + doubleArrayOf(xy, yy, yz), + doubleArrayOf(xz, yz, zz), + ) + val v = arrayOf( + doubleArrayOf(1.0, 0.0, 0.0), + doubleArrayOf(0.0, 1.0, 0.0), + doubleArrayOf(0.0, 0.0, 1.0), + ) + val pairs = arrayOf(intArrayOf(0, 1), intArrayOf(0, 2), intArrayOf(1, 2)) + repeat(16) { + for (pq in pairs) { + val p = pq[0]; val q = pq[1] + val apq = a[p][q] + if (kotlin.math.abs(apq) < 1e-300) continue + // Stable Jacobi rotation that zeros a[p][q] (Numerical-Recipes form): smaller angle. + val theta = (a[q][q] - a[p][p]) / (2.0 * apq) + val t = (if (theta >= 0.0) 1.0 else -1.0) / + (kotlin.math.abs(theta) + kotlin.math.sqrt(theta * theta + 1.0)) + val c = 1.0 / kotlin.math.sqrt(t * t + 1.0) + val s = t * c + val h = t * apq + a[p][p] -= h; a[q][q] += h; a[p][q] = 0.0; a[q][p] = 0.0 + for (k in 0..2) { + if (k == p || k == q) continue + val akp = a[k][p]; val akq = a[k][q] + a[k][p] = c * akp - s * akq; a[p][k] = a[k][p] + a[k][q] = s * akp + c * akq; a[q][k] = a[k][q] + } + for (k in 0..2) { + val vkp = v[k][p]; val vkq = v[k][q] + v[k][p] = c * vkp - s * vkq + v[k][q] = s * vkp + c * vkq + } + } + } + val evalsUnsorted = doubleArrayOf(a[0][0], a[1][1], a[2][2]) + val evecsUnsorted = arrayOf( + Vector3D(v[0][0], v[1][0], v[2][0]), + Vector3D(v[0][1], v[1][1], v[2][1]), + Vector3D(v[0][2], v[1][2], v[2][2]), + ) + val order = intArrayOf(0, 1, 2).sortedBy { evalsUnsorted[it] } + return DoubleArray(3) { evalsUnsorted[order[it]] } to Array(3) { evecsUnsorted[order[it]] } } /** @@ -136,25 +259,52 @@ object ConstellationOrientation { rootId: DeviceId, elevationMarginRad: Double = 0.20, // the raised anchor must read CLEARLY highest (~11°) minRaiseMeters: Double = 0.10, // …and stand at least this far off the base plane - ): Vector3D? { + ): Vector3D? = + pickRaisedAnchor(positions, elevations, rootId, elevationMarginRad, minRaiseMeters) + ?.let { upFromRaised(positions, it) } + + /** + * The unambiguously-RAISED anchor (the table cue): the peer that reads CLEARLY highest on AoA + * elevation (by [elevationMarginRad] over the runner-up) AND stands at least [minRaiseMeters] off + * the base plane of the others — or null if none does. Split out of [upFromBasePlane] so the + * engine can require this pick to PERSIST across several ticks before trusting it; a single noisy + * tick must never crown a flat anchor (that single-tick crowning is the intermittent-twist bug). + * Only the elevation ORDER is used, never its magnitude for placement. + */ + fun pickRaisedAnchor( + positions: Map, + elevations: Map, + rootId: DeviceId, + elevationMarginRad: Double = 0.20, + minRaiseMeters: Double = 0.10, + ): DeviceId? { if (positions.size < 4) return null // need 3 base + 1 raised for an out-of-plane up - // Rank peers by AoA elevation and accept a raised anchor ONLY if it is unambiguously the - // highest — a clear margin over the runner-up. This is what stops noise (or a poor vantage) - // from flipping the pick tick to tick, which was flipping the whole frame's "up". val ranked = positions.keys.filter { it != rootId && it in elevations } .sortedByDescending { elevations.getValue(it) } - if (ranked.isEmpty()) return null // no AoA elevation yet → don't guess "up" + if (ranked.isEmpty()) return null val raised = ranked[0] val runnerUp = ranked.getOrNull(1)?.let { elevations.getValue(it) } ?: Double.NEGATIVE_INFINITY - if (elevations.getValue(raised) - runnerUp < elevationMarginRad) return null // ambiguous → wait + if (elevations.getValue(raised) - runnerUp < elevationMarginRad) return null // ambiguous → no pick val base = positions.filterKeys { it != raised }.values.toList() if (base.size < 3) return null + val n = (base[1] - base[0]).cross(base[2] - base[0]) + if (n.magnitude < 1e-9) return null + val centroid = (base[0] + base[1] + base[2]) / 3.0 + val perp = (positions.getValue(raised) - centroid).dot(n.normalize()) + if (kotlin.math.abs(perp) < minRaiseMeters) return null // ~coplanar → not genuinely raised + return raised + } + + /** World-up-in-gauge from a confirmed [raisedId]: the base plane's normal oriented toward the + * raised anchor. The original [upFromBasePlane] leveling math, reusable once the raise is trusted. */ + fun upFromRaised(positions: Map, raisedId: DeviceId): Vector3D? { + val base = positions.filterKeys { it != raisedId }.values.toList() + if (base.size < 3) return null var n = (base[1] - base[0]).cross(base[2] - base[0]) - if (n.magnitude < 1e-9) return null // base points collinear → no plane + if (n.magnitude < 1e-9) return null n = n.normalize() val centroid = (base[0] + base[1] + base[2]) / 3.0 - val perp = (positions.getValue(raised) - centroid).dot(n) // signed height off the base plane - if (kotlin.math.abs(perp) < minRaiseMeters) return null // ~coplanar → "up" is undetermined + val perp = (positions.getValue(raisedId) - centroid).dot(n) if (perp < 0.0) n = -n return n } 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 5955917..7d31adb 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt @@ -400,6 +400,13 @@ class MultiObserverFusionEngine( * frame stops wobbling with per-tick AoA noise. Cleared on a frame re-establish to re-orient. */ private var frameOrientation: ConstellationOrientation.Result? = null + /** Two-tier orientation state (see [bootstrapReferenceConstellation]): the up-sign significance + * accumulator for the near-horizontal-slab case, and the raised-anchor persistence tracker for + * the table case. Reset wherever [frameOrientation] is cleared, so a re-establish re-earns them. */ + private val upSignAcc = UpSignAccumulator() + private var raisedCandidate: DeviceId? = null + private var raisedStreak = 0 + /** True once the reference frame is fully ESTABLISHED — distance-solved, oriented, and seated. * This is the FRAME LOCK. While set, BOTH [bootstrapReferenceConstellation] and * [refineAnchorConstellation] hold the constellation exactly in place and stop re-seating @@ -437,7 +444,15 @@ class MultiObserverFusionEngine( /** Drop the locked orientation AND release the established hold, so the next maintenance tick * re-solves and re-orients from scratch. Call on a deliberate re-calibration (anchors * repositioned) — [initializeAsRoot] already does this on reset. */ - fun resetOrientationLock() { frameOrientation = null; frameEstablished = false; resetLockStability() } + fun resetOrientationLock() { resetOrientationState(); frameEstablished = false; resetLockStability() } + + /** Drop the committed orientation AND the evidence that earned it (up-sign accumulator + raised + * persistence), so a re-establish re-derives level/mirror/up-sign from scratch rather than + * inheriting stale confidence. */ + private fun resetOrientationState() { + frameOrientation = null + upSignAcc.reset(); raisedCandidate = null; raisedStreak = 0 + } /** * True if any current reference anchor is reporting motion — the override that releases the @@ -587,7 +602,7 @@ class MultiObserverFusionEngine( observationCollector.addReferencePoint( ReferencePoint(id = deviceId, position = Vector3D.ZERO), ) - frameOrientation = null // fresh frame → re-establish orientation + resetOrientationState() // fresh frame → re-earn orientation (level + up-sign + mirror) frameEstablished = false // …and re-solve the constellation from scratch frameWasLocked = false // fresh frame → next lock is a first establishment, not recovery resetLockStability() // …and the trust/stability run starts over @@ -597,7 +612,7 @@ class MultiObserverFusionEngine( /** Drop all engine state — used in tests and on hard reset. */ fun reset() { - frameOrientation = null + resetOrientationState() frameEstablished = false frameWasLocked = false resetLockStability() @@ -1453,9 +1468,37 @@ class MultiObserverFusionEngine( // re-orients. Until a confident orientation exists the frame stays in the raw gauge — // stable, just not yet levelled ("placed-raw"). if (frameOrientation == null) { - val up = ConstellationOrientation.upFromBasePlane(solved, selfElevations, rootId) + // TWO-TIER, CONFIDENCE-GATED leveling — commit an orientation ONLY when level + up-sign + + // mirror are all confident, else leave it null so the frame stays honestly CONVERGING + // rather than latching a twisted/inverted gauge (the intermittent-twist bug). + // + // TIER 1 — a GENUINELY raised anchor (table case). Require the pick to PERSIST for several + // ticks so a single noisy tick can no longer crown a flat anchor and flip "up". + val rid = ConstellationOrientation.pickRaisedAnchor(solved, selfElevations, rootId) + if (rid != null && rid == raisedCandidate) raisedStreak++ + else { raisedCandidate = rid; raisedStreak = if (rid != null) 1 else 0 } + val raisedUp = if (rid != null && raisedStreak >= config.constellationRaisedPersistTicks) + ConstellationOrientation.upFromRaised(solved, rid) else null + + // TIER 2 — near-horizontal SLAB (no raised anchor): the best-fit plane normal gives the + // leveling AXIS; the up/down SIGN is the one bit distances+azimuth cannot see, resolved by + // an accumulated elevation-sign significance test (never guesses on a truly-flat set). + val up: Vector3D? = raisedUp ?: run { + val axis = ConstellationOrientation.bestFitPlaneNormal(solved) + if (axis == null || axis.planarityRatio < config.constellationMinPlanarityRatio) null + else { + upSignAcc.observe( + ConstellationOrientation.elevationUpSignVote(solved, selfElevations, axis.normal, rootId), + ) + if (upSignAcc.confident) axis.normal * upSignAcc.sign else null + } + } + val o = up?.let { ConstellationOrientation.solve(solved, it, selfBearings, rootId) } - if (o != null && o.bearingsUsed >= 2) frameOrientation = o + // Commit only on a confident level AND a decisively-picked (non-coin-flip) mirror. + if (o != null && o.bearingsUsed >= 2 && o.mirrorMarginRad >= config.constellationMirrorMarginRad) { + frameOrientation = o + } } val orientation = frameOrientation val placedPositions = diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/UpSignAccumulator.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/UpSignAccumulator.kt new file mode 100644 index 0000000..0033c94 --- /dev/null +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/UpSignAccumulator.kt @@ -0,0 +1,56 @@ +package com.aether.mofe.engine + +import kotlin.math.abs +import kotlin.math.max +import kotlin.math.sqrt + +/** + * Resolves the ONE bit a near-horizontal anchor slab cannot observe on any single tick: whether the + * best-fit plane normal points physically UP or DOWN. Distance geometry can't supply it, gravity+ + * azimuth can't (an up-flip is exactly the in-plane y-mirror), and the only cue — AoA elevation sign — + * is barely observable per tick (true peer elevations of a few degrees against a ±15° noise floor). So + * this runs a SEQUENTIAL SIGNIFICANCE TEST over the per-tick votes from + * [ConstellationOrientation.elevationUpSignVote]: + * + * • a genuinely tilted signal (a real slab: peers truly above/below the root) makes the mean vote + * drift off zero, so the z-statistic grows ~√ticks and eventually crosses [zThreshold] → [confident] + * on the CORRECT [sign]; + * • a truly-flat / unbiased set (all peers at the root's height) has zero-mean votes that random-walk + * with a bounded z-statistic → [confident] NEVER becomes true, so the engine refuses to guess a + * coin-flip up-sign (it stays honestly un-oriented instead of latching a twisted frame). + * + * Pure and dependency-free; unit-testable with synthetic vote streams. + */ +class UpSignAccumulator( + /** z-statistic needed to declare the sign known. Higher = safer (fewer wrong latches), slower. */ + private val zThreshold: Double = 4.0, + /** Never declare confidence before this many votes (guards tiny-sample flukes). */ + private val minTicks: Int = 6, +) { + private var sum = 0.0 + private var sumSq = 0.0 + private var ticks = 0 + + /** Fold in one per-tick vote (sign·|elevation| correlation); ignores non-finite votes. */ + fun observe(vote: Double) { + if (vote.isFinite()) { sum += vote; sumSq += vote * vote; ticks++ } + } + + /** True once the accumulated votes are statistically significant (|mean| ≫ its standard error). */ + val confident: Boolean + get() { + if (ticks < minTicks) return false + val mean = sum / ticks + val varr = max(sumSq / ticks - mean * mean, 1e-12) + val se = sqrt(varr / ticks) // standard error of the mean vote + return abs(mean) / se >= zThreshold + } + + /** +1 if the plane normal is up, −1 if it must be flipped. Meaningful only when [confident]. */ + val sign: Double get() = if (sum >= 0.0) 1.0 else -1.0 + + /** Number of votes folded in so far (diagnostic). */ + val voteCount: Int get() = ticks + + fun reset() { sum = 0.0; sumSq = 0.0; ticks = 0 } +} diff --git a/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt b/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt index d6e3fba..9d90dee 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt @@ -430,6 +430,21 @@ data class MofeConfig( * for them to count as "agreeing" for the stability gate. */ val anchorConstellationLockStabilityEpsilonMeters: Double = 0.02, + // ── Orientation (leveling / mirror) confidence gates ─────────────────────── + // A near-horizontal anchor SLAB (no physically-raised anchor) has no single-anchor "up" cue, so + // the frame levels off the best-fit plane normal and resolves up-vs-down by an accumulated + // elevation-sign significance test. These gate committing an orientation so a noisy tick can never + // latch a twisted/inverted frame; unconfident ⇒ the frame stays CONVERGING (honest non-latch). + /** The anchor cloud must be this planar (λ1/λ0 of its covariance) before its plane normal is + * trusted as a leveling axis — rejects a non-planar set where the normal is meaningless. */ + val constellationMinPlanarityRatio: Double = 6.0, + /** Minimum |resR − resN| (radians) azimuth-chirality margin to commit a mirror — a smaller margin + * is a reflection-ambiguous bearing set and must NOT latch a coin-flip. */ + val constellationMirrorMarginRad: Double = 0.30, + /** A raised anchor must clear the elevation-margin gate this many CONSECUTIVE ticks before the + * table-case leveling is trusted — stops a single noise tick from crowning a flat anchor. */ + val constellationRaisedPersistTicks: Int = 5, + /** * Maximum age of a buffered UWB range before it's discarded as stale. * Cross-anchor fusion waits for ranges from multiple anchors with diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationSlabOrientationTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationSlabOrientationTest.kt new file mode 100644 index 0000000..ea5ae95 --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/ConstellationSlabOrientationTest.kt @@ -0,0 +1,163 @@ +package com.aether.mofe.engine + +import com.aether.mofe.model.DeviceId +import com.aether.mofe.model.Quaternion +import com.aether.mofe.model.Vector3D +import kotlin.math.PI +import kotlin.math.atan2 +import kotlin.math.hypot +import kotlin.random.Random +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.test.assertFalse +import kotlin.test.assertNotNull +import kotlin.test.assertTrue + +/** + * The room-scale-slab orientation fix (#44): a near-horizontal 4-anchor layout with NO physically + * raised anchor used to orient off a noise-crowned "raised" pick, flipping the leveling up-SIGN at + * random (→ intermittent twisted/mirrored frame, never latching). The fix levels off the best-fit + * plane normal and resolves up-vs-down by an accumulated elevation-sign significance test. These + * tests use the REAL field survey and prove: the slab orients to the correct (never inverted) up + * across many noise seeds, a truly-flat set refuses to orient, and the mirror margin gates coin-flips. + */ +class ConstellationSlabOrientationTest { + + private val a1 = DeviceId("A1") // root + private val a2 = DeviceId("A2") + private val a3 = DeviceId("A3") + private val a4 = DeviceId("A4") + // Real survey (world frame). A4 is the physically LOWEST (h=0.889); A1 the root. + private val world = mapOf( + a1 to Vector3D(0.0, 0.0, 1.2318), + a2 to Vector3D(2.286, 0.0, 1.3843), + a3 to Vector3D(0.0, 3.6957, 1.4478), + a4 to Vector3D(2.286, 2.921, 0.889), + ) + private val peers = listOf(a2, a3, a4) + + /** Root-relative world coords (root at origin, as the gauge has it). */ + private val rel = world.mapValues { (_, p) -> p - world.getValue(a1) } + private val trueElev = peers.associateWith { atan2(rel.getValue(it).z, hypot(rel.getValue(it).x, rel.getValue(it).y)) } + private val trueAz = peers.associateWith { atan2(rel.getValue(it).y, rel.getValue(it).x) } + + /** A fixed, non-trivial gauge rotation — the distance solve returns the shape in an arbitrary + * orientation, so the fix must recover world-up regardless of it. */ + private val gaugeRot = Quaternion.fromAxisAngle(Vector3D(0.3, 0.5, 0.8).normalize(), 0.7) + private val gauge: Map = rel.mapValues { (_, p) -> gaugeRot.rotate(p) } + + private fun noisy(base: Map, rng: Random, boundRad: Double) = + base.mapValues { (_, v) -> v + (rng.nextDouble() - 0.5) * 2.0 * boundRad } + + // ── UpSignAccumulator ────────────────────────────────────────────────────── + @Test + fun accumulator_latches_a_biased_stream_and_never_a_flat_one() { + val rng = Random(1) + val up = UpSignAccumulator() + repeat(60) { up.observe(0.2 + (rng.nextDouble() - 0.5) * 0.3) } // mean +0.2 + assertTrue(up.confident && up.sign == 1.0, "a biased +stream converges to +1") + + val down = UpSignAccumulator() + repeat(60) { down.observe(-0.2 + (rng.nextDouble() - 0.5) * 0.3) } // mean -0.2 + assertTrue(down.confident && down.sign == -1.0, "a biased -stream converges to -1") + + val flat = UpSignAccumulator() + repeat(1000) { flat.observe((rng.nextDouble() - 0.5) * 0.6) } // zero mean + assertFalse(flat.confident, "a zero-mean stream must NEVER reach confidence (no coin-flip)") + + val tiny = UpSignAccumulator() + repeat(3) { tiny.observe(5.0) } + assertFalse(tiny.confident, "fewer than minTicks is never confident") + } + + // ── bestFitPlaneNormal ───────────────────────────────────────────────────── + @Test + fun best_fit_plane_normal_is_accurate_and_deterministic() { + val axis = ConstellationOrientation.bestFitPlaneNormal(gauge) + assertNotNull(axis) + val trueUp = gaugeRot.rotate(Vector3D.UNIT_Z) + val cos = kotlin.math.abs(axis.normal.dot(trueUp)) + assertTrue(cos > kotlin.math.cos(6.0 * PI / 180.0), "plane normal within 6° of true up; cos=$cos") + assertTrue(axis.planarityRatio > 6.0, "the slab is genuinely planar; ratio=${axis.planarityRatio}") + + // DETERMINISM (the anti-twist invariant): different key-insertion orders → bit-identical normal. + val shuffled = linkedMapOf().apply { + listOf(a4, a1, a3, a2).forEach { put(it, gauge.getValue(it)) } + } + repeat(10) { + val again = ConstellationOrientation.bestFitPlaneNormal(shuffled)!!.normal + assertEquals(axis.normal.x, again.x, 0.0) + assertEquals(axis.normal.y, again.y, 0.0) + assertEquals(axis.normal.z, again.z, 0.0) + } + } + + // ── The core regression: reproduce + kill the intermittent twist ─────────── + @Test + fun real_slab_orients_to_correct_up_across_many_noise_seeds_never_inverted() { + var latched = 0 + val seeds = 60 + for (seed in 0 until seeds) { + val rng = Random(seed.toLong() * 2654435761L) + val acc = UpSignAccumulator() + var oriented: ConstellationOrientation.Result? = null + for (tick in 0 until 400) { + val elev = noisy(trueElev, rng, 0.35) // ±20° elevation noise ≫ the true 3–5° signal + val az = noisy(trueAz, rng, 0.26) // ±15° azimuth noise + val axis = ConstellationOrientation.bestFitPlaneNormal(gauge) ?: continue + acc.observe(ConstellationOrientation.elevationUpSignVote(gauge, elev, axis.normal, a1)) + if (!acc.confident) continue + val up = axis.normal * acc.sign + val o = ConstellationOrientation.solve(gauge, up, az, a1) + if (o != null && o.bearingsUsed >= 2 && o.mirrorMarginRad >= 0.30) { oriented = o; break } + } + assertNotNull(oriented, "seed $seed must latch an orientation within the tick budget") + // ANTI-TWIST: recovered heights must POSITIVELY correlate with true heights — i.e. the + // up-sign is not inverted. (Plane-normal leveling carries an inherent ~6° tilt for this + // slab, which can swap the two near-equal-height anchors A2/A3 that differ by only 7 cm; + // that residual tilt is the documented IMU-refinement follow-on, NOT the twist.) + val z = peers.associateWith { oriented.apply(gauge.getValue(it)).z } + val corr = peers.sumOf { z.getValue(it) * rel.getValue(it).z } + assertTrue(corr > 0.0, "seed $seed: recovered heights must correlate with truth (up NOT inverted); z=$z") + // The clearly-lowest anchor (A4, 0.56 m below the top) stays below the highest despite tilt. + assertTrue(z.getValue(a4) < z.getValue(a3), "seed $seed: A4 stays below A3 (no inversion); z=$z") + latched++ + } + assertEquals(seeds, latched, "every seed latched a correct, non-twisted orientation") + } + + @Test + fun a_truly_flat_slab_never_orients() { + // All anchors at the ROOT's height → true elevations 0 → only noise → the significance gate + // must never crown an up-sign, so the frame stays honestly un-oriented (CONVERGING). + val flatWorld = mapOf( + a1 to Vector3D(0.0, 0.0, 1.20), + a2 to Vector3D(2.0, 0.0, 1.20), + a3 to Vector3D(0.0, 3.0, 1.20), + a4 to Vector3D(2.0, 2.5, 1.20), + ) + val flatRel = flatWorld.mapValues { (_, p) -> p - flatWorld.getValue(a1) } + val flatGauge = flatRel.mapValues { (_, p) -> gaugeRot.rotate(p) } + val zeroElev = peers.associateWith { 0.0 } + val acc = UpSignAccumulator() + val rng = Random(7) + repeat(500) { + val elev = noisy(zeroElev, rng, 0.26) + val axis = ConstellationOrientation.bestFitPlaneNormal(flatGauge)!! + acc.observe(ConstellationOrientation.elevationUpSignVote(flatGauge, elev, axis.normal, a1)) + } + assertFalse(acc.confident, "a truly-flat slab must never reach up-sign confidence over 500 ticks") + } + + // ── mirror margin gates a coin-flip ──────────────────────────────────────── + @Test + fun mirror_margin_equals_residual_gap_and_is_large_for_a_chiral_layout() { + val up = gaugeRot.rotate(Vector3D.UNIT_Z) // correct up → solve levels correctly + val o = ConstellationOrientation.solve(gauge, up, trueAz, a1) + assertNotNull(o) + // This layout is genuinely chiral (azimuths span >90°), so the wrong mirror fits far worse → + // a large margin ≫ the 0.30 rad gate. (The engine refuses to latch only when this is small.) + assertTrue(o.mirrorMarginRad >= 0.30, "chiral layout picks its mirror decisively; margin=${o.mirrorMarginRad}") + assertFalse(o.reflected, "no reflection needed when the gauge is a pure rotation of world") + } +} -- 2.43.0