diff options
3 files changed, 278 insertions, 104 deletions
diff --git a/android-app/app/src/main/kotlin/org/terst/nav/track/TackDetector.kt b/android-app/app/src/main/kotlin/org/terst/nav/track/TackDetector.kt index b0d256b..763e693 100644 --- a/android-app/app/src/main/kotlin/org/terst/nav/track/TackDetector.kt +++ b/android-app/app/src/main/kotlin/org/terst/nav/track/TackDetector.kt @@ -1,50 +1,130 @@ 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 in [MIN_DELTA, MAX_DELTA]. + * + * 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 - private const val MIN_DELTA = 60.0 - private const val MAX_DELTA = 140.0 - private const val STABILITY_MAX = 30.0 - private const val MIN_GAP_MS = 45_000L // 45 s minimum between tacks - private const val START_SKIP_MS = 120_000L // skip first 2 min (GPS cold-start noise) + 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 = 60.0 // ° — minimum heading change to count as tack/jibe + private const val MAX_DELTA = 160.0 // ° — maximum heading change (beyond = U-turn, not a tack) + 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 lastTackMs: Long? = null + 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.filter { it.timestampMs in beforeStart until beforeEnd } + if (before.size < MIN_PTS) continue - for (i in HALF_WIN until points.size - HALF_WIN) { - if (points[i].timestampMs - points.first().timestampMs < START_SKIP_MS) continue - if (lastTackMs != null && points[i].timestampMs - lastTackMs < MIN_GAP_MS) continue + val afterStart = t + T_MANEUVER / 2 + val afterEnd = afterStart + T_SETTLE + val after = points.filter { it.timestampMs > afterStart && it.timestampMs <= afterEnd } + if (after.size < MIN_PTS) continue - 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 cogBefore = circularMean(before.map { it.cogDeg }) + val spreadBefore = circularMAD(before.map { it.cogDeg }, cogBefore) + if (spreadBefore > STAB_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 || delta > MAX_DELTA) continue + + raw += Candidate(i, t, delta, cogBefore, cogAfter) + } + if (raw.isEmpty()) return emptyList() - if (delta in MIN_DELTA..MAX_DELTA) { - results += TackEvent(i, points[i].lat, points[i].lon, cogBefore, cogAfter) - lastTackMs = points[i].timestampMs + // 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 + + 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) + } + 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() } 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 b0d256b..763e693 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,130 @@ 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 in [MIN_DELTA, MAX_DELTA]. + * + * 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 - private const val MIN_DELTA = 60.0 - private const val MAX_DELTA = 140.0 - private const val STABILITY_MAX = 30.0 - private const val MIN_GAP_MS = 45_000L // 45 s minimum between tacks - private const val START_SKIP_MS = 120_000L // skip first 2 min (GPS cold-start noise) + 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 = 60.0 // ° — minimum heading change to count as tack/jibe + private const val MAX_DELTA = 160.0 // ° — maximum heading change (beyond = U-turn, not a tack) + 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 lastTackMs: Long? = null + 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.filter { it.timestampMs in beforeStart until beforeEnd } + if (before.size < MIN_PTS) continue - for (i in HALF_WIN until points.size - HALF_WIN) { - if (points[i].timestampMs - points.first().timestampMs < START_SKIP_MS) continue - if (lastTackMs != null && points[i].timestampMs - lastTackMs < MIN_GAP_MS) continue + val afterStart = t + T_MANEUVER / 2 + val afterEnd = afterStart + T_SETTLE + val after = points.filter { it.timestampMs > afterStart && it.timestampMs <= afterEnd } + if (after.size < MIN_PTS) continue - 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 cogBefore = circularMean(before.map { it.cogDeg }) + val spreadBefore = circularMAD(before.map { it.cogDeg }, cogBefore) + if (spreadBefore > STAB_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 || delta > MAX_DELTA) continue + + raw += Candidate(i, t, delta, cogBefore, cogAfter) + } + if (raw.isEmpty()) return emptyList() - if (delta in MIN_DELTA..MAX_DELTA) { - results += TackEvent(i, points[i].lat, points[i].lon, cogBefore, cogAfter) - lastTackMs = points[i].timestampMs + // 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 + + 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) + } + 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() } diff --git a/test-runner/src/test/kotlin/org/terst/nav/track/TackDetectorTest.kt b/test-runner/src/test/kotlin/org/terst/nav/track/TackDetectorTest.kt index 540683d..6d946a0 100644 --- a/test-runner/src/test/kotlin/org/terst/nav/track/TackDetectorTest.kt +++ b/test-runner/src/test/kotlin/org/terst/nav/track/TackDetectorTest.kt @@ -5,28 +5,26 @@ import org.junit.Test class TackDetectorTest { - // Build a track that's well past the 2-minute cold-start skip. - // Use 30s spacing: 6 warmup points (3 min) + tack points. - private val STEP_MS = 30_000L // 30 s between points - - private fun warmupPoints(cog: Double, count: Int = 5): List<TrackPoint> = + // 1 Hz GPS (FULL mode). Time-based windows: + // T_SETTLE=30s, T_MANEUVER=30s, START_SKIP=60s. + // Minimum for a candidate to fire: START_SKIP(60) + T_SETTLE(30) + T_MANEUVER/2(15) = 105 s from t0. + // To also have a 30s after-window: apex at ~105s, after-window ends at 105+15+30=150s. + // So a safe warmup leg is 120 pts (t=0..119s), tack apex around t=120s, after-leg ≥ 60 pts. + private val STEP_MS = 1_000L // 1 s between points + + private fun leg(cog: Double, count: Int, startMs: Long, sogKnots: Double = 5.0): List<TrackPoint> = (0 until count).map { i -> - TrackPoint(37.0 + i * 0.005, -122.0, 5.0, cog, 12.0, 0.0, true, i * STEP_MS) + TrackPoint(37.0 + (startMs / 1000 + i) * 0.00001, -122.0, sogKnots, cog, 12.0, 0.0, true, + startMs + i * STEP_MS) } - private fun tackTrack(cogBefore: Double, cogAfter: Double, eachLeg: Int = 5): List<TrackPoint> { - val warmup = warmupPoints(cogBefore) // 5 pts × 30s = 2.5 min, tack at ~2.5+ min - val after = (0 until eachLeg).map { i -> - TrackPoint(37.0 + (warmup.size + i) * 0.005, -122.0, 5.0, cogAfter, 12.0, 0.0, true, - (warmup.size + i) * STEP_MS) - } - return warmup + after - } + // Standard two-leg track: 120 s warmup on cogBefore, 120 s on cogAfter. + // Tack apex candidates fire around t≈120 s, well past START_SKIP=60s. + private fun tackTrack(cogBefore: Double, cogAfter: Double): List<TrackPoint> = + leg(cogBefore, 120, 0L) + leg(cogAfter, 120, 120_000L) @Test fun `straight course yields no tacks`() { - val pts = warmupPoints(90.0) + (5 until 15).map { i -> - TrackPoint(37.0 + i * 0.01, -122.0, 5.0, 90.0, 12.0, 0.0, true, i * STEP_MS) - } + val pts = leg(90.0, 300, 0L) assertTrue(TackDetector.detectTacks(pts).isEmpty()) } @@ -42,51 +40,40 @@ class TackDetectorTest { } @Test fun `detects jibe on broad reach`() { - assertEquals(1, TackDetector.detectTacks(tackTrack(150.0, 250.0)).size) + // 150° → 290°: delta = 140°, within [60°, 160°] + assertEquals(1, TackDetector.detectTacks(tackTrack(150.0, 290.0)).size) } @Test fun `handles 0-360 wrap`() { assertEquals(1, TackDetector.detectTacks(tackTrack(350.0, 80.0)).size) } - @Test fun `gradual course change is not a tack`() { - val pts = (0 until 30).map { i -> - TrackPoint(37.0 + i * 0.01, -122.0, 5.0, i * 5.0, 12.0, 0.0, true, i * STEP_MS) - } - assertTrue(TackDetector.detectTacks(pts).isEmpty()) - } - - @Test fun `small heading change below threshold is ignored`() { + @Test fun `heading change below MIN_DELTA is ignored`() { + // 45° delta — below MIN_DELTA=60° assertTrue(TackDetector.detectTacks(tackTrack(90.0, 135.0)).isEmpty()) } - @Test fun `very large heading change above threshold is ignored`() { - assertTrue(TackDetector.detectTacks(tackTrack(90.0, 250.0)).isEmpty()) + @Test fun `heading change above MAX_DELTA is ignored`() { + // 170° delta — above MAX_DELTA=160° (U-turn, not a tack) + assertTrue(TackDetector.detectTacks(tackTrack(90.0, 260.0)).isEmpty()) } - @Test fun `detects two tacks in sequence`() { - // 5 pts warmup + 5 pts leg2 + 5 pts leg3; gaps of 5*30s=150s > MIN_GAP_MS=45s - val warmup = warmupPoints(330.0) - val leg2 = (5 until 10).map { i -> - TrackPoint(37.0 + i * 0.005, -122.0, 5.0, 70.0, 12.0, 0.0, true, i * STEP_MS) - } - val leg3 = (10 until 15).map { i -> - TrackPoint(37.0 + i * 0.005, -122.0, 5.0, 330.0, 12.0, 0.0, true, i * STEP_MS) - } - assertEquals(2, TackDetector.detectTacks(warmup + leg2 + leg3).size) + @Test fun `detects two tacks in sequence with sufficient gap`() { + // leg1: 120s, leg2: 180s (> MIN_GAP_MS=60s), leg3: 120s + val leg1 = leg(330.0, 120, 0L) + val leg2 = leg(70.0, 180, 120_000L) + val leg3 = leg(330.0, 120, 300_000L) + val tacks = TackDetector.detectTacks(leg1 + leg2 + leg3) + assertEquals(2, tacks.size) } - @Test fun `deduplicates tacks within MIN_GAP_MS`() { - // Both tack candidates at i=5 and i=6 are within 30s of each other — only 1 should register - val warmup = warmupPoints(330.0) - val after = (5 until 15).map { i -> - TrackPoint(37.0 + i * 0.005, -122.0, 5.0, 70.0, 12.0, 0.0, true, i * STEP_MS) - } - val tacks = TackDetector.detectTacks(warmup + after) + @Test fun `deduplicates adjacent candidates into single tack`() { + // Single abrupt tack fires many consecutive raw candidates; must collapse to 1 + val tacks = TackDetector.detectTacks(tackTrack(330.0, 70.0)) assertEquals(1, tacks.size) } - @Test fun `tack event contains correct position`() { + @Test fun `tack event contains plausible position`() { val tacks = TackDetector.detectTacks(tackTrack(330.0, 70.0)) assertEquals(1, tacks.size) assertTrue(tacks[0].lat in 37.0..37.1) @@ -94,20 +81,47 @@ class TackDetectorTest { } @Test fun `too few points returns empty`() { - val pts = (0 until 3).map { i -> - TrackPoint(37.0, -122.0, 5.0, 90.0, 12.0, 0.0, true, i * STEP_MS) + val pts = leg(90.0, 2, 0L) + assertTrue(TackDetector.detectTacks(pts).isEmpty()) + } + + @Test fun `cold start suppresses tack in first 60 seconds`() { + // Tack apex at t=30s — inside START_SKIP_MS=60s → filtered + val pts = leg(330.0, 40, 0L) + leg(70.0, 60, 40_000L) + assertEquals(0, TackDetector.detectTacks(pts).size) + } + + @Test fun `noisy anchor COG does not produce false tack`() { + // Simulate boat at anchor: COG rotates 0..360 uniformly (MAD ≈ 90°, fails STAB_MAX=20°) + val pts = (0 until 300).map { i -> + TrackPoint(37.0, -122.0, 0.1, (i * 37.0) % 360.0, 12.0, 0.0, true, i * STEP_MS) } assertTrue(TackDetector.detectTacks(pts).isEmpty()) } - @Test fun `cold start filter suppresses tacks in first 2 minutes`() { - // Tack happens at t=60s from track start → filtered by START_SKIP_MS=120_000 - val start = 0L - val pts = (0 until 6).map { i -> - TrackPoint(37.0, -122.0, 5.0, 330.0, 12.0, 0.0, true, start + i * 10_000L) - } + (0 until 6).map { i -> - TrackPoint(37.0, -122.0, 5.0, 70.0, 12.0, 0.0, true, start + (6 + i) * 10_000L) + @Test fun `unstable before-window prevents false detection`() { + // Before-window has scattered headings (std dev >> STAB_MAX), after-window is stable + val noisyBefore = (0 until 120).map { i -> + TrackPoint(37.0, -122.0, 3.0, (i * 47.0) % 360.0, 12.0, 0.0, true, i * STEP_MS) } - assertEquals(0, TackDetector.detectTacks(pts).size) + val stableAfter = leg(70.0, 120, 120_000L) + assertTrue(TackDetector.detectTacks(noisyBefore + stableAfter).isEmpty()) + } + + @Test fun `gradual 5-degree-per-second course change is not a tack`() { + // Course changes 5°/s for 60 s = 300° total change but never a sudden tack + val pts = (0 until 300).map { i -> + TrackPoint(37.0 + i * 0.0001, -122.0, 5.0, (i * 5.0) % 360.0, 12.0, 0.0, true, i * STEP_MS) + } + assertTrue(TackDetector.detectTacks(pts).isEmpty()) + } + + @Test fun `two tacks closer than MIN_GAP_MS produces only one event`() { + // leg1=120s, leg2=45s (< MIN_GAP_MS=60s gap to leg3 candidates), leg3=120s + val leg1 = leg(330.0, 120, 0L) + val leg2 = leg(70.0, 45, 120_000L) + val leg3 = leg(330.0, 120, 165_000L) + val tacks = TackDetector.detectTacks(leg1 + leg2 + leg3) + assertEquals(1, tacks.size) } } |