User Story #26 » 0003-feat-engine-antenna-delay-calibration-for-inter-anch.patch
| common/src/commonMain/kotlin/com/aether/mofe/engine/AntennaDelayCalibrator.kt | ||
|---|---|---|
|
package com.aether.mofe.engine
|
||
|
import com.aether.mofe.model.DeviceId
|
||
|
import com.aether.mofe.model.Vector3D
|
||
|
import kotlin.math.abs
|
||
|
import kotlin.math.sqrt
|
||
|
/**
|
||
|
* Per-device antenna-delay calibration for inter-anchor ranging.
|
||
|
*
|
||
|
* An uncalibrated UWB radio adds a fixed per-device offset to every range it takes (its "antenna
|
||
|
* delay"), so a measured inter-anchor distance is
|
||
|
*
|
||
|
* d_measured(i, j) ≈ ‖p_i − p_j‖ + b_i + b_j
|
||
|
*
|
||
|
* with b_i the device's delay in metres. A per-device bias is nearly invisible to the constellation
|
||
|
* solver — it inflates ALL of a device's edges together, which the distance fit can largely absorb
|
||
|
* into position — yet it distorts the solved frame: field data (a surveyed 2.3 m × 3.7 m room) shows
|
||
|
* one low anchor reading +0.39 m long on every link, which pushes it ~0.4 m out of place and skews
|
||
|
* the heading of every client that AoA-anchors to it.
|
||
|
*
|
||
|
* Given the user-surveyed layout ([MeshGroundTruth]) the true pairwise distances are known, so the
|
||
|
* per-edge error e_ij = d_measured − ‖p_i − p_j‖ = b_i + b_j is a LINEAR system in the delays. This
|
||
|
* solves it by least squares so the engine can subtract b_i + b_j from every measured range and hand
|
||
|
* the constellation solver bias-corrected distances.
|
||
|
*
|
||
|
* Solvability: the b_i + b_j system is uniquely determined when the ranged graph is connected AND
|
||
|
* non-bipartite — i.e. it contains an odd cycle, which any triangle of three mutually-ranging anchors
|
||
|
* provides. A purely bipartite graph leaves a 1-DOF gauge in the ABSOLUTE delays, but the per-edge
|
||
|
* correction b_i + b_j is gauge-invariant on every MEASURED edge, so the ranges still correct
|
||
|
* correctly; only the reported absolute offsets would be ambiguous.
|
||
|
*/
|
||
|
object AntennaDelayCalibrator {
|
||
|
data class Result(
|
||
|
/** Per-device antenna-delay offset b_i (metres) to SUBTRACT from each of its ranges. */
|
||
|
val delays: Map<DeviceId, Double>,
|
||
|
/** RMS of the fit residual e_ij − (b_i + b_j) over the used edges (metres). Low ⇒ the errors
|
||
|
* really are a per-device bias the correction removes; high ⇒ NLOS/per-link error it can't. */
|
||
|
val residualRmsMeters: Double,
|
||
|
val edgesUsed: Int,
|
||
|
/** True when enough edges over ≥2 surveyed devices produced a delay estimate. */
|
||
|
val ok: Boolean,
|
||
|
) {
|
||
|
companion object { val EMPTY = Result(emptyMap(), 0.0, 0, false) }
|
||
|
}
|
||
|
/**
|
||
|
* @param truth surveyed positions (any origin/axes — only pairwise distances are used).
|
||
|
* @param measured measured inter-anchor distances keyed by an unordered device pair.
|
||
|
* @param minEdges require at least this many usable edges, else an empty (no-op) result.
|
||
|
*/
|
||
|
fun solve(
|
||
|
truth: Map<DeviceId, Vector3D>,
|
||
|
measured: Map<Pair<DeviceId, DeviceId>, Double>,
|
||
|
minEdges: Int = 3,
|
||
|
): Result {
|
||
|
// Usable edge = both endpoints surveyed, distinct, finite error.
|
||
|
val ei = ArrayList<Int>(); val ej = ArrayList<Int>(); val ee = ArrayList<Double>()
|
||
|
val order = ArrayList<DeviceId>(); val idx = HashMap<String, Int>()
|
||
|
fun index(id: DeviceId): Int = idx.getOrPut(id.value) { order.add(id); order.size - 1 }
|
||
|
for ((pair, d) in measured) {
|
||
|
if (pair.first == pair.second) continue
|
||
|
val pi = truth[pair.first] ?: continue
|
||
|
val pj = truth[pair.second] ?: continue
|
||
|
val err = d - pi.distanceTo(pj)
|
||
|
if (!err.isFinite()) continue
|
||
|
ei.add(index(pair.first)); ej.add(index(pair.second)); ee.add(err)
|
||
|
}
|
||
|
val m = ee.size
|
||
|
val n = order.size
|
||
|
if (m < minEdges || n < 2) return Result.EMPTY
|
||
|
// Normal equations AᵀA·b = Aᵀe for e_k = b_{i_k} + b_{j_k}. AᵀA = deg·I + adjacency.
|
||
|
val ata = Array(n) { DoubleArray(n) }
|
||
|
val ate = DoubleArray(n)
|
||
|
for (k in 0 until m) {
|
||
|
val a = ei[k]; val b = ej[k]
|
||
|
ata[a][a] += 1.0; ata[b][b] += 1.0
|
||
|
ata[a][b] += 1.0; ata[b][a] += 1.0
|
||
|
ate[a] += ee[k]; ate[b] += ee[k]
|
||
|
}
|
||
|
// Tiny ridge: negligible when a triangle makes the system full rank; picks the min-norm
|
||
|
// solution (per-edge corrections still exact) in the degenerate bipartite case.
|
||
|
for (k in 0 until n) ata[k][k] += 1e-9
|
||
|
val b = solveLinear(ata, ate) ?: return Result.EMPTY
|
||
|
var ss = 0.0
|
||
|
for (k in 0 until m) {
|
||
|
val r = ee[k] - (b[ei[k]] + b[ej[k]])
|
||
|
ss += r * r
|
||
|
}
|
||
|
val delays = order.withIndex().associate { (k, id) -> id to b[k] }
|
||
|
return Result(delays, sqrt(ss / m), m, true)
|
||
|
}
|
||
|
/** Gaussian elimination with partial pivoting; null if singular. */
|
||
|
private fun solveLinear(a: Array<DoubleArray>, rhs: DoubleArray): DoubleArray? {
|
||
|
val n = rhs.size
|
||
|
val mm = Array(n) { i -> DoubleArray(n + 1).also { row ->
|
||
|
for (j in 0 until n) row[j] = a[i][j]; row[n] = rhs[i] } }
|
||
|
for (col in 0 until n) {
|
||
|
var piv = col
|
||
|
for (r in col + 1 until n) if (abs(mm[r][col]) > abs(mm[piv][col])) piv = r
|
||
|
val t = mm[col]; mm[col] = mm[piv]; mm[piv] = t
|
||
|
if (abs(mm[col][col]) < 1e-15) return null
|
||
|
for (r in col + 1 until n) {
|
||
|
val f = mm[r][col] / mm[col][col]
|
||
|
for (c in col..n) mm[r][c] -= f * mm[col][c]
|
||
|
}
|
||
|
}
|
||
|
val x = DoubleArray(n)
|
||
|
for (i in n - 1 downTo 0) {
|
||
|
var s = mm[i][n]
|
||
|
for (j in i + 1 until n) s -= mm[i][j] * x[j]
|
||
|
x[i] = s / mm[i][i]
|
||
|
}
|
||
|
return x
|
||
|
}
|
||
|
}
|
||
| common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt | ||
|---|---|---|
|
/** Maintenance-tick counter that paces constellation refinement. */
|
||
|
private var maintenanceTicks = 0
|
||
|
/** Per-device antenna-delay offsets (metres) solved by [calibrateAntennaDelays] and SUBTRACTED
|
||
|
* from every measured inter-anchor range before the constellation solve. A per-device range
|
||
|
* bias (an uncalibrated UWB antenna — field-observed at +0.39 m on one anchor) is nearly
|
||
|
* invisible to the distance solver yet pushes that anchor ~0.4 m out of place; removing
|
||
|
* b_a + b_b un-distorts the frame. Empty (identity) until the surveyed layout is entered. */
|
||
|
private var antennaDelays: Map<DeviceId, Double> = emptyMap()
|
||
|
private var _antennaDelayCalibration: AntennaDelayCalibrator.Result? = null
|
||
|
/** Single mesh-wide event detector — predicates apply to every target. */
|
||
|
private val eventDetector = EventDetector()
|
||
| ... | ... | |
|
// the static anchor constellation we want to refine.
|
||
|
val anchors = frameManager.getAllReferencePoints().associate { it.id to it.position }
|
||
|
if (anchors.size < config.minAnchorsForFusion) return 0
|
||
|
// Keep only edges whose BOTH endpoints are current reference anchors.
|
||
|
val edges = interAnchorDistances.filterKeys { it.first in anchors && it.second in anchors }
|
||
|
// Keep only edges whose BOTH endpoints are current reference anchors, with per-device
|
||
|
// antenna delays removed (see [correctedInterAnchorDistances]).
|
||
|
val edges = correctedInterAnchorDistances().filterKeys { it.first in anchors && it.second in anchors }
|
||
|
if (edges.size < config.anchorConstellationMinEdges) return 0
|
||
|
val result = AnchorConstellationSolver.refine(
|
||
| ... | ... | |
|
// may never place references (unusable phone AoA, or an ANCHOR role that never reached
|
||
|
// the band roster), leaving the engine stuck below canSolvePositions with the whole
|
||
|
// pipeline dead at Stage 1. A distance-only bootstrap needs neither bearings nor roles.
|
||
|
// Solve per-device antenna delays from the surveyed layout once enough ranges exist, so the
|
||
|
// bootstrap below consumes bias-corrected inter-anchor distances.
|
||
|
maybeCalibrateAntennaDelays()
|
||
|
bootstrapReferenceConstellation()
|
||
|
// Rigid-body anchor refinement on its own (slower) cadence.
|
||
|
maintenanceTicks++
|
||
| ... | ... | |
|
pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "edges<min", ids.size, edgeCount, 0, emptyList()); return 0
|
||
|
}
|
||
|
// Antenna-delay corrected distances: remove each device's per-device range bias before
|
||
|
// solving, so an uncalibrated antenna can't distort the frame. Identity until calibrated.
|
||
|
val corrected = correctedInterAnchorDistances()
|
||
|
// Closed-form K4 cascade first. It needs EVERY inter-anchor edge (the 4th anchor is
|
||
|
// trilaterated against the other three), so one permanently-missing link — e.g. a TV
|
||
|
// between two anchors that blocks their UWB range (NLOS never ranges through it) — makes
|
||
| ... | ... | |
|
// seed: fall back to refining that seed against the edges we DO have (see
|
||
|
// [refineConstellationFromPrior]) so the mesh still crosses quorum.
|
||
|
var viaFallback = false
|
||
|
val solved = AnchorMeshBootstrap.bootstrap(ids, interAnchorDistances, rootId)
|
||
|
?: refineConstellationFromPrior(ids, rootId)?.also { viaFallback = true }
|
||
|
val solved = AnchorMeshBootstrap.bootstrap(ids, corrected, rootId)
|
||
|
?: refineConstellationFromPrior(ids, rootId, corrected)?.also { viaFallback = true }
|
||
|
?: run { pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "solve-null", ids.size, edgeCount, 0, emptyList()); return 0 }
|
||
|
// ORIENT the distance-solved shape into the mesh +Z=up world frame — the one thing
|
||
| ... | ... | |
|
* well-constrained anchor is corrected and, crucially, the frame reaches quorum, which the
|
||
|
* missing edge otherwise blocks entirely.
|
||
|
*/
|
||
|
private fun refineConstellationFromPrior(ids: Set<DeviceId>, rootId: DeviceId): Map<DeviceId, Vector3D>? {
|
||
|
private fun refineConstellationFromPrior(
|
||
|
ids: Set<DeviceId>, rootId: DeviceId, distances: Map<Pair<DeviceId, DeviceId>, Double>,
|
||
|
): Map<DeviceId, Vector3D>? {
|
||
|
val known = frameManager.getAllReferencePoints().associate { it.id to it.position }
|
||
|
val prior = LinkedHashMap<DeviceId, Vector3D>()
|
||
|
for (id in ids) prior[id] = if (id == rootId) Vector3D.ZERO else (known[id] ?: return null)
|
||
|
if (prior.size < config.minAnchorsForFusion) return null
|
||
|
val refined = AnchorConstellationSolver.refine(prior, interAnchorDistances, rootId)
|
||
|
val refined = AnchorConstellationSolver.refine(prior, distances, rootId)
|
||
|
// Reject a fit that cannot satisfy the edges we have — inconsistent/garbage ranging.
|
||
|
val maxSeedResidualMeters = 0.5
|
||
|
return if (refined.converged && refined.rmsResidualMeters <= maxSeedResidualMeters) refined.positions else null
|
||
|
}
|
||
|
// ═════════════════════════════════════════════════════════════════════
|
||
|
// Antenna-delay calibration (per-device inter-anchor ranging bias)
|
||
|
// ═════════════════════════════════════════════════════════════════════
|
||
|
/**
|
||
|
* Solve per-device antenna delays from the user-surveyed layout ([MeshGroundTruth]) and the
|
||
|
* accumulated inter-anchor ranges, then apply them to every subsequent constellation solve.
|
||
|
* An uncalibrated UWB antenna adds a fixed per-device offset to every range it takes; that
|
||
|
* per-device bias barely moves the distance-fit residual yet pushes the device ~0.4 m out of
|
||
|
* position and skews the heading of every client that AoA-anchors to it (field-observed at
|
||
|
* +0.39 m). With the survey the true distances are known, so err_ij = measured − true = b_i + b_j
|
||
|
* is a linear system in the delays. No-op (identity) until the survey is entered and enough
|
||
|
* inter-anchor edges exist. Idempotent — always solves from the RAW ranges, so re-running as
|
||
|
* more edges arrive only sharpens it. Confined to the engine thread.
|
||
|
*/
|
||
|
fun calibrateAntennaDelays(): AntennaDelayCalibrator.Result {
|
||
|
if (MeshGroundTruth.isEmpty()) return AntennaDelayCalibrator.Result.EMPTY
|
||
|
val truth = MeshGroundTruth.positions().mapKeys { DeviceId(it.key) }
|
||
|
val result = AntennaDelayCalibrator.solve(truth, interAnchorDistances, config.anchorConstellationMinEdges)
|
||
|
if (result.ok) { antennaDelays = result.delays; _antennaDelayCalibration = result }
|
||
|
return result
|
||
|
}
|
||
|
/** Latest antenna-delay calibration (per-device delays + fit residual), or null if never run.
|
||
|
* The residual is the un-removable (NLOS/per-link) part of the ranging error. Diagnostic/UI. */
|
||
|
fun antennaDelayCalibration(): AntennaDelayCalibrator.Result? = _antennaDelayCalibration
|
||
|
/** Solved per-device antenna-delay offsets (metres); empty until [calibrateAntennaDelays]. */
|
||
|
fun antennaDelays(): Map<DeviceId, Double> = antennaDelays
|
||
|
/**
|
||
|
* Inter-anchor distances with per-device antenna delays removed (raw − b_a − b_b). Identity
|
||
|
* until [calibrateAntennaDelays] has solved delays. The constellation solve consumes THESE so a
|
||
|
* per-device antenna bias does not distort the frame; drift detection still sees the raw ranges
|
||
|
* (it tracks CHANGE, not absolute bias, so a constant delay is harmless there).
|
||
|
*/
|
||
|
private fun correctedInterAnchorDistances(): Map<Pair<DeviceId, DeviceId>, Double> {
|
||
|
if (antennaDelays.isEmpty()) return interAnchorDistances
|
||
|
return interAnchorDistances.mapValues { (pair, d) ->
|
||
|
d - (antennaDelays[pair.first] ?: 0.0) - (antennaDelays[pair.second] ?: 0.0)
|
||
|
}
|
||
|
}
|
||
|
/** Auto-calibrate once the surveyed layout is present and enough edges have accumulated; retries
|
||
|
* each maintenance tick until it succeeds, then holds (re-run [calibrateAntennaDelays] to redo). */
|
||
|
private fun maybeCalibrateAntennaDelays() {
|
||
|
if (antennaDelays.isNotEmpty()) return
|
||
|
if (MeshGroundTruth.isEmpty()) return
|
||
|
if (interAnchorDistances.size < config.anchorConstellationMinEdges) return
|
||
|
calibrateAntennaDelays()
|
||
|
}
|
||
|
// ═════════════════════════════════════════════════════════════════════
|
||
|
// Predicate registration
|
||
|
// ═════════════════════════════════════════════════════════════════════
|
||
| common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt | ||
|---|---|---|
|
assertTrue(maxErr < 0.03, "distance-solved constellation matches truth; maxErr=${maxErr}m")
|
||
|
}
|
||
|
@Test
|
||
|
fun antenna_delay_calibration_corrects_a_biased_constellation() {
|
||
|
// A per-device antenna delay (A4 the worst — field-observed +0.39 m) inflates every range
|
||
|
// that device takes, distorting the distance-solved frame almost invisibly (a per-device
|
||
|
// bias barely moves the fit residual). Given the surveyed layout, calibrateAntennaDelays()
|
||
|
// solves the delays and the constellation solve then consumes bias-corrected distances →
|
||
|
// the frame matches truth. Uncorrected, A4's edges are ~0.5 m long and the frame is skewed.
|
||
|
val delays = mapOf(a1 to 0.14, a2 to -0.01, a3 to -0.03, a4 to 0.39)
|
||
|
MeshGroundTruth.clear()
|
||
|
try {
|
||
|
for ((id, p) in truth) MeshGroundTruth.set(id.value, MeshGroundTruth.Pose(p))
|
||
|
val h = MofeTestHarness(MofeConfig()).build()
|
||
|
h.engine.initializeAsRoot(a1)
|
||
|
// AoA prior (garbage z), as on-device, so a frame exists to be corrected.
|
||
|
h.engine.registerMeshNode(a2, Vector3D(2.2, 0.0, -1.3))
|
||
|
h.engine.registerMeshNode(a3, Vector3D(0.0, 3.6, -1.1))
|
||
|
h.engine.registerMeshNode(a4, Vector3D(2.2, 2.9, -1.5))
|
||
|
// Feed BIASED inter-anchor ranges: true + b_i + b_j on all six edges.
|
||
|
val ids = listOf(a1, a2, a3, a4)
|
||
|
repeat(6) {
|
||
|
for (i in ids.indices) for (j in i + 1 until ids.size) {
|
||
|
val d = truth.getValue(ids[i]).distanceTo(truth.getValue(ids[j])) +
|
||
|
delays.getValue(ids[i]) + delays.getValue(ids[j])
|
||
|
h.engine.processInterAnchorRanging(ids[i], ids[j], d)
|
||
|
}
|
||
|
}
|
||
|
// Calibrate from the survey → recovers the per-device delays.
|
||
|
val cal = h.engine.calibrateAntennaDelays()
|
||
|
assertTrue(cal.ok, "calibration must solve")
|
||
|
assertTrue(cal.residualRmsMeters < 0.02, "a clean per-device bias fits tightly; got ${cal.residualRmsMeters}m")
|
||
|
for (id in ids) assertTrue(abs(cal.delays.getValue(id) - delays.getValue(id)) < 0.03,
|
||
|
"delay for ${id.value} recovered (~${delays.getValue(id)}); got ${cal.delays[id]}")
|
||
|
// The constellation solve now uses corrected distances → matches truth (pairwise).
|
||
|
val placed = h.engine.bootstrapReferenceConstellation()
|
||
|
assertTrue(placed > 0, "frame must (re)establish on corrected distances; placed=$placed")
|
||
|
val refs = h.frameManager.getAllReferencePoints().associate { it.id to it.position }
|
||
|
var maxErr = 0.0
|
||
|
for (i in ids.indices) for (j in i + 1 until ids.size) {
|
||
|
val got = refs.getValue(ids[i]).distanceTo(refs.getValue(ids[j]))
|
||
|
val want = truth.getValue(ids[i]).distanceTo(truth.getValue(ids[j]))
|
||
|
maxErr = maxOf(maxErr, abs(got - want))
|
||
|
}
|
||
|
assertTrue(maxErr < 0.05, "corrected constellation must match true distances; maxErr=${maxErr}m")
|
||
|
} finally {
|
||
|
MeshGroundTruth.clear()
|
||
|
}
|
||
|
}
|
||
|
@Test
|
||
|
fun gauge_is_deterministic_regardless_of_input_order() {
|
||
|
// The solve must pick its axes (a1→+X, a2→XY, a3→+Z) from a STABLE id order, so the
|
||
| common/src/commonTest/kotlin/com/aether/mofe/engine/AntennaDelayCalibratorTest.kt | ||
|---|---|---|
|
package com.aether.mofe.engine
|
||
|
import com.aether.mofe.model.DeviceId
|
||
|
import com.aether.mofe.model.Vector3D
|
||
|
import kotlin.math.abs
|
||
|
import kotlin.test.Test
|
||
|
import kotlin.test.assertTrue
|
||
|
/**
|
||
|
* Per-device antenna-delay calibration. Field data (a surveyed 2.3 m × 3.7 m room) showed the
|
||
|
* inter-anchor ranges biased long — up to +0.54 m on the low anchor's links — which a per-device
|
||
|
* additive model (antenna delay) fit to ~9 cm RMS, dominated by that one anchor (+0.39 m). Left in,
|
||
|
* it pushes the anchor ~0.4 m out of place and skews every client's heading. These tests exercise
|
||
|
* the solver that recovers those per-device offsets from the surveyed layout.
|
||
|
*/
|
||
|
class AntennaDelayCalibratorTest {
|
||
|
private val a1 = DeviceId("A1"); private val a2 = DeviceId("A2")
|
||
|
private val a3 = DeviceId("A3"); private val a4 = DeviceId("A4")
|
||
|
// Surveyed layout (xy rel to A1, z rel; A4 the low corner) — the real field geometry.
|
||
|
private val truth = mapOf(
|
||
|
a1 to Vector3D(0.0, 0.0, 0.0),
|
||
|
a2 to Vector3D(2.286, 0.0, 0.1525),
|
||
|
a3 to Vector3D(0.0, 3.6957, 0.216),
|
||
|
a4 to Vector3D(2.286, 2.921, -0.3428),
|
||
|
)
|
||
|
private val ids = listOf(a1, a2, a3, a4)
|
||
|
private fun measuredWith(
|
||
|
delays: Map<DeviceId, Double>,
|
||
|
nlos: Map<Pair<DeviceId, DeviceId>, Double> = emptyMap(),
|
||
|
): Map<Pair<DeviceId, DeviceId>, Double> {
|
||
|
val m = HashMap<Pair<DeviceId, DeviceId>, Double>()
|
||
|
for (i in ids.indices) for (j in i + 1 until ids.size) {
|
||
|
val a = ids[i]; val b = ids[j]
|
||
|
var d = truth.getValue(a).distanceTo(truth.getValue(b)) + delays.getValue(a) + delays.getValue(b)
|
||
|
d += nlos[a to b] ?: 0.0
|
||
|
m[a to b] = d
|
||
|
}
|
||
|
return m
|
||
|
}
|
||
|
@Test
|
||
|
fun recovers_injected_per_device_delays_exactly() {
|
||
|
val delays = mapOf(a1 to 0.142, a2 to -0.014, a3 to -0.082, a4 to 0.393)
|
||
|
val r = AntennaDelayCalibrator.solve(truth, measuredWith(delays))
|
||
|
assertTrue(r.ok, "must solve")
|
||
|
assertTrue(r.residualRmsMeters < 1e-4, "a clean per-device bias fits exactly; got ${r.residualRmsMeters}")
|
||
|
for (id in ids) assertTrue(abs(r.delays.getValue(id) - delays.getValue(id)) < 1e-3,
|
||
|
"recover ${id.value}: want ${delays.getValue(id)} got ${r.delays[id]}")
|
||
|
}
|
||
|
@Test
|
||
|
fun absorbs_per_device_bias_and_leaves_nlos_in_the_residual() {
|
||
|
// Per-device delays + a single-link NLOS bump: the delays soak up the per-device part; the
|
||
|
// NLOS survives as residual — the signal the calibration UI should surface as a bad link.
|
||
|
val delays = mapOf(a1 to 0.10, a2 to 0.0, a3 to 0.0, a4 to 0.40)
|
||
|
val nlos = mapOf((a1 to a3) to 0.15)
|
||
|
val r = AntennaDelayCalibrator.solve(truth, measuredWith(delays, nlos))
|
||
|
assertTrue(r.ok && r.residualRmsMeters > 0.02,
|
||
|
"an NLOS bump on one link must leave a residual; got ${r.residualRmsMeters}")
|
||
|
// A clean edge (A1-A2) still corrects back to ~truth.
|
||
|
val meas = measuredWith(delays, nlos)
|
||
|
val corr = meas.getValue(a1 to a2) - r.delays.getValue(a1) - r.delays.getValue(a2)
|
||
|
assertTrue(abs(corr - truth.getValue(a1).distanceTo(truth.getValue(a2))) < 0.08,
|
||
|
"a clean edge must correct to ~truth; got $corr")
|
||
|
}
|
||
|
@Test
|
||
|
fun declines_when_too_few_edges() {
|
||
|
val r = AntennaDelayCalibrator.solve(truth, mapOf((a1 to a2) to 2.3), minEdges = 3)
|
||
|
assertTrue(!r.ok, "one edge cannot calibrate the constellation")
|
||
|
}
|
||
|
}
|
||