1、算法2优化

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java

@@ -1,11 +1,13 @@
 package com.imdroid.sideslope.bd;
 
 
+import java.time.LocalDateTime;
+
 public interface FocusCalculator {
     void reset();
 
     // 数据收集：b562位置、惯导角度、基站到测站时延
-    void addXyz(double[] xyz);
+    void addXyz(double[] xyz, LocalDateTime dataTime);
     void addTilt(Tilt tilt);
     void addDelayMs(int ms);
 

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator1.java

@@ -76,7 +76,7 @@ public class FocusCalculator1 implements FocusCalculator{
      * @param xyz
      */
     @Override
-    public void addXyz(double[] xyz){
+    public void addXyz(double[] xyz, LocalDateTime dataTime){
         if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++;
         else if((int)xyz[3] == UBXUtil.FLOAT_RESULT) counterNoFixed++;
         else {
@@ -112,9 +112,12 @@ public class FocusCalculator1 implements FocusCalculator{
     }
     @Override
     public double[] getReferPoint(){
-        if(referPoint==null || referPointTime==null) return null;
+        if(referPoint==null || referPointTime==null) {
+            return null;
+        }
 
         if(LocalDateTime.now().isAfter(referPointTime.plusHours(24))) {
+            logger.info("refer time is expired");
             referPoint = null;
         }
         return referPoint;

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator2.java

@@ -12,35 +12,35 @@ import java.util.*;
  * 6.。。。。。。
  */
 public class FocusCalculator2 extends FocusCalculator1{
-    public final static int MAX_B562_NUM = 1800;
+    final static int B562_EXPIRED_HOUR = 4;
-    final static int B562_EXPIRED_HOUR = 1;
 
-    final List<LocalDateTime> pointTimeList = Collections.synchronizedList(new ArrayList<>());
+    final List<Map.Entry<LocalDateTime, List<double[]>>> cyclePointList = Collections.synchronizedList(new ArrayList<>());
-    final List<double[]> pointAuxList = Collections.synchronizedList(new ArrayList<>());
-    final List<LocalDateTime> pointTimeAuxList = Collections.synchronizedList(new ArrayList<>());
 
-    boolean referPointFilteredFlag = false;
+    final static int INIT_REFER_POINT_NOT_SET = 0;
-    LocalDateTime initReferPointTime;
+    final static int INIT_REFER_POINT_IN_USE = 1;
-
+    final static int INIT_REFER_POINT_EXPIRED = 2;
- /*   public FocusCalculator2(double height, Tilt tilt0, double[] position0){
+    int originalReferPointFlag= INIT_REFER_POINT_NOT_SET; // 0:not set; 1:in use; 2:expired
-        super();
+    LocalDateTime originalReferPointTime;
-        iterStep = 0.2;
-    }
-*/
-    public FocusCalculator2(){
-        super();
-        iterStep = 2;//2mm
-        gravityMaxR = 500;
-    }
 
+    class EComparator implements Comparator<double[]>{
          @Override
-    public void reset(){
+         public int compare(double[] point1, double[] point2) {
-        //pointList.clear();
+             return (int) ((point1[0] - point2[0])*100);
-        pointAuxList.clear();
+         }
-        pointTimeAuxList.clear();
+     }
-        counterNoB562 = 0;
+
-        counterNoFixed = 0;
+    class NComparator implements Comparator<double[]>{
-        counterFixedResult = 0;
+        @Override
+        public int compare(double[] point1, double[] point2) {
+            return (int) ((point1[1] - point2[1])*100);
+        }
+    }
+
+    class DComparator implements Comparator<double[]>{
+        @Override
+        public int compare(double[] point1, double[] point2) {
+            return (int) ((point1[2] - point2[2])*100);
+        }
     }
 
     /**
@@ -48,85 +48,70 @@ public class FocusCalculator2 extends FocusCalculator1{
      * @param xyz
      */
     @Override
-    public void addXyz(double[] xyz){
+    public void addXyz(double[] xyz, LocalDateTime dataTime){
-        LocalDateTime now = LocalDateTime.now();
+        Map.Entry entry = null;
+        if(cyclePointList.size()==0 || counterFixedResult+counterNoB562+counterNoFixed == 0){
+            entry = new AbstractMap.SimpleEntry(dataTime, new ArrayList<double[]>());
+            cyclePointList.add(entry);
+            // 检查有没有过期的数据
+            Iterator<Map.Entry<LocalDateTime, List<double[]>>> iterator = cyclePointList.iterator();
+            while (iterator.hasNext()){
+                LocalDateTime t = iterator.next().getKey();
+                if(t.isBefore(dataTime.minusHours(B562_EXPIRED_HOUR))){
+                    iterator.remove();
+                }
+            }
+        }
+        else{
+            entry = cyclePointList.get(cyclePointList.size()-1);
+        }
 
         if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++;
         else if((int)xyz[3] == UBXUtil.FLOAT_RESULT) counterNoFixed++;
         else {
             counterFixedResult ++;
             if (filter(xyz)) {
-                pointAuxList.add(xyz);
+                List<double[]> list = (List<double[]>) entry.getValue();
-                pointTimeAuxList.add(now);
+                list.add(xyz);
             }
-            if (pointAuxList.size() > gravityMaxCount) {
-                pointAuxList.remove(0);
-                pointTimeAuxList.remove(0);
-            }
-        }
-    }
-    /*public void addXyz(double[] xyz){
-        LocalDateTime now = LocalDateTime.now();
-        Iterator<LocalDateTime> iterPointTime = pointTimeList.iterator();
-        Iterator<double[]> iterPoint = pointList.iterator();
-        while(iterPointTime.hasNext() && iterPoint.hasNext()){
-            LocalDateTime pointTime = iterPointTime.next();
-            iterPoint.next();
-            if(now.isAfter(pointTime.plusHours(B562_EXPIRED_HOUR))){
-                iterPointTime.remove();
-                iterPoint.remove();
-            }
-            else{
-                break;
         }
     }
 
-        if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++;
+    double calcGravity(List<double[]> list , int index){
-        else if((int)xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++;
+        double sum = 0;
-        else {
+        int begin = (int) (list.size() * 0.25);
-            counterFixedResult ++;
+        int end = (int) (list.size() * 0.75);
-            if (filter(xyz)) {
+        if(end-begin == 0) return 0;
-                pointList.add(xyz);
+
-                pointTimeList.add(now);
+        for(int i=begin; i<end; i++){
+            sum += list.get(i)[index];
         }
-            if (pointList.size() > gravityMaxCount) {
+        return sum/(end-begin);
-                pointList.remove(0);
-                pointTimeList.remove(0);
     }
-        }
-    }*/
 
     public double[] resultB562(){
-        if(counterFixedResult<30) return null;
         // 把本轮的固定解添加到pointList一起解算
-        int totalNum = pointList.size()+pointAuxList.size();
+        if(counterFixedResult<30) return null;
-        LocalDateTime latestTime = pointTimeAuxList.get(pointTimeAuxList.size()-1);
+        // 求pointList的重心：当固定解大于300个时，去掉25%最大值和25%最小值
-        Iterator<LocalDateTime> iterPointTime = pointTimeList.iterator();
+        pointList.clear();
-        Iterator<double[]> iterPoint = pointList.iterator();
+        Iterator<Map.Entry<LocalDateTime, List<double[]>>> iterator = cyclePointList.iterator();
-        while(iterPointTime.hasNext() && iterPoint.hasNext()){
+        while (iterator.hasNext()){
-            LocalDateTime pointTime = iterPointTime.next();
+            pointList.addAll(iterator.next().getValue());
-            iterPoint.next();
-            if(totalNum>MAX_B562_NUM || latestTime.isAfter(pointTime.plusHours(B562_EXPIRED_HOUR))){
-                iterPointTime.remove();
-                iterPoint.remove();
-                totalNum--;
         }
-            else{
+        logger.info("total fixed point num to calculated: {},{},{}",pointList.size(),
-                break;
+                cyclePointList.get(0).getKey(), cyclePointList.get(cyclePointList.size()-1).getKey());
-            }
+
-        }
+        if(pointList.size() >= gravityMaxCount){
-        pointList.addAll(pointAuxList);
+            Collections.sort(pointList, new EComparator());
-        pointTimeList.addAll(pointTimeAuxList);
+            double e = calcGravity(pointList,0);
-        logger.info("point list: {},{},{},{}",pointTimeList.get(0),pointTimeAuxList.get(pointTimeAuxList.size()-1),pointTimeList.size(),pointList.size());
+            Collections.sort(pointList, new NComparator());
-        // 求pointList的重心
+            double n = calcGravity(pointList,1);
-        return super.resultB562();
+            Collections.sort(pointList, new DComparator());
+            double d = calcGravity(pointList,2);
+            return new double[]{e,n,d};
         }
 
-    @Override
+        return null;
-    public int[] getB562Stat(){
-        int[] b562Stat = new int[]{counterFixedResult,counterNoFixed,counterNoB562};
-        reset();
-        return b562Stat;
     }
 
     @Override
@@ -146,24 +131,25 @@ public class FocusCalculator2 extends FocusCalculator1{
 
     @Override
     public void setReferPoint(double[] point){
-        if(referPointFilteredFlag){
+        if(originalReferPointFlag==INIT_REFER_POINT_IN_USE){
             // 初始参考位置保持24小时
-            if(initReferPointTime!=null &&
+            if(originalReferPointTime!=null &&
-                    LocalDateTime.now().isAfter(initReferPointTime.plusHours(24))){
+                    LocalDateTime.now().isBefore(originalReferPointTime.plusHours(24))){
+                return;
+            }
+            else originalReferPointFlag = INIT_REFER_POINT_EXPIRED;
+        }
         referPoint = point;
-            }
+        referPointTime = LocalDateTime.now();
-        }
-        else{
-            referPoint = point;
-        }
     }
 
-    public boolean isReferPointFiltered(){
+    public boolean isOriginalReferPointSet(){
-        return referPointFilteredFlag;
+        return (originalReferPointFlag!=INIT_REFER_POINT_NOT_SET);
     }
-    public void setInitReferPoint(double[] point){
+    public void setOriginalReferPoint(double[] point){
         referPoint = point;
-        referPointFilteredFlag = true;
+        referPointTime = LocalDateTime.now();
-        initReferPointTime = LocalDateTime.now();
+        originalReferPointFlag = INIT_REFER_POINT_IN_USE;
+        originalReferPointTime = LocalDateTime.now();
     }
 }

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java

@@ -97,7 +97,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
 
         // 单次b562
         double[] doubles = message.getB562_loc();//unit: mm
-        focusCalculator.addXyz(doubles);
+        focusCalculator.addXyz(doubles,message.getCreateTime());
         if (logger.isDebugEnabled()) {
             logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
         }
@@ -207,7 +207,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
             }
             else {
                 FocusCalculator2 focusCalculator2 = (FocusCalculator2)focusCalculator;
-                if(focusCalculator2.isReferPointFiltered()) {
+                if(focusCalculator2.isOriginalReferPointSet()) {
                     if (locationRecord.getEnabled() && locationRecord.getRpose() != null) {
                         focusCalculator2.setReferPoint(new double[]{locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd()});
                         logger.info("{} set reference pos to filter result:{},{},{}",
@@ -220,7 +220,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
                 else{
                     // 查询设备有没有配置初始位置
                     if(device.getIPose()!=null){
-                        focusCalculator2.setInitReferPoint(new double[]{device.getIPose(),device.getIPosn(),device.getIPosd()});
+                        focusCalculator2.setOriginalReferPoint(new double[]{device.getIPose(),device.getIPosn(),device.getIPosd()});
                         logger.info("{} set reference pos to init pos:{},{},{}",
                                 deviceId,device.getIPose(),device.getIPosn(),device.getIPosd());
                     }

sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java

@@ -42,7 +42,7 @@ public class BeidouDevice {
             execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_5xx.log");
             execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_6xx.log");
 */
-/*
+
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_1xx.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_2xx.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_3xx.log");
@@ -69,16 +69,16 @@ public class BeidouDevice {
           //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412270_1.log");
             //executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412270_1xx.log");
             //executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412270_2xx.log");
-            executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_1.log");
+/*            executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_1.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_2.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_1xx.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_2xx.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_3xx.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_4xx.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_5xx.log");
- */
+
             execute("C:\\Users\\wd\\Desktop\\log\\2353120_0424_1.log");
-/*
+
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2353120_0424_1.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2353120_0424_2.log");
             executeAsD342("C:\\Users\\wd\\Desktop\\log\\2353120_0424_1xx.log");