1、增加算法6

readme.txt

@@ -27,3 +27,12 @@ beidou-rtcm     9904        9902(外),9903(外)
 beidou-fwd      9906
 ntrip-proxy     9910        11001(外)
 beidou-exapi    9908(外)
+
+2024-9
+算法：
+1）固定值筛选：a）排序，取中间50%；b）取距离参考点最近的50%
+2）坏点判断：a）和前一个比较；b）和参考点比较
+3）幅度压缩：压缩值=上个周期平滑值+(单位周期解算值-上个周期平滑值)*幅度压缩系数，平滑值=滤波窗口内avg(压缩值)
+4）初值作为参考点的有效时长，缺省为1个周期。从初始值更新时间开始算起
+
+小树林算法：1b+2b+3高程0.1+4的48小时

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

@@ -9,7 +9,7 @@ import java.util.*;
  * 1.简化重心计算，选取和参考点最近的50%的点求均值
  */
 public class FocusCalculator4 extends FocusCalculator1{
-    final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class);
+    final Logger logger = LoggerFactory.getLogger(FocusCalculator4.class);
     // b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数
 
     public FocusCalculator4(){
@@ -22,6 +22,8 @@ public class FocusCalculator4 extends FocusCalculator1{
         try {
             if (pointList.size() >= gravityMinCount) {
                 List<Point> selectPoints = new ArrayList<>();
+                // 如果有参考点，则选取距离参考点最近的50%的点
+                // 否则排序取中间50%
                 if (referPoint != null && referPoint.length > 0) {
                     // 计算所有点位与变量之间的距离，并存入集合
                     for (double[] point : pointList) {
@@ -42,7 +44,6 @@ public class FocusCalculator4 extends FocusCalculator1{
                     points = selectPoints.subList(0, selectPoints.size() / 2);
                     double[] focusZ = focusPointObj(points);
                     return new double[]{focusX[0], focusY[1], focusZ[2]};
-
                 }
                 else {
                     for (int i = pointList.size()/2; i < pointList.size(); i++) {

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

@@ -0,0 +1,253 @@
+package com.imdroid.sideslope.bd;
+
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+import java.time.LocalDateTime;
+import java.util.*;
+import java.util.function.Function;
+
+/**
+ * 1.取距离参考点最近的50%
+ * 2.和参考点比较
+ * 3.初始参考点更新时间和有效期
+ * 4.压缩率
+ */
+public class FocusCalculator6 implements FocusCalculator{
+    final Logger logger = LoggerFactory.getLogger(FocusCalculator6.class);
+    // 固定解4元组：E、N、U、Quality
+    final List<Point> pointList = Collections.synchronizedList(new ArrayList<>());
+    String deviceId;
+    int minPointNum = 50;
+    int maxPointNum = 3000;
+    double[] referPoint; //参考点，一般是上一次计算的位置，用来辅助计算重心
+    LocalDateTime referPointExpiredTime;
+    boolean isOrginalReferPointInUsed = false;
+
+    double xyCompressRate = 0.2;
+    double zCompressRate = 0.1;
+
+    // 惯导算法相关
+    final List<Tilt> tilts = Collections.synchronizedList(new ArrayList<>());
+    boolean isShock = false;
+    final double shockThreshold = 1.5;
+
+    //其他
+    int delay_ms;
+    int delay_counter;
+    int counterNoB562;
+    int counterNoFixed;
+    int counterFixedResult;
+
+    public FocusCalculator6(){
+        counterNoB562 = 0;
+        counterNoFixed = 0;
+        counterFixedResult = 0;
+        delay_ms = 0;
+        delay_counter = 0;
+    }
+
+    public FocusCalculator6(String deviceId, int minPointNum){
+        counterNoB562 = 0;
+        counterNoFixed = 0;
+        counterFixedResult = 0;
+        delay_ms = 0;
+        delay_counter = 0;
+        this.deviceId = deviceId;
+        this.minPointNum = minPointNum;
+    }
+
+    @Override
+    public void reset(){
+        if(pointList.size()>=minPointNum) {
+            pointList.clear();
+        }
+        else{
+            logger.info("{} num of fixed points:{}, less than {}",deviceId, pointList.size(),minPointNum);
+        }
+        tilts.clear();
+        counterNoB562 = 0;
+        counterNoFixed = 0;
+        counterFixedResult = 0;
+        delay_ms = 0;
+        delay_counter = 0;
+    }
+
+    /**
+     * 加入pointList
+     * @param xyz
+     */
+    @Override
+    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 {
+            counterFixedResult ++;
+            if (filter(xyz)) {//是否是无效值null
+                Point point = new Point(xyz[0], xyz[1], xyz[2]);
+                if(referPoint!=null){
+                    point.setXDistance(Math.abs(xyz[0]-referPoint[0]));
+                    point.setYDistance(Math.abs(xyz[1]-referPoint[1]));
+                    point.setZDistance(Math.abs(xyz[2]-referPoint[2]));
+                }
+                pointList.add(point);
+            }
+            if (pointList.size() > maxPointNum) {
+                pointList.remove(0);
+            }
+        }
+    }
+
+    @Override
+    public int[] getB562Stat(){
+        return new int[]{counterFixedResult,counterNoFixed,counterNoB562};
+    }
+
+    @Override
+    public void setReferPoint(double[] point){
+        LocalDateTime now = LocalDateTime.now();
+        // 如果初值设置在24小时内，仍使用初值作为参考值
+        if(isOrginalReferPointInUsed){
+            if(now.isBefore(referPointExpiredTime)) return;
+            else {
+                logger.info("{} original refer point expired, changed to new point", deviceId);
+                isOrginalReferPointInUsed = false;
+            }
+        }
+        referPoint = point;
+        referPointExpiredTime = now.plusHours(24);
+    }
+
+    public void setOriginalReferPoint(double[] point){
+        logger.info("{} set original refer point {},{},{}", deviceId, point[0],point[1],point[2]);
+
+        isOrginalReferPointInUsed = true;
+        referPoint = point;
+        referPointExpiredTime = LocalDateTime.now().plusHours(24);
+    }
+
+    @Override
+    public double[] getReferPoint(){
+        if(referPoint==null || referPointExpiredTime==null) {
+            return null;
+        }
+
+        if(LocalDateTime.now().isAfter(referPointExpiredTime)) {
+            logger.info("{} refer time is expired", deviceId);
+            referPoint = null;
+        }
+        return referPoint;
+    }
+
+    @Override
+    public double[] resultB562(){
+        if(pointList.size()<minPointNum) return null;
+
+        double focusX = sortAndAvg(Point::getXDistance, Point::getX, referPoint != null);
+        double focusY = sortAndAvg(Point::getYDistance, Point::getY, referPoint != null);
+        double focusZ = sortAndAvg(Point::getZDistance, Point::getZ, referPoint != null);
+
+        if(referPoint!=null){
+            focusX = referPoint[0] + (focusX - referPoint[0])*xyCompressRate;
+            focusY = referPoint[1] + (focusY - referPoint[1])*xyCompressRate;
+            focusZ = referPoint[2] + (focusZ - referPoint[2])*zCompressRate;
+        }
+        return new double[]{focusX, focusY, focusZ};
+    }
+
+    double sortAndAvg(Function<Point, Double> sortFunc, Function<Point, Double> avgFunc, boolean hasRef){
+        List<Point> points;
+        if(hasRef) {
+            pointList.sort(Comparator.comparing(sortFunc));
+            points = pointList.subList(0, pointList.size() / 2);
+        }
+        else{
+            pointList.sort(Comparator.comparing(avgFunc));
+            int begin = pointList.size()/4;
+            points = pointList.subList(begin, pointList.size()-begin);
+        }
+        return avgPoint(points, avgFunc);
+    }
+
+    double avgPoint(List<Point> points, Function<Point, Double> getFunction){
+        if(points.size() == 0) return 0;
+        double sumValue = 0;
+        for (Point point:points){
+            sumValue += getFunction.apply(point);
+        }
+        return sumValue/points.size();
+    }
+
+    /**
+     * 过滤，剔除0,无穷大，与nan
+     * @param xyz
+     * @return true表示数据正常
+     */
+    boolean filter(double[] xyz){
+        double a = xyz[0]*xyz[1]*xyz[2];
+        if(a == 0 || Double.isInfinite(a) || Double.isNaN(a)){
+            return false;
+        }
+        return true;
+    }
+
+    /**
+     * 加入tilts
+     * @param tilt
+     */
+    @Override
+    public void addTilt(Tilt tilt){
+        tilts.add(tilt);
+        if(tilt.getShock() > shockThreshold) isShock = true;
+
+        if(tilts.size() > maxPointNum){
+            tilts.remove(0);
+        }
+    }
+
+    @Override
+    public boolean isShocked() {
+        return isShock;
+    }
+
+    @Override
+    public int getVer() {
+        return 6;
+    }
+
+    @Override
+    public Tilt avgTilt() {
+        if (tilts.size() == 0) {
+            return null;
+        }
+        Iterator<Tilt> iterator = tilts.iterator();
+        double roll = 0;
+        double pitch = 0;
+        double yaw = 0;
+        while (iterator.hasNext()) {
+            Tilt tilt = iterator.next();
+            roll += tilt.getRoll();
+            pitch += tilt.getPitch();
+            yaw += tilt.getYaw();
+        }
+        return new Tilt(pitch/tilts.size(),roll/tilts.size(),yaw/tilts.size());
+    }
+
+    @Override
+    public void addDelayMs(int ms){
+        delay_ms += ms;
+        delay_counter ++;
+    }
+
+    @Override
+    public void addGGA(Gga gga) {
+
+    }
+
+    @Override
+    public int getAvgDelayMs(){
+        if(delay_counter==0) return -1;
+        else return delay_ms/delay_counter;
+    }
+
+}

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

@@ -15,6 +15,9 @@ public class Point {
         this.x = x;
         this.y = y;
         this.z = z;
+        xDistance = 0;
+        yDistance = 0;
+        zDistance = 0;
     }
 
     public Point plus(Point p){

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

@@ -10,8 +10,10 @@ import org.springframework.beans.factory.annotation.Autowired;
 import org.springframework.stereotype.Service;
 
 import java.time.LocalDateTime;
+import java.util.Comparator;
 import java.util.List;
 import java.util.concurrent.ConcurrentHashMap;
+import java.util.function.Function;
 
 @Service
 public class GNSSCalcFilterService {
@@ -331,4 +333,51 @@ public class GNSSCalcFilterService {
         }
         msgMapper.insert(gnssMsg);
     }
+
+    public double[] calcFilterValueByLastDay(GnssCalcData curCalcData, GnssGroupCalc groupCalc,
+                                             double xyCompressRate, double zCompressRate){
+        LocalDateTime now = curCalcData.getCreatetime();
+        LocalDateTime beginTime = now.minusHours(25);
+        QueryWrapper<GnssCalcData> query = new QueryWrapper<>();
+        query.eq("deviceid", curCalcData.getDeviceid());
+        query.ge("createtime", beginTime);
+        query.orderByAsc("createtime");
+
+        List<GnssCalcData> calcDataList = repository.selectList(query);
+        if(calcDataList.size()<16*6) return null; //至少有2/3的时间有固定解
+        else{//周期至少24小时
+            if(calcDataList.get(0).getCreatetime().isAfter(now.minusHours(24))) return null;
+        }
+
+        // 分别按东北天排序
+        double e = avgPoint(calcDataList, GnssCalcData::getB562e);
+        double n = avgPoint(calcDataList, GnssCalcData::getB562n);
+        double u = avgPoint(calcDataList, GnssCalcData::getB562d);
+
+        // 比较是否是好点
+        if(Math.abs(e-curCalcData.getB562e())*xyCompressRate<=groupCalc.getXy_threshold() &&
+                Math.abs(n-curCalcData.getB562n())*xyCompressRate<=groupCalc.getXy_threshold()&&
+                Math.abs(u-curCalcData.getB562d())*zCompressRate<=groupCalc.getZ_threshold()){
+            curCalcData.setEnabled(true);
+            curCalcData.setRpose(NumberUtils.scaleTwo(e));
+            curCalcData.setRposn(NumberUtils.scaleTwo(n));
+            curCalcData.setRposd(NumberUtils.scaleTwo(u));
+            return new double[]{e,n,u};
+        }
+        return null;
+    }
+
+    double avgPoint(List<GnssCalcData> points, Function<GnssCalcData, Double> getFunction){
+        if(points.size() == 0) return 0;
+        int begin = points.size()/4;
+        int end = points.size() - begin;
+        points.sort(Comparator.comparing(getFunction));
+        List<GnssCalcData> subList = points.subList(begin, end);
+
+        double sumValue = 0;
+        for (GnssCalcData point:subList){
+            sumValue += getFunction.apply(point);
+        }
+        return sumValue/points.size();
+    }
 }

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

@@ -76,29 +76,32 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
         Device device = deviceService.findByDeviceId(deviceId);
         if(device == null) return;
         GnssGroupCalc groupCalc = getGroupCalc(device.getCalcGroupId());
+        if(groupCalc==null) return;
 
         if(completeWhenIdle) resultOutputTimer(device, groupCalc, message.getCreateTime());
 
         //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
         FocusCalculator focusCalculator;
-        if(groupCalc!=null && groupCalc.getVer() == 5){
+        if(groupCalc.getVer() == 6){
+            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator6(deviceId, 50));
+        }
+        else if(groupCalc.getVer() == 5){
             focusCalculator = calculatorMap.get(deviceId);
             if(focusCalculator ==null) {
                 Short removeRate = groupCalc.getRemove_rate();
                 if (removeRate == null) removeRate = 20;
                 focusCalculator = new FocusCalculator5(deviceId, 50, removeRate);
-                //((FocusCalculator5) focusCalculator).setParas(deviceId, 50, removeRate);
                 calculatorMap.put(deviceId,focusCalculator);
             }
         }
-        if(groupCalc!=null && groupCalc.getVer() == 4){
+        else if(groupCalc.getVer() == 4){
             focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4());
         }
-        else if(groupCalc!=null && groupCalc.getVer() == 3){
+        else if(groupCalc.getVer() == 3){
             Device bsDevice = deviceService.findByDeviceId(device.getParentId());
             focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator3(bsDevice));
         }
-        else if(groupCalc!=null && groupCalc.getVer() == 2){
+        else if(groupCalc.getVer() == 2){
             focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator2());
         }
         else {
@@ -226,12 +229,29 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
             locationRecord.setPps(focusCalculator.getAvgDelayMs());
             // 滤波后的结果
             double[] latestRpos = getLatestRpos(deviceId);
+            if(latestRpos != null) {
                 gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos);
+            }
+            else {
+                gnssCalcFilterService.calc(device, groupCalc, locationRecord, focusCalculator.getReferPoint());
+            }
 
             // 记录本次位置，作为下次的参考
-            // 算法1：以上轮位置作为参考，跟随变化速度快
+            if(focusCalculator.getVer()==6) {
-            // 算法2：以滤波后的位置作为参考，跟随变化速度慢；如果倾角大于某一门限，则跟随上轮位置
+                if(locationRecord.getRpose()!=null) {
-            if(focusCalculator.getVer()==4) {
+                    focusCalculator.setReferPoint(
+                            new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()});
+                }
+                //检查过去24小时是否有有效解，如果没有，用24小时固定解求一个均值作为
+                else if(focusCalculator.getReferPoint() == null){
+                    double[] avgEnu = gnssCalcFilterService.calcFilterValueByLastDay(locationRecord, groupCalc,0.2,0.1);
+                    logger.info("{} calc 24 hours filtered pos",deviceId);
+                    if(avgEnu != null){
+                        ((FocusCalculator6)focusCalculator).setOriginalReferPoint(avgEnu);
+                    }
+                }
+            }
+            else if(focusCalculator.getVer()==4) {
                 if(locationRecord.getRpose()!=null) {
                     focusCalculator.setReferPoint(
                             new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()});

sec-beidou/src/main/resources/templates/page/table/gnss_add_group_calc.html

@@ -38,6 +38,7 @@
                     <option value="3">3</option>
                     <option value="4">4</option>
                     <option value="5">5</option>
+                    <option value="5">6</option>
                 </select>
             </div>
         </div>