From d3af38d7dc9415eb2bfdd4cfbf2ce92e4cbf9726 Mon Sep 17 00:00:00 2001
From: zhongchao <zhongchao@zhidaoauto.com>
Date: Fri, 29 Apr 2022 10:09:06 +0800
Subject: [PATCH] fix identify

---
 .../core/function/map/IdentifyDataDrawer.java | 23 +++++++------------
 .../eagle/core/function/map/KalmanFilter.java |  5 +---
 2 files changed, 9 insertions(+), 19 deletions(-)

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 2984881937..5d5ec92459 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
@@ -126,14 +126,13 @@ public class IdentifyDataDrawer {
             //首次过来的数据不添加,首次未添加的感知物在调用完绘制方法后再塞入cache map
             MessagePad.TrackedObject cacheData = mMarkersCaches.get(uuid);
             if (cacheData != null) {
-                MessagePad.TrackedObject correctData;
-                correctData = kalmanCorrectData(data);
-                mFilterTrafficData.add(correctData);
+                if (data.getSpeed() < 0.5) {
+                    data.toBuilder().setHeading(cacheData.getHeading()).setLongitude(cacheData.getLongitude()).setLatitude(cacheData.getLatitude()).build();
+                }
+                mFilterTrafficData.add(data);
                 //更新已存在的感知物体数据
-                mMarkersCaches.put(uuid, correctData);
-            } else {
-                mMarkersCaches.put(uuid, data);
             }
+            mMarkersCaches.put(uuid, data);
             trafficDataUuidList.add(uuid);
         }
         return mFilterTrafficData;
@@ -146,17 +145,11 @@ public class IdentifyDataDrawer {
             Object o = algoCache.get(uuid);
             KalmanFilter kf = (KalmanFilter) o;
             assert kf != null;
-            double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude());
             algoCache.put(uuid, kf);
             MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
             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, 需要对每个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();
+            if (data.getSpeed() < 0.5) {
+                return data.toBuilder().setHeading(cacheTrackObj.getHeading()).setLongitude(cacheTrackObj.getLongitude()).setLatitude(cacheTrackObj.getLatitude()).build();
             } else {
                 return data;
             }
@@ -165,7 +158,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(), data.getHeading(), r));
+            algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), 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 08c2408a39..9552063ede 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,13 +10,10 @@ public class KalmanFilter {
     double[][] k = new double[][]{{0.0D, 0.0D}, {0.0D, 0.0D}};
     int idx = 1;
 
-    double cacheHeading;
-
-    public KalmanFilter(double lon, double lat, double originHeading, double r) {
+    public KalmanFilter(double lon, double lat, 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) {