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