Refactor WebSocketMessageBroadcaster to enhance position filtering logic and introduce adaptive speed thresholds for improved accuracy。优化平滑速度策略

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<String, PositionTrackState> 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) {
+    private double resolveMaxAllowedJumpMeters(double speedMps, double jumpMarginMeter, double dtSec) {
-        double safeDtSec = Math.max(dtSec, 0.05);
+        double safeDtSec = Math.max(dtSec, MIN_DT_SECONDS);
-        double speedMps = resolveMaxSpeedMps(objectType);
+        return speedMps * safeDtSec + Math.max(jumpMarginMeter, 0.0);
-        return speedMps * safeDtSec + Math.max(maxJumpMarginMeter, 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) {