Imdroid/security-monitor
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

1、所有算法的好点判断总是先和滤波值比较

Browse Source
This commit is contained in:
weidong 2024-08-05 21:03:19 +08:00
parent 0b61447274
commit ebcf103019
2 changed files with 20 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -63,9 +63,6 @@ public class GNSSCalcFilterService {
public boolean isGoodGnssData(GnssCalcData newRecord, double[] referPos, List<GnssCalcData> gnssHistoryRecords, public boolean isGoodGnssData(GnssCalcData newRecord, double[] referPos, List<GnssCalcData> gnssHistoryRecords,
float xyThreshold, float zThreshold, boolean isAdvFilter) { float xyThreshold, float zThreshold, boolean isAdvFilter) {
boolean isGood = true;
// 检查是不是坏点
GnssCalcData record0 = null; GnssCalcData record0 = null;
if(gnssHistoryRecords.size()>0) record0 = gnssHistoryRecords.get(0); if(gnssHistoryRecords.size()>0) record0 = gnssHistoryRecords.get(0);
@ -75,51 +72,28 @@ public class GNSSCalcFilterService {
if(Math.abs(record0.getB562e() - newRecord.getB562e())<0.001) { if(Math.abs(record0.getB562e() - newRecord.getB562e())<0.001) {
return false; return false;
} }
//和上一个点相比，超过门限则认为无效
if (Math.abs(newRecord.getB562e() - record0.getB562e()) > xyThreshold || // 和最近一个滤波值相比，判断是否是好点
Math.abs(newRecord.getB562n() - record0.getB562n()) > xyThreshold || boolean isGood =(referPos!=null && (Math.abs(newRecord.getB562e() - referPos[0]) <= xyThreshold &&
Math.abs(newRecord.getB562d() - record0.getB562d()) > zThreshold) { Math.abs(newRecord.getB562n() - referPos[1]) <= xyThreshold &&
isGood = false; //记录为坏点，下次不参与滤波 Math.abs(newRecord.getB562d() - referPos[2]) <= zThreshold));
logger.debug("{} bad point",newRecord.getDeviceid()); logger.debug("{} point judge {}",newRecord.getDeviceid(), isGood);
} //如果和参考点比无效，则和上一个点相比
// 如果是好点，对于增强算法还需做进一步判断 if(!isGood) {
if(isGood && isAdvFilter){ isGood =(Math.abs(newRecord.getB562e() - record0.getB562e()) <= xyThreshold &&
//和参考点比，如果是坏点，再和前二个点比如果还是坏点才认为是坏点 Math.abs(newRecord.getB562n() - record0.getB562n()) <= xyThreshold &&
isGood =(referPos!=null && (Math.abs(newRecord.getB562e() - referPos[0]) <= xyThreshold && Math.abs(newRecord.getB562d() - record0.getB562d()) <= zThreshold);
Math.abs(newRecord.getB562n() - referPos[1]) <= xyThreshold && // 如果是好点，对于增强算法还需和前2个比较
Math.abs(newRecord.getB562d() - referPos[2]) <= zThreshold)); if(isGood && isAdvFilter){
if(!isGood){
GnssCalcData record1 = null; GnssCalcData record1 = null;
if (gnssHistoryRecords.size() > 1) record1 = gnssHistoryRecords.get(1); if (gnssHistoryRecords.size() > 1) record1 = gnssHistoryRecords.get(1);
isGood =(record1 == null || 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.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); 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) {
logger.debug("{} bad point,delta: {},{},{} ",newRecord.getDeviceid(),
newRecord.getB562e()- referPos[0],newRecord.getB562n()- referPos[1],newRecord.getB562d()- referPos[2]);
isGood = false; //记录为坏点，下次不参与滤波
}
else if(isAdvFilter){
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.getB562n() - record1.getB562n()) > xyThreshold*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; return isGood;
} }
@ -336,7 +310,7 @@ public class GNSSCalcFilterService {
groupCalc.getXy_threshold(), groupCalc.getZ_threshold(), groupCalc.getAdv_filter()); groupCalc.getXy_threshold(), groupCalc.getZ_threshold(), groupCalc.getAdv_filter());
repository.updateById(calcData); repository.updateById(calcData);
lastTime = calcData.getCreatetime(); lastTime = calcData.getCreatetime();
referPos = new double[]{calcData.getB562e(), calcData.getB562n(), calcData.getB562d()}; referPos = new double[]{calcData.getRpose(), calcData.getRposn(), calcData.getRposd()};
} }
return lastTime; return lastTime;
} }

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

@ -215,7 +215,8 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
// pps // pps
locationRecord.setPps(focusCalculator.getAvgDelayMs()); locationRecord.setPps(focusCalculator.getAvgDelayMs());
// 滤波后的结果 // 滤波后的结果
gnssCalcFilterService.calc(device, groupCalc, locationRecord, referPos); double[] latestRpos = getLatestRpos(deviceId);
gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos);
// 记录本次位置，作为下次的参考 // 记录本次位置，作为下次的参考
// 算法1：以上轮位置作为参考，跟随变化速度快 // 算法1：以上轮位置作为参考，跟随变化速度快
@ -226,8 +227,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()}); new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()});
} }
else if(focusCalculator.getReferPoint() == null){ else if(focusCalculator.getReferPoint() == null){
double[] latestPos = getLatestPos(deviceId); if(latestRpos != null) focusCalculator.setReferPoint(latestRpos);
if(latestPos != null) focusCalculator.setReferPoint(latestPos);
else focusCalculator.setReferPoint(b562Result); else focusCalculator.setReferPoint(b562Result);
} }
} }
@ -292,7 +292,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
logger.info("device paras changed"); logger.info("device paras changed");
} }
double[] getLatestPos(String deviceId){ double[] getLatestRpos(String deviceId){
QueryWrapper<GnssCalcData> queryWrapper = new QueryWrapper<>(); QueryWrapper<GnssCalcData> queryWrapper = new QueryWrapper<>();
queryWrapper.eq("deviceid",deviceId); queryWrapper.eq("deviceid",deviceId);
queryWrapper.ge("createtime",LocalDateTime.now().minusHours(12)); queryWrapper.ge("createtime",LocalDateTime.now().minusHours(12));