diff options
Diffstat (limited to 'test-runner/src/main/kotlin')
| -rw-r--r-- | test-runner/src/main/kotlin/org/terst/nav/track/TackDetector.kt | 139 |
1 files changed, 115 insertions, 24 deletions
diff --git a/test-runner/src/main/kotlin/org/terst/nav/track/TackDetector.kt b/test-runner/src/main/kotlin/org/terst/nav/track/TackDetector.kt index b392f67..5bf86c3 100644 --- a/test-runner/src/main/kotlin/org/terst/nav/track/TackDetector.kt +++ b/test-runner/src/main/kotlin/org/terst/nav/track/TackDetector.kt @@ -1,50 +1,141 @@ package org.terst.nav.track import kotlin.math.abs +import kotlin.math.atan2 +import kotlin.math.cos +import kotlin.math.sin +/** + * Detects tacks and jibes in a recorded GPS track. + * + * A tack or jibe has three phases: + * [── settled leg A (≥30 s) ──][─ guard (15 s) ─][apex][─ guard (15 s) ─][── settled leg B (≥30 s) ──] + * + * For each GPS fix as candidate apex: collect before/after settle windows (excluding the guard zone), + * require MIN_PTS fixes in each, compute circularMean + circularMAD, reject if MAD > STAB_MAX, + * accept if heading delta ≥ MIN_DELTA (no upper cap — 180° crash-tacks are valid). + * + * De-duplicate: group candidates within MIN_GAP_MS, keep max-delta per group. + * Refine position: within the maneuver zone, find the point with the greatest instantaneous + * heading rate of change as the map pin. + * + * No SOG gate: circularMAD stability check handles noise at any speed. + * A boat at anchor has effectively random COG (MAD ≈ 90°) and is always rejected. + * A real settled tack leg has MAD typically 5–15° and passes. + * Mode: CRUISING — conservative (prefers missing a tack over reporting a phantom). + * Race mode (shorter windows, wider MAD) is a future backlog item. + */ object TackDetector { - private const val HALF_WIN = 2 // look 2 points before and after - private const val MIN_DELTA = 60.0 - private const val MAX_DELTA = 140.0 - private const val SKIP_AFTER = 5 // suppress re-detection for this many indices - private const val STABILITY_MAX = 30.0 // max internal spread of a before/after pair + private const val T_SETTLE = 30_000L // ms — stable heading window required before/after + private const val T_MANEUVER = 30_000L // ms — guard zone around apex (±15 s each side) + private const val STAB_MAX = 20.0 // ° — max circularMAD in settle windows + private const val MIN_DELTA = 75.0 // ° — minimum heading change to count as a maneuver + private const val MIN_GAP_MS = 60_000L // ms — minimum time between accepted events + private const val START_SKIP_MS = 60_000L // ms — skip first 60 s (cold-start noise) + private const val MIN_PTS = 3 // minimum GPS fixes required in each settle window + + private data class Candidate( + val index: Int, + val timestampMs: Long, + val delta: Double, + val cogBefore: Double, + val cogAfter: Double + ) fun detectTacks(points: List<TrackPoint>): List<TackEvent> { - if (points.size < 2 * HALF_WIN + 1) return emptyList() + if (points.size < MIN_PTS) return emptyList() + val t0 = points.first().timestampMs - val results = mutableListOf<TackEvent>() - var nextCandidate = 0 + val raw = mutableListOf<Candidate>() + + for (i in points.indices) { + val t = points[i].timestampMs + if (t - t0 < START_SKIP_MS) continue + + val beforeEnd = t - T_MANEUVER / 2 + val beforeStart = beforeEnd - T_SETTLE + val before = points.subList(lowerBound(points, beforeStart), lowerBound(points, beforeEnd)) + if (before.size < MIN_PTS) continue + + val afterStart = t + T_MANEUVER / 2 + val afterEnd = afterStart + T_SETTLE + val after = points.subList(upperBound(points, afterStart), upperBound(points, afterEnd)) + if (after.size < MIN_PTS) continue - for (i in HALF_WIN until points.size - HALF_WIN) { - if (i < nextCandidate) continue + val cogBefore = circularMean(before.map { it.cogDeg }) + val spreadBefore = circularMAD(before.map { it.cogDeg }, cogBefore) + if (spreadBefore > STAB_MAX) continue - // Require both "before" and "after" pairs to be internally stable; - // if one spans the transition itself, that window is noise. - val spreadBefore = abs(angleDiff(points[i - 2].cogDeg, points[i - 1].cogDeg)) - val spreadAfter = abs(angleDiff(points[i + 1].cogDeg, points[i + 2].cogDeg)) - if (spreadBefore > STABILITY_MAX || spreadAfter > STABILITY_MAX) continue + val cogAfter = circularMean(after.map { it.cogDeg }) + val spreadAfter = circularMAD(after.map { it.cogDeg }, cogAfter) + if (spreadAfter > STAB_MAX) continue - val cogBefore = circularMean(points[i - 2].cogDeg, points[i - 1].cogDeg) - val cogAfter = circularMean(points[i + 1].cogDeg, points[i + 2].cogDeg) val delta = abs(angleDiff(cogBefore, cogAfter)) + if (delta < MIN_DELTA) continue + + raw += Candidate(i, t, delta, cogBefore, cogAfter) + } + if (raw.isEmpty()) return emptyList() + + // De-duplicate: if consecutive raw candidates are within MIN_GAP_MS of each other, they + // belong to the same maneuver. Keep the max-delta candidate per group. + // Compare against the PREVIOUS candidate (adjacent comparison) so a long stream of + // close candidates from one maneuver stays in a single group regardless of total span. + val results = mutableListOf<TackEvent>() + var best: Candidate? = null + var prevMs = Long.MIN_VALUE / 2 - if (delta in MIN_DELTA..MAX_DELTA) { - results += TackEvent(i, points[i].lat, points[i].lon, cogBefore, cogAfter) - nextCandidate = i + SKIP_AFTER + for (c in raw) { + if (best != null && c.timestampMs - prevMs >= MIN_GAP_MS) { + results += buildTackEvent(points, best!!) + best = c + } else { + if (best == null || c.delta > best!!.delta) best = c } + prevMs = c.timestampMs } + best?.let { results += buildTackEvent(points, it) } return results } + private fun buildTackEvent(points: List<TrackPoint>, c: Candidate): TackEvent { + // Refine map pin: find max instantaneous heading rate within maneuver zone + val maneuvRange = (c.timestampMs - T_MANEUVER / 2)..(c.timestampMs + T_MANEUVER / 2) + var bestIdx = c.index + var bestRate = 0.0 + for (i in 1 until points.size) { + if (points[i].timestampMs !in maneuvRange) continue + val rate = abs(angleDiff(points[i - 1].cogDeg, points[i].cogDeg)) + if (rate > bestRate) { bestRate = rate; bestIdx = i } + } + return TackEvent(bestIdx, points[bestIdx].lat, points[bestIdx].lon, c.cogBefore, c.cogAfter) + } + + private fun lowerBound(points: List<TrackPoint>, ms: Long): Int { + var lo = 0; var hi = points.size + while (lo < hi) { val mid = (lo + hi) ushr 1; if (points[mid].timestampMs < ms) lo = mid + 1 else hi = mid } + return lo + } + + private fun upperBound(points: List<TrackPoint>, ms: Long): Int { + var lo = 0; var hi = points.size + while (lo < hi) { val mid = (lo + hi) ushr 1; if (points[mid].timestampMs <= ms) lo = mid + 1 else hi = mid } + return lo + } + internal fun angleDiff(from: Double, to: Double): Double { var diff = to - from - while (diff > 180) diff -= 360 + while (diff > 180) diff -= 360 while (diff < -180) diff += 360 return diff } - private fun circularMean(a: Double, b: Double): Double { - val half = angleDiff(a, b) / 2.0 - return ((a + half) % 360.0 + 360.0) % 360.0 + private fun circularMean(angles: List<Double>): Double { + val sinSum = angles.sumOf { sin(Math.toRadians(it)) } + val cosSum = angles.sumOf { cos(Math.toRadians(it)) } + return ((Math.toDegrees(atan2(sinSum, cosSum)) % 360.0) + 360.0) % 360.0 } + + private fun circularMAD(angles: List<Double>, mean: Double): Double = + angles.map { abs(angleDiff(it, mean)) }.average() } |