From 0b61447274f12c919d751bc09654c89694e47078 Mon Sep 17 00:00:00 2001 From: weidong Date: Mon, 5 Aug 2024 17:44:02 +0800 Subject: [PATCH] =?UTF-8?q?1=E3=80=81=E4=BC=98=E5=8C=96=E6=9C=89=E6=95=88?= =?UTF-8?q?=E7=82=B9=E5=88=A4=E6=96=AD=E6=96=B9=E6=B3=95=EF=BC=9A=E6=BB=A1?= =?UTF-8?q?=E8=B6=B3=E4=BB=A5=E4=B8=8B=E4=B8=A4=E4=B8=AA=E6=9D=A1=E4=BB=B6?= =?UTF-8?q?=E4=B8=BA=E6=9C=89=E6=95=88=EF=BC=9A=201=EF=BC=89=E5=92=8C?= =?UTF-8?q?=E5=89=8D=E4=B8=80=E4=B8=AA=E8=A7=A3=E7=9B=B8=E6=AF=94=E5=B0=8F?= =?UTF-8?q?=E4=BA=8E=E9=97=A8=E9=99=90=202=EF=BC=89=E5=92=8C=E5=89=8D?= =?UTF-8?q?=E4=BA=8C=E4=B8=AA=E8=A7=A3=E7=9B=B8=E6=AF=94=E5=B0=8F=E4=BA=8E?= =?UTF-8?q?=E9=97=A8=E9=99=90=EF=BC=8C=E6=88=96=E8=80=85=E5=92=8C=E6=BB=A4?= =?UTF-8?q?=E6=B3=A2=E5=90=8E=E7=9A=84=E8=A7=A3=E7=9B=B8=E6=AF=94=E5=B0=8F?= =?UTF-8?q?=E4=BA=8E=E9=97=A8=E9=99=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sideslope/calc/GNSSCalcFilterService.java | 44 +++++++++++++++---- 1 file changed, 35 insertions(+), 9 deletions(-) diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java index 897ec163..874ae472 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java @@ -68,14 +68,37 @@ public class GNSSCalcFilterService { // 检查是不是坏点 GnssCalcData record0 = null; if(gnssHistoryRecords.size()>0) record0 = gnssHistoryRecords.get(0); - if(record0 != null && Math.abs(record0.getB562e() - newRecord.getB562e())<0.001) { + + //如果上一个点为空，则这个点是第一个点，判为无效 + if(record0 == null) return false; + //连续两个相同的点也认为无效 + if(Math.abs(record0.getB562e() - newRecord.getB562e())<0.001) { return false; } - if(referPos==null && record0!=null) { - referPos = new double[]{record0.getB562e(), record0.getB562n(), record0.getB562d()}; + //和上一个点相比，超过门限则认为无效 + if (Math.abs(newRecord.getB562e() - record0.getB562e()) > xyThreshold || + Math.abs(newRecord.getB562n() - record0.getB562n()) > xyThreshold || + Math.abs(newRecord.getB562d() - record0.getB562d()) > zThreshold) { + isGood = false; //记录为坏点，下次不参与滤波 + logger.debug("{} bad point",newRecord.getDeviceid()); } - if(referPos==null) return false; - + // 如果是好点，对于增强算法还需做进一步判断 + if(isGood && isAdvFilter){ + //和参考点比，如果是坏点，再和前二个点比如果还是坏点才认为是坏点 + isGood =(referPos!=null && (Math.abs(newRecord.getB562e() - referPos[0]) <= xyThreshold && + Math.abs(newRecord.getB562n() - referPos[1]) <= xyThreshold && + Math.abs(newRecord.getB562d() - referPos[2]) <= zThreshold)); + if(!isGood){ + GnssCalcData record1 = null; + if (gnssHistoryRecords.size() > 1) record1 = gnssHistoryRecords.get(1); + isGood =(record1 == null || + ((Math.abs(newRecord.getB562e() - record1.getB562e()) <= xyThreshold && + Math.abs(newRecord.getB562n() - record1.getB562n()) <= xyThreshold && + Math.abs(newRecord.getB562d() - record1.getB562d()) <= zThreshold))); + } + logger.debug("{} point adv judge {}",newRecord.getDeviceid(), isGood); + } + /* if (Math.abs(newRecord.getB562e() - referPos[0]) > xyThreshold || Math.abs(newRecord.getB562n() - referPos[1]) > xyThreshold || Math.abs(newRecord.getB562d() - referPos[2]) > zThreshold) { @@ -87,13 +110,16 @@ public class GNSSCalcFilterService { GnssCalcData record1 = null; if(gnssHistoryRecords.size()>1) record1 = gnssHistoryRecords.get(1); if(record1 == null || - Math.abs(newRecord.getB562e() - record1.getB562e()) > xyThreshold*2 || + (Math.abs(newRecord.getB562e() - record1.getB562e()) > xyThreshold*2 || Math.abs(newRecord.getB562n() - record1.getB562n()) > xyThreshold*2 || - Math.abs(newRecord.getB562d() - record1.getB562d()) > zThreshold*2) { + Math.abs(newRecord.getB562d() - record1.getB562d()) > zThreshold*2) || + (Math.abs(newRecord.getB562e() - referPos[0]) > xyThreshold || + Math.abs(newRecord.getB562n() - referPos[1]) > xyThreshold || + Math.abs(newRecord.getB562d() - referPos[2]) > zThreshold)) { logger.debug("{} adv filter bad point",newRecord.getDeviceid()); isGood = false; //记录为坏点，下次不参与滤波 } - } + }*/ return isGood; } @@ -174,7 +200,7 @@ public class GNSSCalcFilterService { // 选取[newRecordTime-filterCycleHour, newRcordTime]之间的记录做平滑 // 如果这个时间段的记录数少于FILTER_MIN_RECORD_NUM，本次不做平滑 LocalDateTime newRecordTime = newRecord.getCreatetime(); - LocalDateTime filterAfterTime = newRecordTime.minusHours(filterCycleHour