diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
index 9a9fb4728d..2984881937 100644
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
@@ -88,7 +88,6 @@ public class IdentifyDataDrawer {
             return;
         }
 
-        long start = System.currentTimeMillis();
         //清除缓存
         for (MessagePad.TrackedObject data : resultList) {
             if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
@@ -100,18 +99,13 @@ public class IdentifyDataDrawer {
             algoCache.remove(uuid);
         });
 
-        CallerLogger.INSTANCE.d(M_HMI + "arrow48", "origin data size : " + resultList.size());
         ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
 
-        long cost = System.currentTimeMillis() - start;
-        CallerLogger.INSTANCE.d(M_HMI + "arrow48", "cost : " + cost);
-
         if (filterList.size() > 0) {
             // 绘制新数据
             MogoMarkerManager.getInstance(mContext)
                     .updateBatchMarkerPosition(filterList);
         }
-        CallerLogger.INSTANCE.d(M_HMI + "arrow48", "mMarkersCaches : " + mMarkersCaches.size());
     }
 
     /**
@@ -158,9 +152,11 @@ public class IdentifyDataDrawer {
             assert cacheTrackObj != null;
             if (data.getSpeed() >= 1.5) {
                 double heading = MogoMap.getInstance().getMogoMap().getUIController().getAngle(cacheTrackObj.getLongitude(), cacheTrackObj.getLatitude(), lonLat[0], lonLat[1]);
-                double correct = Math.abs(heading - data.getHeading()) > 30 && Math.abs(heading - data.getHeading()) < 120 ? heading : data.getHeading();//todo test
-                CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , origin H : " + data.getHeading() + " , cal H : " + heading + ", correct : " + correct + " , 使用滤波角度 : " + (Math.abs(heading - data.getHeading()) > 30 && Math.abs(heading - data.getHeading()) < 120));
-                return data.toBuilder().setHeading(heading).setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
+                double correct = Math.abs(heading - data.getHeading()) > 30 && Math.abs(heading - data.getHeading()) < 120 ? heading : data.getHeading();//todo test, 需要对每个uuid做航向角缓存
+                if (data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TA_CHE.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_BUS.getType() || data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_TRUCK.getType()) {
+                    CallerLogger.INSTANCE.d(M_HMI + "type : " + data.getType(), " uuid : " + uuid + " , origin H : " + data.getHeading() + " , cal H : " + heading + ", correct : " + correct + " , 使用滤波角度 : " + (Math.abs(heading - data.getHeading()) > 30 && Math.abs(heading - data.getHeading()) < 120));
+                }
+                return data.toBuilder().setHeading(correct).setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
             } else {
                 return data;
             }
@@ -169,7 +165,7 @@ public class IdentifyDataDrawer {
             if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
                 r = 0.00001;
             }
-            algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
+            algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), data.getHeading(), r));
             return data;
         }
     }
diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java
index 08605437fa..08c2408a39 100644
--- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java
+++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/KalmanFilter.java
@@ -10,14 +10,17 @@ public class KalmanFilter {
     double[][] k = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
     int idx = 1;
 
-    public KalmanFilter(double lon, double lat, double r) {
+    double cacheHeading;
+
+    public KalmanFilter(double lon, double lat, double originHeading, double r) {
         this.xhat[0][0] = lon;
         this.xhat[0][1] = lat;
         this.r = r;
+        this.cacheHeading = originHeading;
     }
 
     public double[] filter(double lon, double lat) {
-        for(int i = 0; i < 2; ++i) {
+        for (int i = 0; i < 2; ++i) {
             this.xhatminus[this.idx][i] = this.xhat[1 - this.idx][i];
             this.pMinus[this.idx][i] = this.p[1 - this.idx][i] + 1.0E-6D;
             this.k[this.idx][i] = this.pMinus[this.idx][i] / (this.pMinus[this.idx][i] + this.r);
@@ -31,4 +34,5 @@ public class KalmanFilter {
         this.idx = 1 - this.idx;
         return new double[]{lon1, lat1};
     }
+
 }