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

1、增加算法5

Browse Source
This commit is contained in:
weidong 2024-08-17 17:33:40 +08:00
parent 5a1fc85cd6
commit 5df387716f
9 changed files with 289 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
sec-api/src/main/java/com/imdroid/secapi/dto/GnssGroupCalc.java
View File

@ -19,4 +19,5 @@ public class GnssGroupCalc {
Integer device_num;
Boolean auto_upload;
Short ver;
Short remove_rate;
}

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

@ -0,0 +1,248 @@
package com.imdroid.sideslope.bd;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import java.time.LocalDateTime;
import java.util.*;
/**
* 1.如果本次有效解数量少于50个，本轮不处理，累计到下次
* 2.enu分别排序，去掉前后20%，然后取交集，再求平均
* 3.最多取最后500*6=3000个点计算
*/
public class FocusCalculator5 implements FocusCalculator{
final Logger logger = LoggerFactory.getLogger(FocusCalculator5.class);
// 固定解4元组：E、N、U、Quality
final List<double[]> pointList = Collections.synchronizedList(new ArrayList<>());
String deviceId;
int minPointNum = 50;
int maxPointNum = 3000;
double[] referPoint; //参考点，一般是上一次计算的位置，用来辅助计算重心
LocalDateTime referPointTime;
short removeRate;
// 惯导算法相关
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;
static class EComparator implements Comparator<double[]>{
@Override
public int compare(double[] point1, double[] point2) {
return (int) ((point1[0] - point2[0])*100);
}
}
static class NComparator implements Comparator<double[]>{
@Override
public int compare(double[] point1, double[] point2) {
return (int) ((point1[1] - point2[1])*100);
}
}
static class UComparator implements Comparator<double[]>{
@Override
public int compare(double[] point1, double[] point2) {
return (int) ((point1[2] - point2[2])*100);
}
}
public FocusCalculator5(){
counterNoB562 = 0;
counterNoFixed = 0;
counterFixedResult = 0;
delay_ms = 0;
delay_counter = 0;
removeRate = 10;
}
public FocusCalculator5(String deviceId, int minPointNum, short removeRate){
counterNoB562 = 0;
counterNoFixed = 0;
counterFixedResult = 0;
delay_ms = 0;
delay_counter = 0;
this.deviceId = deviceId;
this.minPointNum = minPointNum;
this.removeRate = (short) (removeRate/2);
}
public void setParas(String deviceId, int minPointNum, short removeRate){
this.deviceId = deviceId;
this.minPointNum = minPointNum;
this.removeRate = (short) (removeRate/2);
}
@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
pointList.add(xyz);
}
if (pointList.size() > maxPointNum) {
pointList.remove(0);
}
}
}
@Override
public int[] getB562Stat(){
return new int[]{counterFixedResult,counterNoFixed,counterNoB562};
}
@Override
public void setReferPoint(double[] point){
referPoint = point;
referPointTime = LocalDateTime.now();
}
@Override
public double[] getReferPoint(){
if(referPoint==null || referPointTime==null) {
return null;
}
if(LocalDateTime.now().isAfter(referPointTime.plusHours(24))) {
logger.info("{} refer time is expired", deviceId);
referPoint = null;
}
return referPoint;
}
@Override
public double[] resultB562(){
if(pointList.size()<minPointNum) return null;
//enu分别排序并取中间值
List<double[]> eList = sortList(new EComparator());
List<double[]> nList = sortList(new NComparator());
List<double[]> uList = sortList(new UComparator());
//取交集
eList.retainAll(nList);
eList.retainAll(uList);
logger.info("{} fixed point selected: {}", deviceId, eList.size());
if(eList.size() == 0) return null;
//求均值
double eSum = 0;
double nSum = 0;
double uSum = 0;
for(double[] item : eList){
eSum += item[0];
nSum += item[1];
uSum += item[2];
}
return new double[]{eSum/eList.size(), nSum/eList.size(), uSum/eList.size()};
}
List<double[]> sortList(Comparator comparator){
List<double[]> pList = new ArrayList<>();
pList.addAll(pointList);
pList.sort(comparator);
int begin = pList.size()*removeRate/100;
int end = pList.size()-begin;
return pList.subList(begin, end);
}
/**
* 过滤，剔除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 5;
}
@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;
}
}

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

@ -81,6 +81,16 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
//todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
FocusCalculator focusCalculator;
if(groupCalc!=null && 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){
focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4());
}
@ -111,14 +121,14 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
double[] doubles = message.getB562_loc();//unit: mm
if(doubles !=null) {
focusCalculator.addXyz(doubles, message.getCreateTime());
logger.info("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
}
// 单次GGA
Gga gga = message.getGga();
if(gga != null) {
focusCalculator.addGGA(message.getGga());
logger.info("测站" + deviceId + "的gga单次解析结果:{},{},{},{}",
logger.debug("测站" + deviceId + "的gga单次解析结果:{},{},{},{}",
gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality());
}
}
@ -231,7 +241,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
else focusCalculator.setReferPoint(b562Result);
}
}
else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.isShocked()) {
else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5 || focusCalculator.isShocked()) {
focusCalculator.setReferPoint(b562Result);
}
else {

22
sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java
View File

@ -1,6 +1,7 @@
import com.imdroid.sideslope.bd.FocusCalculator;
import com.imdroid.sideslope.bd.FocusCalculator1;
import com.imdroid.sideslope.bd.FocusCalculator2;
import com.imdroid.sideslope.bd.FocusCalculator5;
import org.ejml.simple.SimpleMatrix;
import org.junit.Test;
import org.junit.runner.RunWith;
@ -23,7 +24,8 @@ import java.util.ListIterator;
classes = {
FocusCalculatorTest.class,
FocusCalculator1.class,
FocusCalculator2.class
FocusCalculator2.class,
FocusCalculator5.class
}
)
@ -46,15 +48,16 @@ public class FocusCalculatorTest {
@Test
public void run() throws IOException, ParseException {
System.out.println("*********** 2307042 *************");
System.out.println("算法1");
/*System.out.println("算法1");
focusCalculator = new FocusCalculator1();
test();
System.out.println("算法2");
focusCalculator = new FocusCalculator2();
test();
/*System.out.println("算法3");
focusCalculator = new FocusCalculator3();
test();*/
System.out.println("算法3");
focusCalculator = new FocusCalculator5();
((FocusCalculator5)focusCalculator).setParas("2307042",50, (short) 20);
test();
}
@ -79,9 +82,9 @@ public class FocusCalculatorTest {
//if(calcResultList.size()>=36) break;//18个小时
}
focusCalculator.addXyz(new double[]{
Double.parseDouble(arrs[2]),
Double.parseDouble(arrs[3]),
Double.parseDouble(arrs[4])},LocalDateTime.now());
Double.parseDouble(arrs[2])*10,
Double.parseDouble(arrs[3])*10,
Double.parseDouble(arrs[4])*10,2},LocalDateTime.now());
}
br.close();
fr.close();
@ -97,6 +100,7 @@ public class FocusCalculatorTest {
runFilter(4);
calcStd(filterResults);
filterResults.removeAll(filterResults);
/*
System.out.println("4 hours filter, igg3");
runFilter2(4);
calcStd(filterResults);
@ -110,7 +114,7 @@ public class FocusCalculatorTest {
runFilter2(8);
calcStd(filterResults);
filterResults.removeAll(filterResults);
*/
calcResultList.removeAll(calcResultList);
}

1
sec-beidou/src/main/resources/db/schema.sql
View File

@ -96,6 +96,7 @@ CREATE TABLE IF NOT EXISTS `gnssgroupcalc` (
`ver` smallint DEFAULT 1 COMMENT '算法版本',
`device_num` int DEFAULT 0,
`auto_upload` bit(1) DEFAULT 0,
`remove_rate` smallint DEFAULT NULL COMMENT '固定解剔除比例',
PRIMARY KEY (`id`)
) ENGINE=InnoDB DEFAULT CHARSET=utf8mb4;

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

@ -37,6 +37,7 @@
<option value="2">2</option>
<option value="3">3</option>
<option value="4">4</option>
<option value="5">5</option>
</select>
</div>
</div>
@ -103,6 +104,12 @@
</select>
</div>
</div>
<div class="layui-form-item">
<label class="layui-form-label">剔除比例(%)</label>
<div class="layui-input-block">
<input type="number" name="remove_rate" id="remove_rate" class="layui-input">
</div>
</div>
<div class="layui-form-item">
<div class="layui-input-block">
@ -157,6 +164,7 @@
$('#auto_upload').val(data.auto_upload?'1':'0');
$('#ver').val(data.ver);
$('#name').val(data.name);
$('#remove_rate').val(data.remove_rate);
form.render();
}
</script>

2
sec-beidou/src/test/java/UtilTest.java
View File

@ -1,4 +1,4 @@
import com.imdroid.secapi.utils.HexUtil;
import com.imdroid.common.util.HexUtil;
public class UtilTest {
public static void main(String[] args) {

9
sec-test-device/src/main/java/com/imdroid/beidou/test_device/BeidouTestApp.java
View File

@ -1,23 +1,22 @@
package com.imdroid.beidou.test_device;
import com.imdroid.beidou.test_device.task.BeidouDevice;
import org.springframework.boot.SpringApplication;
import org.springframework.boot.autoconfigure.SpringBootApplication;
import org.springframework.context.annotation.ComponentScan;
/**
* @author Layton
* @date 2023/1/31 20:33
*/
@SpringBootApplication(scanBasePackages = {"com.imdroid"})
@ComponentScan({"com.imdroid.*"})
@SpringBootApplication(scanBasePackages = {"com.imdroid.beidou.test_device"})
public class BeidouTestApp {
public static void main(String[] args) {
SpringApplication.run(BeidouTestApp.class, args);
/*BeidouDevice beidouDevice = new BeidouDevice();
BeidouDevice beidouDevice = new BeidouDevice();
beidouDevice.connectServer();
beidouDevice.run();*/
beidouDevice.run();
}
}

3
sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java
View File

@ -119,9 +119,6 @@ public class BeidouDevice {
Thread.sleep(30 * 1000);
cycleCount++;
}
if(arrs[2].contains("24474e47")){
System.out.println(time+" 24474e47");
}
udpClient.sendData(ByteUtil.hexStringTobyte(arrs[2]));
b562Count++;