Aether

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

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<DeviceId, Vector3D>): 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<DeviceId, Vector3D>,

elevations: Map<DeviceId, Double>,

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<DoubleArray, Array<Vector3D>> {

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]] }

): 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<DeviceId, Vector3D>,

elevations: Map<DeviceId, Double>,

rootId: DeviceId,

elevationMarginRad: Double = 0.20,

minRaiseMeters: Double = 0.10,

): DeviceId? {

if (ranked.isEmpty()) return null

if (elevations.getValue(raised) - runnerUp < elevationMarginRad) return null // ambiguous → no pick

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<DeviceId, Vector3D>, raisedId: DeviceId): Vector3D? {

val base = positions.filterKeys { it != raisedId }.values.toList()

if (base.size < 3) return null

if (n.magnitude < 1e-9) return null

val perp = (positions.getValue(raisedId) - centroid).dot(n)

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

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

}

resetOrientationState() // fresh frame → re-earn orientation (level + up-sign + mirror)

resetOrientationState()

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

}

}

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

}

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 }

}

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

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<DeviceId, Vector3D> = rel.mapValues { (_, p) -> gaugeRot.rotate(p) }

private fun noisy(base: Map<DeviceId, Double>, 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<DeviceId, Vector3D>().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")

}

}