From b6b915f0d2e28828074a99f34b1a8d9c971d687b Mon Sep 17 00:00:00 2001 From: weidong Date: Mon, 5 Aug 2024 22:08:48 +0800 Subject: [PATCH] =?UTF-8?q?1=E3=80=81=E4=B8=8A=E4=B8=AA=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E5=AF=BC=E8=87=B4=E4=B8=8D=E5=87=BA=E5=8D=95=E7=82=B9=E8=A7=A3?= =?UTF-8?q?=E2=80=94=E2=80=94=E6=89=BE=E5=88=B0bug=EF=BC=8CselectOne?= =?UTF-8?q?=E7=9A=84=E6=9D=A1=E4=BB=B6=E5=B0=91=E4=BA=86limit=201?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sideslope/calc/GNSSCalcFilterService.java | 34 ++++++++----------- .../calc/SingleLineGNSSCalcService.java | 9 ++--- 2 files changed, 20 insertions(+), 23 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 66303d92..1f69681a 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 @@ -63,9 +63,6 @@ public class GNSSCalcFilterService { public boolean isGoodGnssData(GnssCalcData newRecord, double[] referPos, List gnssHistoryRecords, float xyThreshold, float zThreshold, boolean isAdvFilter) { - boolean isGood = true; - - // 检查是不是坏点 GnssCalcData record0 = null; if(gnssHistoryRecords.size()>0) record0 = gnssHistoryRecords.get(0); @@ -75,26 +72,25 @@ public class GNSSCalcFilterService { if(Math.abs(record0.getB562e() - newRecord.getB562e())<0.001) { return false; } - //和上一个点相比，超过门限则认为无效 - 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(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){ + + // 和最近一个滤波值相比，判断是否是好点 + boolean isGood =(referPos!=null && (Math.abs(newRecord.getB562e() - referPos[0]) <= xyThreshold && + Math.abs(newRecord.getB562n() - referPos[1]) <= xyThreshold && + Math.abs(newRecord.getB562d() - referPos[2]) <= zThreshold)); + logger.debug("{} point judge {}",newRecord.getDeviceid(), isGood); + //如果和参考点比无效，则和上一个点相比 + if(!isGood) { + isGood =(Math.abs(newRecord.getB562e() - record0.getB562e()) <= xyThreshold && + Math.abs(newRecord.getB562n() - record0.getB562n()) <= xyThreshold && + Math.abs(newRecord.getB562d() - record0.getB562d()) <= zThreshold); + // 如果是好点，对于增强算法还需和前2个比较 + if(isGood && isAdvFilter){ GnssCalcData record1 = null; if (gnssHistoryRecords.size() > 1) record1 = gnssHistoryRecords.get(1); isGood =(record1 == null || - ((Math.abs(newRecord.getB562e() - record1.getB562e()) <= xyThreshold && + (Math.abs(newRecord.getB562e() - record1.getB562e()) <= xyThreshold && Math.abs(newRecord.getB562n() - record1.getB562n()) <= xyThreshold && - Math.abs(newRecord.getB562d() - record1.getB562d()) <= zThreshold))); + Math.abs(newRecord.getB562d() - record1.getB562d()) <= zThreshold)); } logger.debug("{} point adv judge {}",newRecord.getDeviceid(), isGood); } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java index caa578e1..b3c55ca2 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java @@ -215,7 +215,8 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { // pps locationRecord.setPps(focusCalculator.getAvgDelayMs()); // 滤波后的结果 - gnssCalcFilterService.calc(device, groupCalc, locationRecord, referPos); + double[] latestRpos = getLatestRpos(deviceId); + gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos); // 记录本次位置，作为下次的参考 // 算法1：以上轮位置作为参考，跟随变化速度快 @@ -226,8 +227,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()}); } else if(focusCalculator.getReferPoint() == null){ - double[] latestPos = getLatestPos(deviceId); - if(latestPos != null) focusCalculator.setReferPoint(latestPos); + if(latestRpos != null) focusCalculator.setReferPoint(latestRpos); else focusCalculator.setReferPoint(b562Result); } } @@ -292,12 +292,13 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { logger.info("device paras changed"); } - double[] getLatestPos(String deviceId){ + double[] getLatestRpos(String deviceId){ QueryWrapper queryWrapper = new QueryWrapper<>(); queryWrapper.eq("deviceid",deviceId); queryWrapper.ge("createtime",LocalDateTime.now().minusHours(12)); queryWrapper.isNotNull("rpose"); queryWrapper.orderByDesc("createtime"); + queryWrapper.last("limit 1"); GnssCalcData gnssCalcData = dataMapper.selectOne(queryWrapper); if(gnssCalcData != null){ logger.info("{} getLatestPos", deviceId);