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

1、增加算法6

Browse Source
This commit is contained in:
weidong 2024-09-04 06:19:13 +08:00
parent a7f8df9f97
commit 26625b4510
7 changed files with 349 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
readme.txt
View File

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

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

@ -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++) {

253
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator6.java Normal file
View File

@ -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;
}
}

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

@ -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){

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

@ -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();
}
}

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

@ -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);
gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos);
if(latestRpos != null) {
gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos);
}
else {
gnssCalcFilterService.calc(device, groupCalc, locationRecord, focusCalculator.getReferPoint());
}
// 记录本次位置，作为下次的参考
// 算法1：以上轮位置作为参考，跟随变化速度快
// 算法2：以滤波后的位置作为参考，跟随变化速度慢；如果倾角大于某一门限，则跟随上轮位置
if(focusCalculator.getVer()==4) {
if(focusCalculator.getVer()==6) {
if(locationRecord.getRpose()!=null) {
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()});

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

@ -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>