Aether

/**

* #63 — inter-anchor edges flagged BIASED by GRAPH RESIDUAL on the given [positions] and measured

* [distances]: a stable, lopsided link the rigid solve cannot reconcile with the rest of the graph

* (the capture4 cross-room bias the triangle-inequality guard and per-link health both miss).

* Returns each biased pair (canonical order) → its residual, EMPTY unless the graph is REDUNDANT

* (≥5 anchors / >3n−6 edges) — on an exactly-rigid graph a single bias is absorbed with ZERO

* residual and is undetectable (see [InterAnchorLinkReport.biasedEdges]). Adds a STABILITY gate on

* top of the pure detector: a bias is trusted only from a WELL-SAMPLED link (≥ strongMinSamples),

* never a transient bad read — reusing the same per-link sample accounting the STRONG/WEAK health

* uses. Single-sources detection for both [interAnchorLinkReport]'s flag and the refine down-weight.

*/

private fun biasedInterAnchorEdges(

positions: Map<DeviceId, Vector3D>,

distances: Map<Pair<DeviceId, DeviceId>, Double>,

): Map<Pair<DeviceId, DeviceId>, Double> {

val candidates = InterAnchorLinkReport.biasedEdges(positions, distances, interAnchorLinkThresholds)

if (candidates.isEmpty()) return candidates

return candidates.filterKeys { pair ->

val st = interAnchorLinkStats[pair] ?: interAnchorLinkStats[pair.second to pair.first]

(st?.count ?: 0) >= interAnchorLinkThresholds.strongMinSamples

}

}

/**

* #69b-ii — per-edge weights that down-weight the biased edges [biasedInterAnchorEdges] finds, for

* [AnchorConstellationSolver.refine]. GUARDRAIL: never push the solve under-determined. A rigid 3-D

* graph of n nodes needs ≥ 3n−6 full-weight edges; only the SURPLUS is down-weighted (most-biased

* first), so at least 3n−6 edges keep unit weight. Detection already requires a redundant graph

* (edges > 3n−6), so the budget is ≥ 1 for a lone biased edge. No flags (or the feature off) ⇒

* EMPTY map ⇒ the refine is byte-identical to the un-weighted solve.

*/

private fun biasedEdgeWeights(

positions: Map<DeviceId, Vector3D>,

distances: Map<Pair<DeviceId, DeviceId>, Double>,

): Map<Pair<DeviceId, DeviceId>, Double> {

if (!config.anchorConstellationDownWeightBiasedEnabled) return emptyMap()

val biased = biasedInterAnchorEdges(positions, distances)

if (biased.isEmpty()) return emptyMap()

val usable = distances.keys.filter { it.first in positions && it.second in positions }

val nodes = usable.flatMapTo(HashSet()) { listOf(it.first, it.second) }

val budget = usable.size - maxOf(3 * nodes.size - 6, 0) // full-weight edges to preserve = 3n−6

if (budget <= 0) return emptyMap()

val w = config.anchorConstellationBiasedEdgeWeight

return biased.entries.sortedByDescending { it.value }.take(budget).associate { it.key to w }

}

// #69b-ii: down-weight any inter-anchor edge flagged BIASED by graph residual on the CURRENT

// (already-placed) constellation, so the good edges out-vote the bias in this refine instead of

// the frame warping to absorb it. EMPTY on a rigid/clean graph (or the feature off) ⇒ the

// solve is byte-identical to before. The guardrail in [biasedEdgeWeights] keeps ≥ 3n−6 full-

// weight edges so the graph never goes under-determined.

val edgeWeights = biasedEdgeWeights(anchors, edges)

weights = edgeWeights,

// #63: mark stable, lopsided edges by graph residual on the current solved constellation. Root

// sits at the gauge origin; distances are rotation-invariant so the oriented frame positions

// serve directly. EMPTY on a rigid/clean graph ⇒ no biased flags ⇒ report is unchanged.

val rootId = frameManager.topology?.frame?.originDeviceId

val positions = buildMap {

if (rootId != null) put(rootId, Vector3D.ZERO)

frameManager.getAllReferencePoints().forEach { if (it.id != rootId) put(it.id, it.position) }

}

val corrected = correctedInterAnchorDistances()

.filterKeys { it.first in positions && it.second in positions }

val biasedPairs = biasedInterAnchorEdges(positions, corrected).keys

return InterAnchorLinkReport.build(ids, samples, nowMicros, interAnchorLinkThresholds, biasedPairs)

/** #63/#69b-ii: down-weight an inter-anchor edge the graph-residual detector flags BIASED (a

* stable, lopsided link — see [InterAnchorLinkReport.biasedEdges]) in the leader's rigid-body

* refine, so the good edges out-vote the bias instead of the frame warping to absorb it. Only

* ever bites on a REDUNDANT graph (≥5 anchors / >3n−6 edges) with a dominant, well-sampled

* outlier, and never down-weights so many edges that the solve goes under-determined — so with

* no such edge it is a no-op (empty weight map ⇒ byte-identical solve). */

val anchorConstellationDownWeightBiasedEnabled: Boolean = true,

/** Weight applied to a down-weighted biased inter-anchor edge. ≪1 so the good edges dominate the

* fit, but non-zero so the edge is only SOFTENED, not deleted — the distance graph stays connected. */

val anchorConstellationBiasedEdgeWeight: Double = 0.01,

import kotlin.math.abs

/** #63 graph-residual BIAS detection ([InterAnchorLinkReport.biasedEdges]). An edge is flagged

* biased when its post-solve graph residual |‖p_a−p_b‖ − d| clears BOTH this absolute floor (m) … */

val biasResidualMeters: Double = 0.10,

/** … AND this multiple of the MEDIAN edge residual, so only a lopsided edge that stands out from

* the consensus is flagged — a uniform/global error (every edge equally off) flags nothing. On a

* minimally-redundant graph (5 anchors, one surplus edge) the solve spreads a lone bias the most,

* leaving the biased edge only ~2.4× the median, so the gate sits at 2× (the biased edge is still

* the sole one clearing both this AND the absolute floor). */

val biasDominanceRatio: Double = 2.0,

/** #63: a stable, LOPSIDED link — it ranges fine (often [InterAnchorLinkStatus.STRONG]) yet carries

* a large, dominant post-solve GRAPH residual the rigid solve could not absorb, i.e. a directional

* ranging bias. Orthogonal to [status] (a biased edge is usually still "ranging well"). Only ever

* set on a REDUNDANT constellation (≥5 anchors / >3n−6 edges); on an exactly-rigid graph a bias is

* absorbed with zero residual and is undetectable this way. See [InterAnchorLinkReport.biasedEdges]. */

val biased: Boolean = false,

/** Links flagged [InterAnchorLink.biased] by graph residual (#63). 0 unless [build] was given the

* solved positions to detect against, and the constellation is redundant enough to detect at all. */

val biased: Int = 0,

/** At least one stable, lopsided inter-anchor link — a directional ranging bias that a redundant

* solve can down-weight (#69b-ii) but a rigid one silently absorbs into a warped frame. */

val hasBiasedLink: Boolean get() = biased > 0

*

* [biasedPairs] (canonical order, from [biasedEdges]) marks the [InterAnchorLink.biased] flag

* on the matching links. Empty (the default) ⇒ every link biased=false and biased count 0 —

* byte-identical to the ranging-only classification.

biasedPairs: Set<Pair<DeviceId, DeviceId>> = emptySet(),

var biased = 0

// A MISSING link has no distance, so it can never be in biasedPairs; a biased flag

// therefore only ever lands on a link that is actually ranging.

val isBiased = key in biasedPairs

if (isBiased) biased++

biased = isBiased,

return InterAnchorLinkReport(

links, ids.size, ids.size * (ids.size - 1) / 2, strong, weak, missing, biased,

)

}

/**

* #63 — flag stable, lopsided inter-anchor edges by GRAPH RESIDUAL. Given the solved anchor

* [positions] (any rigid orientation — inter-anchor distances are rotation/translation

* invariant) and the measured inter-anchor [distances], return each biased edge (canonical

* order) mapped to its post-solve residual |‖p_a−p_b‖ − d|.

*

* The gap this closes: the triangle-inequality guard ([CrossAnchorFuser.applyGeometric-

* ConsistencyCheck]) catches only distances that break geometry outright, and the per-link

* STRONG/WEAK health catches a large per-sample residual — but a TIGHT, STABLE bias (a fixed

* offset on one cross-room link, the capture4 field failure) breaks neither: it ranges

* consistently and can satisfy the triangle inequality. It only shows up once the WHOLE graph

* is solved together and one edge cannot be reconciled with the rest.

*

* ── CRITICAL CORRECTNESS INVARIANT (honor + do not "optimise" away) ───────────────────────

* This works ONLY on a REDUNDANT (over-determined) graph. A rigid 3-D constellation of n

* nodes has 3n−6 internal DOF. With EXACTLY 3n−6 edges (n=4 ⇒ 6 edges = complete K4) the

* least-squares solve has just enough freedom to satisfy every edge, so a biased edge is

* absorbed by WARPING the frame and its post-solve residual is ZERO — invisible here. Only

* when edges > 3n−6 (n≥5, e.g. K5 = 10 edges > 9 DOF) does the bias have nowhere to hide and

* surface as a dominant residual. Below that this returns EMPTY, by design, not as a miss.

*

* A flagged edge must be both ABSOLUTELY large (≥ [InterAnchorLinkThresholds.biasResidualMeters])

* and DOMINANT over the median edge residual (≥ ratio×median), so a single lopsided link is

* caught while a uniform/global error (which no single down-weight could fix) flags nothing.

*/

fun biasedEdges(

positions: Map<DeviceId, Vector3D>,

distances: Map<Pair<DeviceId, DeviceId>, Double>,

thresholds: InterAnchorLinkThresholds = InterAnchorLinkThresholds(),

): Map<Pair<DeviceId, DeviceId>, Double> {

// Canonicalize; keep only edges whose BOTH endpoints have a solved position + a valid range.

val edges = HashMap<Pair<DeviceId, DeviceId>, Double>()

for ((pair, d) in distances) {

val (a, b) = pair

if (a == b || d <= 0.0 || !d.isFinite()) continue

if (a !in positions || b !in positions) continue

edges[if (a.value <= b.value) a to b else b to a] = d

}

// n = anchors actually in the ranging graph (not merely positioned) — that is what the

// 3n−6 rigidity DOF is measured against.

val nodes = edges.keys.flatMapTo(HashSet()) { listOf(it.first, it.second) }

val n = nodes.size

if (n < 5) return emptyMap() // < 5 ⇒ at best exactly-rigid ⇒ bias absorbed

val dof = 3 * n - 6

if (edges.size <= dof) return emptyMap() // not redundant ⇒ residual is zero ⇒ undetectable

val resid = edges.mapValues { (pair, d) ->

abs(positions.getValue(pair.first).distanceTo(positions.getValue(pair.second)) - d)

}

val median = resid.values.sorted().let { it[it.size / 2] }

val floor = thresholds.biasResidualMeters

val ratio = thresholds.biasDominanceRatio

return resid.filterValues { r -> r >= floor && r >= ratio * median }

package com.aether.mofe.engine

import com.aether.mofe.integration.MofeTestHarness

import com.aether.mofe.model.DeviceId

import com.aether.mofe.model.InterAnchorLinkReport

import com.aether.mofe.model.InterAnchorLinkSample

import com.aether.mofe.model.MofeConfig

import com.aether.mofe.model.Vector3D

import kotlin.math.abs

import kotlin.test.Test

import kotlin.test.assertEquals

import kotlin.test.assertFalse

import kotlin.test.assertTrue

/**

* #63 (detect) + #69b-ii (down-weight) — flag a STABLE, LOPSIDED inter-anchor edge by GRAPH RESIDUAL,

* then down-weight it in the rigid-body solve so the good edges out-vote the bias.

*

* The gap these close: [CrossAnchorFuser.applyGeometricConsistencyCheck] catches only triangle-

* inequality breaks, and per-link STRONG/WEAK health catches a large per-sample residual — but a

* tight, stable cross-room bias (capture4: +0.57/+0.75 m on the long links) breaks neither and warps

* the frame. It only surfaces once the WHOLE graph is solved and one edge cannot be reconciled.

*

* ── CRITICAL CORRECTNESS FACT (the whole reason for the redundancy gate) ─────────────────────────

* On an EXACTLY-RIGID 4-anchor graph (6 edges = 6 DOF) the least-squares solve has just enough

* freedom to satisfy every edge, so the biased edge's post-solve residual is ZERO — the solve absorbs

* the bias by WARPING the frame and detection is impossible. Graph-residual detection therefore works

* ONLY on a REDUNDANT graph (≥5 anchors / >3n−6 edges). Both facts are asserted below.

*/

class BiasedInterAnchorEdgeTest {

private val a1 = DeviceId("A1"); private val a2 = DeviceId("A2")

private val a3 = DeviceId("A3"); private val a4 = DeviceId("A4"); private val a5 = DeviceId("A5")

private fun dist(a: Vector3D, b: Vector3D) = a.distanceTo(b)

private fun canon(a: DeviceId, b: DeviceId) = if (a.value <= b.value) a to b else b to a

/** Every unordered inter-anchor distance for [pts], with [biasPair] inflated by [bias] m. */

private fun distances(

pts: Map<DeviceId, Vector3D>, biasPair: Pair<DeviceId, DeviceId>, bias: Double,

): Map<Pair<DeviceId, DeviceId>, Double> {

val ids = pts.keys.toList()

val out = HashMap<Pair<DeviceId, DeviceId>, Double>()

for (i in ids.indices) for (j in i + 1 until ids.size) {

val (x, y) = ids[i] to ids[j]

val biased = (x to y) == biasPair || (y to x) == biasPair

out[x to y] = dist(pts.getValue(x), pts.getValue(y)) + if (biased) bias else 0.0

}

return out

}

private val truth5 = mapOf(

a1 to Vector3D(0.0, 0.0, 0.0), a2 to Vector3D(2.0, 0.0, 0.0),

a3 to Vector3D(0.0, 2.0, 0.0), a4 to Vector3D(1.0, 1.0, 0.6), a5 to Vector3D(2.0, 2.0, 0.3),

)

// ── Part A: DETECT ───────────────────────────────────────────────────────────────────────────

@Test

fun exactly_rigid_four_anchor_graph_absorbs_the_bias_so_nothing_is_flagged() {

// 4 anchors = 6 edges = 6 DOF: the solve satisfies every edge, so the +0.6 bias is absorbed

// into a warped frame with ZERO post-solve residual — undetectable by graph residual, BY DESIGN.

val truth4 = mapOf(

a1 to Vector3D(0.0, 0.0, 0.0), a2 to Vector3D(2.0, 0.0, 0.0),

a3 to Vector3D(0.0, 2.0, 0.0), a4 to Vector3D(1.0, 1.0, 0.6),

)

val biased = distances(truth4, a2 to a3, 0.6)

val solved = AnchorConstellationSolver.refine(truth4, biased, rootId = a1)

// The frame really did warp (A3 pushed off truth) — the bias went somewhere …

val a3err = solved.positions.getValue(a3).distanceTo(truth4.getValue(a3))

assertTrue(a3err > 0.15, "rigid graph should absorb the bias by warping (A3 err=${a3err} m)")

// … but NOT into any edge residual, so detection correctly finds nothing.

val flagged = InterAnchorLinkReport.biasedEdges(solved.positions, biased)

assertTrue(flagged.isEmpty(),

"an exactly-rigid graph must flag NO biased edge (residual is absorbed); got $flagged")

}

@Test

fun redundant_five_anchor_graph_flags_exactly_the_biased_edge() {

val biased = distances(truth5, a2 to a3, 0.6)

val solved = AnchorConstellationSolver.refine(truth5, biased, rootId = a1)

val flagged = InterAnchorLinkReport.biasedEdges(solved.positions, biased)

val residuals = biased.entries.joinToString { (p, d) ->

"${p.first.value}-${p.second.value}=" +

"${abs(solved.positions.getValue(p.first).distanceTo(solved.positions.getValue(p.second)) - d)}"

}

assertTrue(canon(a2, a3) in flagged,

"the redundant solve must flag the biased A2-A3 edge. residuals=[$residuals] flagged=$flagged")

assertEquals(setOf(canon(a2, a3)), flagged.keys,

"ONLY the biased edge should be flagged. residuals=[$residuals] flagged=$flagged")

}

// ── Part B: DOWN-WEIGHT (the required detect → down-weight → recover loop) ──────────────────────

@Test

fun detect_then_downweight_recovers_the_biased_anchor() {

val biased = distances(truth5, a2 to a3, 0.6)

// 1) UN-weighted solve: an over-determined graph still spreads a single bias into the frame.

val unweighted = AnchorConstellationSolver.refine(truth5, biased, rootId = a1)

val errUnweighted = unweighted.positions.getValue(a3).distanceTo(truth5.getValue(a3))

assertTrue(errUnweighted > 0.10, "the un-weighted solve spreads the bias into A3 (err=${errUnweighted} m)")

// 2) DETECT the biased edge from that solve (no ground truth used — pure graph residual).

val flagged = InterAnchorLinkReport.biasedEdges(unweighted.positions, biased)

assertTrue(flagged.isNotEmpty(), "detection must find the biased edge to drive the down-weight")

// 3) DOWN-WEIGHT the flagged edges and re-solve → the good edges recover A3.

val weights = flagged.keys.associateWith { 0.01 }

val weighted = AnchorConstellationSolver.refine(truth5, biased, rootId = a1, weights = weights)

val errWeighted = weighted.positions.getValue(a3).distanceTo(truth5.getValue(a3))

assertTrue(errWeighted < 0.05,

"detect + down-weight must recover A3 (err ${errWeighted} m vs un-weighted ${errUnweighted} m)")

}

// ── Part A wiring: the flag surfaces on InterAnchorLinkReport ───────────────────────────────────

@Test

fun build_surfaces_the_biased_flag_and_count_and_is_a_noop_when_empty() {

val ids = truth5.keys.toList()

val samples = buildMap {

val d = distances(truth5, a2 to a3, 0.6)

for ((pair, dm) in d) put(canon(pair.first, pair.second), InterAnchorLinkSample(dm, null, 0L, 5))

}

// Empty biasedPairs ⇒ unchanged report (no biased flags).

val plain = InterAnchorLinkReport.build(ids, samples, nowMicros = 0L)

assertEquals(0, plain.biased)

assertFalse(plain.hasBiasedLink)

assertTrue(plain.links.none { it.biased })

// With the flag set, exactly that link is marked and counted.

val flagged = InterAnchorLinkReport.build(ids, samples, nowMicros = 0L, biasedPairs = setOf(canon(a2, a3)))

assertEquals(1, flagged.biased)

assertTrue(flagged.hasBiasedLink)

val link = flagged.links.first { it.a == canon(a2, a3).first && it.b == canon(a2, a3).second }

assertTrue(link.biased, "the A2-A3 link must carry the biased flag")

assertEquals(1, flagged.links.count { it.biased })

}

// ── End-to-end through the real engine (report flag + refine down-weight wiring) ───────────────

private fun feedBiased(h: MofeTestHarness) {

val d = distances(truth5, a2 to a3, 0.6)

for ((pair, dm) in d) repeat(4) { h.engine.processInterAnchorRanging(pair.first, pair.second, dm) }

}

private fun refs(h: MofeTestHarness): Map<DeviceId, Vector3D> =

h.frameManager.getAllReferencePoints().associate { it.id to it.position }

private fun maxPairwiseError(got: Map<DeviceId, Vector3D>): Double {

val ids = truth5.keys.toList()

var maxErr = 0.0

for (i in ids.indices) for (j in i + 1 until ids.size) {

val g = got.getValue(ids[i]).distanceTo(got.getValue(ids[j]))

val w = truth5.getValue(ids[i]).distanceTo(truth5.getValue(ids[j]))

maxErr = maxOf(maxErr, abs(g - w))

}

return maxErr

}

private fun seedAtTruth(h: MofeTestHarness) {

h.engine.initializeAsRoot(a1)

h.engine.registerMeshNode(a2, truth5.getValue(a2))

h.engine.registerMeshNode(a3, truth5.getValue(a3))

h.engine.registerMeshNode(a4, truth5.getValue(a4))

h.engine.registerMeshNode(a5, truth5.getValue(a5))

}

@Test

fun engine_report_flags_the_biased_link_on_a_redundant_constellation() {

val h = MofeTestHarness(MofeConfig()).build()

seedAtTruth(h)

feedBiased(h)

val report = h.engine.interAnchorLinkReport()

assertTrue(report.hasBiasedLink, "the engine report must flag the stable cross-room bias")

val biasedLinks = report.links.filter { it.biased }.map { canon(it.a, it.b) }

assertTrue(canon(a2, a3) in biasedLinks, "the flagged link must be A2-A3; got $biasedLinks")

}

@Test

fun engine_downweight_prevents_the_bias_from_warping_the_frame() {

// Down-weight ON (default): refineAnchorConstellation holds the frame near truth.

val on = MofeTestHarness(MofeConfig()).build()

seedAtTruth(on); feedBiased(on)

on.engine.refineAnchorConstellation()

val errOn = maxPairwiseError(refs(on))

// Down-weight OFF: the same biased edge warps the constellation on refine.

val off = MofeTestHarness(MofeConfig(anchorConstellationDownWeightBiasedEnabled = false)).build()

seedAtTruth(off); feedBiased(off)

off.engine.refineAnchorConstellation()

val errOff = maxPairwiseError(refs(off))

assertTrue(errOff > 0.10, "without down-weighting the bias must warp the frame (maxErr=${errOff} m)")

assertTrue(errOn < errOff * 0.6,

"down-weighting must keep the frame closer to truth (on=${errOn} m vs off=${errOff} m)")

}

}