diff --git a/qaup-collision/src/main/java/com/qaup/collision/websocket/broadcaster/WebSocketMessageBroadcaster.java b/qaup-collision/src/main/java/com/qaup/collision/websocket/broadcaster/WebSocketMessageBroadcaster.java index c9a92f3..80fc0fe 100644 --- a/qaup-collision/src/main/java/com/qaup/collision/websocket/broadcaster/WebSocketMessageBroadcaster.java +++ b/qaup-collision/src/main/java/com/qaup/collision/websocket/broadcaster/WebSocketMessageBroadcaster.java @@ -44,6 +44,7 @@ public class WebSocketMessageBroadcaster { private static final int REACQUIRE_AFTER_REJECTS = 3; private static final double EARTH_RADIUS_METERS = 6371000.0; + private static final double MIN_DT_SECONDS = 0.05; private final MessageCacheService messageCacheService; private final CollisionWebSocketHandler collisionWebSocketHandler; @@ -53,13 +54,13 @@ public class WebSocketMessageBroadcaster { private final Map positionTrackStates = new ConcurrentHashMap<>(); private final AtomicBoolean positionFlushInProgress = new AtomicBoolean(false); - @Value("${websocket.position.filter.ema-alpha:0.45}") + @Value("${websocket.position.filter.ema-alpha:0.6}") private double positionEmaAlpha; - @Value("${websocket.position.filter.jitter-meter:2.5}") + @Value("${websocket.position.filter.jitter-meter:1.0}") private double jitterMeterThreshold; - @Value("${websocket.position.filter.max-speed-mps-aircraft:150.0}") + @Value("${websocket.position.filter.max-speed-mps-aircraft:120.0}") private double maxSpeedMpsAircraft; @Value("${websocket.position.filter.max-speed-mps-vehicle:35.0}") @@ -68,9 +69,51 @@ public class WebSocketMessageBroadcaster { @Value("${websocket.position.filter.max-speed-mps-unmanned:15.0}") private double maxSpeedMpsUnmanned; - @Value("${websocket.position.filter.max-jump-margin-meter:20.0}") + @Value("${websocket.position.filter.max-jump-margin-meter:15.0}") private double maxJumpMarginMeter; + @Value("${websocket.position.filter.speed-slow-threshold-mps:8.0}") + private double speedSlowThresholdMps; + + @Value("${websocket.position.filter.speed-fast-threshold-mps:35.0}") + private double speedFastThresholdMps; + + @Value("${websocket.position.filter.slow.jitter-meter:0.8}") + private double slowJitterMeter; + + @Value("${websocket.position.filter.slow.ema-alpha:0.75}") + private double slowEmaAlpha; + + @Value("${websocket.position.filter.slow.max-speed-mps:25.0}") + private double slowMaxSpeedMps; + + @Value("${websocket.position.filter.slow.jump-margin-meter:8.0}") + private double slowJumpMarginMeter; + + @Value("${websocket.position.filter.fast.jitter-meter:2.0}") + private double fastJitterMeter; + + @Value("${websocket.position.filter.fast.ema-alpha:0.5}") + private double fastEmaAlpha; + + @Value("${websocket.position.filter.fast.max-speed-mps:70.0}") + private double fastMaxSpeedMps; + + @Value("${websocket.position.filter.fast.jump-margin-meter:15.0}") + private double fastJumpMarginMeter; + + @Value("${websocket.position.filter.airborne.jitter-meter:5.0}") + private double airborneJitterMeter; + + @Value("${websocket.position.filter.airborne.ema-alpha:0.35}") + private double airborneEmaAlpha; + + @Value("${websocket.position.filter.airborne.max-speed-mps:120.0}") + private double airborneMaxSpeedMps; + + @Value("${websocket.position.filter.airborne.jump-margin-meter:25.0}") + private double airborneJumpMarginMeter; + @Value("${websocket.position.filter.state-ttl-ms:120000}") private long positionTrackStateTtlMs; @@ -87,6 +130,20 @@ public class WebSocketMessageBroadcaster { private int rejectedCount; } + private enum MotionProfile { + SLOW_TAXI, + FAST_ROLL, + AIRBORNE + } + + private record AdaptiveFilterParams( + double jitterMeter, + double emaAlpha, + double maxSpeedMps, + double jumpMarginMeter, + MotionProfile profile + ) {} + public WebSocketMessageBroadcaster( MessageCacheService messageCacheService, CollisionWebSocketHandler collisionWebSocketHandler, @@ -356,8 +413,10 @@ public class WebSocketMessageBroadcaster { latitude, longitude ); + double rawSpeedMps = rawDistance / Math.max(dtSec, MIN_DT_SECONDS); + AdaptiveFilterParams adaptive = resolveAdaptiveFilterParams(payload.getObjectType(), rawSpeedMps); - double maxAllowedJump = resolveMaxAllowedJumpMeters(payload.getObjectType(), dtSec); + double maxAllowedJump = resolveMaxAllowedJumpMeters(adaptive.maxSpeedMps(), adaptive.jumpMarginMeter(), dtSec); boolean isOutlierJump = rawDistance > maxAllowedJump; if (isOutlierJump && state.rejectedCount < REACQUIRE_AFTER_REJECTS - 1) { state.rejectedCount++; @@ -372,11 +431,11 @@ public class WebSocketMessageBroadcaster { filteredLongitude = longitude; } else { state.rejectedCount = 0; - if (rawDistance <= jitterMeterThreshold) { + if (rawDistance <= adaptive.jitterMeter()) { filteredLatitude = state.lastFilteredLatitude; filteredLongitude = state.lastFilteredLongitude; } else { - double alpha = clamp(positionEmaAlpha, 0.05, 1.0); + double alpha = clamp(adaptive.emaAlpha(), 0.05, 1.0); filteredLatitude = state.lastFilteredLatitude + alpha * (latitude - state.lastFilteredLatitude); filteredLongitude = state.lastFilteredLongitude + alpha * (longitude - state.lastFilteredLongitude); } @@ -421,10 +480,49 @@ public class WebSocketMessageBroadcaster { return latitude >= -90.0 && latitude <= 90.0 && longitude >= -180.0 && longitude <= 180.0; } - private double resolveMaxAllowedJumpMeters(String objectType, double dtSec) { - double safeDtSec = Math.max(dtSec, 0.05); - double speedMps = resolveMaxSpeedMps(objectType); - return speedMps * safeDtSec + Math.max(maxJumpMarginMeter, 0.0); + private double resolveMaxAllowedJumpMeters(double speedMps, double jumpMarginMeter, double dtSec) { + double safeDtSec = Math.max(dtSec, MIN_DT_SECONDS); + return speedMps * safeDtSec + Math.max(jumpMarginMeter, 0.0); + } + + private AdaptiveFilterParams resolveAdaptiveFilterParams(String objectType, double rawSpeedMps) { + if (!"AIRCRAFT".equals(objectType)) { + return new AdaptiveFilterParams( + jitterMeterThreshold, + positionEmaAlpha, + resolveMaxSpeedMps(objectType), + maxJumpMarginMeter, + MotionProfile.FAST_ROLL + ); + } + + if (rawSpeedMps < speedSlowThresholdMps) { + return new AdaptiveFilterParams( + slowJitterMeter, + slowEmaAlpha, + slowMaxSpeedMps, + slowJumpMarginMeter, + MotionProfile.SLOW_TAXI + ); + } + + if (rawSpeedMps < speedFastThresholdMps) { + return new AdaptiveFilterParams( + fastJitterMeter, + fastEmaAlpha, + fastMaxSpeedMps, + fastJumpMarginMeter, + MotionProfile.FAST_ROLL + ); + } + + return new AdaptiveFilterParams( + airborneJitterMeter, + airborneEmaAlpha, + airborneMaxSpeedMps, + airborneJumpMarginMeter, + MotionProfile.AIRBORNE + ); } private double resolveMaxSpeedMps(String objectType) {