Aether

@Test

fun gyroBiasAboveRestThresholdReadsMovingSoZaruCannotAnchorYaw() {

// Rest-gate BOUNDARY — the on-device diagnostic tree's "motionMode ≠ STATIONARY"

// branch. ZARU only fires while the RAW-gyro rest gate reads the device as still. A

// standing gyro bias that EXCEEDS zuptGyroThreshold makes the raw rate itself look

// like motion, so the device reads MOVING, ZARU never engages, and the bias integrates

// into an ever-growing heading — the regime where only an absolute AoA reference can

// recover yaw. Complements zaruStopsYawDriftWhenStationaryFlat (a bias BELOW the gate,

// which ZARU arrests); together they bracket the gate. Bias is derived from the config

// so the test tracks the threshold rather than hard-coding it.

val cfg = EkfConfig() // zaruEnabled = true (default)

val biasZ = cfg.zuptGyroThreshold + 0.08 // rad/s, safely ABOVE the rest gate

val e = ImuFusionEKF(TARGET_1, cfg)

e.update(PositionSolution(TARGET_1, Timestamp(0), Vector3D(0.0, 0.0, 0.0),

Matrix.diagonal(0.001, 0.001, 0.001), 2.0, true))

var t = 0L

var facingAt150: Vector3D? = null

var facingLast = Vector3D(0.0, 1.0, 0.0)

repeat(250) { i ->

t += 10_000 // 100 Hz

e.predict(ImuSample(Timestamp(t), Vector3D(0.0, 0.0, 9.81), Vector3D(0.0, 0.0, biasZ)))

if (i == 149) facingAt150 = facingOf(e)

if (i >= 149) facingLast = facingOf(e)

}

// Raw rate exceeds the gate → never classified at rest → ZARU disarmed.

assertEquals(MotionMode.MOVING, e.motionMode,

"a gyro bias above zuptGyroThreshold must read MOVING, holding ZARU off")

// With ZARU disarmed the standing bias integrates → the heading keeps walking.

val drift = angle(facingAt150!!, facingLast)

assertTrue(drift > 0.03,

"with the rest gate holding ZARU off, a standing gyro bias must walk the heading (got $drift rad)")

}