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 2025-02-05 07:53:54 +08:00
parent 4ebc66da85
commit accd869dc8
3 changed files with 36 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java
View File

@ -14,37 +14,19 @@ import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.*;
* 博通：用GGA绝对坐标代替相对位置
*/
public class FocusCalculator3 extends FocusCalculator1{
//final static long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000
final static int bad_change_mm = 500;//固定解跳变连续10次超过500mm，认为是周跳
final static int bad_duration = 10;
int bad_count = 0;
Device bsDevice;
public FocusCalculator3(Device bsDevice){
super();
this.bsDevice = bsDevice;
gravityMinCount = 80;
}
/*
static class EComparator implements Comparator<double[]>{
@Override
public int compare(double[] point1, double[] point2) {
return Double.compare(point1[1], point2[1]);
//return (int) ((point1[0] - point2[0])*100);
}
}
static class NComparator implements Comparator<double[]>{
@Override
public int compare(double[] point1, double[] point2) {
return Double.compare(point1[1], point2[1]);
//return (int) ((point1[1] - point2[1])*100);
}
}
static class DComparator implements Comparator<double[]>{
@Override
public int compare(double[] point1, double[] point2) {
return Double.compare(point1[1], point2[1]);
//return (int) ((point1[2] - point2[2])*100);
}
}
*/
@Override
public void addGGA(Gga gga) {
if(gga == null) return;
@ -69,7 +51,6 @@ public class FocusCalculator3 extends FocusCalculator1{
// 以基站为站心的 ENU 坐标
ENU_Coordinate difference_enu = ECEF2ENU(reference_ecef, rover_ecef);
System.out.println("DIFF ENU:" + difference_enu.ENU_E + " " + difference_enu.ENU_N + " " + difference_enu.ENU_U);
end = new double[]{
difference_enu.ENU_E*1000,
difference_enu.ENU_N*1000,
@ -79,7 +60,17 @@ public class FocusCalculator3 extends FocusCalculator1{
if(gga.isFixed()) {
counterFixedResult++;
//pointList.add(xyz);
if(pointList.size()>0){
double[] lastXyz = pointList.get(pointList.size()-1);
if(Math.abs(end[0]-lastXyz[0])>bad_change_mm ||
Math.abs(end[1]-lastXyz[1])>bad_change_mm ||
Math.abs(end[2]-lastXyz[2])>bad_change_mm){
bad_count++;
return;
}
}
bad_count = 0;
pointList.add(end);
}
else if(gga.getQuality() == 5) counterNoFixed++;
@ -95,33 +86,6 @@ public class FocusCalculator3 extends FocusCalculator1{
}
/*double calcGravity(List<double[]> list , int index){
double sum = 0;
int begin = (int) (list.size() * 0.25);
int end = (int) (list.size() * 0.75);
if(end-begin == 0) return 0;
for(int i=begin; i<end; i++){
sum += list.get(i)[index];
}
return sum/(end-begin);
}
@Override
public double[] resultB562(){
if(pointList.size() >= 3){
Collections.sort(pointList, new EComparator());
double e = calcGravity(pointList,0);
Collections.sort(pointList, new NComparator());
double n = calcGravity(pointList,1);
Collections.sort(pointList, new DComparator());
double d = calcGravity(pointList,2);
return new double[]{e,n,d};
}
return null;
}*/
@Override
public double[] resultB562(){
try {
@ -166,9 +130,23 @@ public class FocusCalculator3 extends FocusCalculator1{
return null;
}
@Override
public void reset(){
super.reset();
bad_count = 0;
}
@Override
public int getVer() {
return 3;
}
public boolean isJump(){
if (bad_count>bad_duration){
bad_count = 0;
return true;
}
else return false;
}
}

117
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator7.java
View File

@ -1,117 +0,0 @@
package com.imdroid.sideslope.bd;
import com.imdroid.sideslope.sal.Device;
import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.ECEF2ENU;
import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.LLA2ECEF;
/**
* 博通：用GGA绝对坐标代替相对位置
*/
public class FocusCalculator7 extends FocusCalculator3{
//final static long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000
final static int bad_change_mm = 500;//固定解跳变连续10次超过500mm，认为是周跳
final static int bad_duration = 10;
int bad_count = 0;
public FocusCalculator7(Device bsDevice) {
super(bsDevice);
}
/*@Override
public void addGGA(Gga gga) {
if(gga == null) return;
double[] xyz = new double[]{gga.getLongitude()*scale, gga.getLatitude()*scale, gga.getAltitude()*1000, gga.getQuality()};
if(gga.isFixed()) {
counterFixedResult++;
if(pointList.size()>0){
double[] lastXyz = pointList.get(pointList.size()-1);
if(Math.abs(xyz[0]-lastXyz[0])>bad_change_mm ||
Math.abs(xyz[1]-lastXyz[1])>bad_change_mm ||
Math.abs(xyz[2]-lastXyz[2])>bad_change_mm){
bad_count++;
return;
}
}
bad_count = 0;
pointList.add(xyz);
}
else if(gga.getQuality() == 5) counterNoFixed++;
else counterNoB562 ++;
}*/
@Override
public void addGGA(Gga gga) {
if(gga == null) return;
double[] end;
// 测站的GGA - 测站 LLA 数据
GeoCoordConverterUtil.LLA_Coordinate rover_lla = new GeoCoordConverterUtil.LLA_Coordinate(gga.getLatitude(),gga.getLongitude(),gga.getAltitude());
// 测站 LLA 坐标转 ECEF 坐标，单位米
GeoCoordConverterUtil.ECEF_Coordinate rover_ecef = LLA2ECEF(rover_lla);
if(bsDevice==null || bsDevice.getEcefx()==null){
end = new double[]{
rover_ecef.getECEF_X()*1000,
rover_ecef.getECEF_Y()*1000,
rover_ecef.getECEF_Z()*1000,
gga.getQuality()};
}
else {
// 查询测站的绑定的基站
GeoCoordConverterUtil.ECEF_Coordinate reference_ecef = new GeoCoordConverterUtil.ECEF_Coordinate(bsDevice.getEcefx(), bsDevice.getEcefy(), bsDevice.getEcefz());
// 以基站为站心的 ENU 坐标
GeoCoordConverterUtil.ENU_Coordinate difference_enu = ECEF2ENU(reference_ecef, rover_ecef);
end = new double[]{
difference_enu.ENU_E*1000,
difference_enu.ENU_N*1000,
difference_enu.ENU_U*1000,
gga.getQuality()};
}
if(gga.isFixed()) {
counterFixedResult++;
if(pointList.size()>0){
double[] lastXyz = pointList.get(pointList.size()-1);
if(Math.abs(end[0]-lastXyz[0])>bad_change_mm ||
Math.abs(end[1]-lastXyz[1])>bad_change_mm ||
Math.abs(end[2]-lastXyz[2])>bad_change_mm){
bad_count++;
return;
}
}
bad_count = 0;
pointList.add(end);
}
else if(gga.getQuality() == 5) counterNoFixed++;
else counterNoB562 ++;
}
@Override
public void reset(){
super.reset();
bad_count = 0;
}
@Override
public int getVer() {
return 7;
}
public boolean isJump(){
if (bad_count>bad_duration){
bad_count = 0;
return true;
}
else return false;
}
}

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

@ -85,16 +85,13 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
if(device == null) return;
GnssGroupCalc groupCalc = getGroupCalc(device.getCalcGroupId());
if(groupCalc==null) return;
device.setB562AsCalc(groupCalc.getVer()!=3 && groupCalc.getVer()!=7);
device.setB562AsCalc(groupCalc.getVer()!=3);
if(completeWhenIdle) resultOutputTimer(device, groupCalc, message.getCreateTime());
//todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
FocusCalculator focusCalculator;
if(groupCalc.getVer() == 7){
focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator7(null));
}
else if(groupCalc.getVer() == 6){
if(groupCalc.getVer() == 6){
focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator6(deviceId, 50));
}
else if(groupCalc.getVer() == 5){
@ -145,7 +142,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
focusCalculator.addGGA(gga);
logger.debug("测站{}的gga单次解析结果:{},{},{},{}",deviceId,
gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality());
if(groupCalc.getVer() == 7 && focusCalculator.isJump()){
if(groupCalc.getVer() == 3 && focusCalculator.isJump()){
logger.info("{}发生周跳",deviceId);
hardResetDevice(deviceId);
}
@ -273,7 +270,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos);
}
else {
if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5 || focusCalculator.getVer()==7){
if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5){
gnssCalcFilterService.calc(device, groupCalc, locationRecord, null);
}
else {
@ -316,7 +313,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
else focusCalculator.setReferPoint(b562Result);
}
}
else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5 || focusCalculator.getVer()==7) {
else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5) {
focusCalculator.setReferPoint(b562Result);
}
else {