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

增加解算算法4，一起性取参考点最近的50%的点求均值，参考点为平滑后的值

Browse Source
This commit is contained in:
weidong 2024-07-07 07:49:02 +08:00
parent 6c55f8cbb2
commit 6a8c373188
5 changed files with 94 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,69 @@
package com.imdroid.sideslope.bd;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import java.util.*;
/**
* 1.简化重心计算，选取和参考点最近的50%的点求均值
*/
public class FocusCalculator4 extends FocusCalculator1{
final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class);
// b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数
public FocusCalculator4(){
super();
gravityMinCount = 80;
}
@Override
public double[] resultB562(){
try {
if (pointList.size() >= gravityMinCount) {
List<Point> selectPoints = new ArrayList<>();
if (referPoint != null && referPoint.length > 0) {
// 计算所有点位与变量之间的距离，并存入集合
for (double[] point : pointList) {
Point pointObj = new Point(point[0], point[1], point[2]);
pointObj.setXDistance(Math.abs(point[0] - referPoint[0])); // 设置该点到变量的水平距离
pointObj.setYDistance(Math.abs(point[1] - referPoint[1])); // 设置该点到变量的水平距离
pointObj.setZDistance(Math.abs(point[2] - referPoint[2])); // 设置该点到变量的垂直距离
selectPoints.add(pointObj); // 将点位加入集合
}
//分别对东北天排序
Collections.sort(selectPoints, Comparator.comparing(Point::getXDistance));//排序
List<Point> points = selectPoints.subList(0, selectPoints.size() / 2);
double[] focusX = focusPointObj(points);
Collections.sort(selectPoints, Comparator.comparing(Point::getYDistance));//排序
points = selectPoints.subList(0, selectPoints.size() / 2);
double[] focusY = focusPointObj(points);
Collections.sort(selectPoints, Comparator.comparing(Point::getZDistance));//排序
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++) {
selectPoints.add(new Point(pointList.get(i)[0], pointList.get(i)[1], pointList.get(i)[2]));
}
// 求重心
double[] focus = focusPointObj(selectPoints);
return focus;
}
}
}
catch (Exception e){
logger.error("error!",e);
return null;
}
return resultB562Sub();
}
@Override
public int getVer() {
return 4;
}
}

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

@ -10,7 +10,6 @@ import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;
import java.time.LocalDateTime;
import java.time.format.DateTimeFormatter;
import java.util.List;
import java.util.concurrent.ConcurrentHashMap;
@ -31,8 +30,6 @@ public class GNSSCalcFilterService {
}
final private ConcurrentHashMap<String, VaryFilterCycle> autoCycleDevices = new ConcurrentHashMap<>();
final DateTimeFormatter dateFormatter = DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss");
public void calc(Device gnssDevice, GnssGroupCalc groupCalc, GnssCalcData newRecord,
double[] referPos) {
String deviceId = newRecord.getDeviceid();
@ -114,8 +111,8 @@ public class GNSSCalcFilterService {
LocalDateTime minCycleTime = newRecordTime.minusHours(minCycleHour);
QueryWrapper<GnssCalcData> query = new QueryWrapper<>();
query.eq("deviceid", deviceId);
query.ge("createtime", filterAfterTime.format(dateFormatter));
query.le("createtime", newRecordTime.format(dateFormatter));
query.ge("createtime", filterAfterTime);
query.le("createtime", newRecordTime);
//query.eq("enabled", true);// 坏点参与比较，不参与滤波
query.orderByDesc("createtime");
@ -183,8 +180,8 @@ public class GNSSCalcFilterService {
LocalDateTime minCycleTime = newRecordTime.minusHours(minCycleHour);
QueryWrapper<GnssCalcData> query = new QueryWrapper<>();
query.eq("deviceid", deviceId);
query.ge("createtime", filterAfterTime.format(dateFormatter));
query.le("createtime", newRecordTime.format(dateFormatter));
query.ge("createtime", filterAfterTime);
query.le("createtime", newRecordTime);
//query.eq("enabled", true);// 坏点参与比较，不参与滤波
query.orderByDesc("createtime");
@ -254,7 +251,7 @@ public class GNSSCalcFilterService {
LocalDateTime cmpTime = now.minusHours(groupCalc.getFilter_min_hour());
QueryWrapper<GnssCalcData> queryWrapper = new QueryWrapper<>();
queryWrapper.eq("deviceid", deviceId);
queryWrapper.le("createtime",cmpTime.format(dateFormatter));
queryWrapper.le("createtime",cmpTime);
queryWrapper.eq("enabled",true);
queryWrapper.isNotNull("rpose");
queryWrapper.last("limit 1");
@ -299,8 +296,8 @@ public class GNSSCalcFilterService {
LocalDateTime beforTime = afterTime.plusHours(groupCalc.getFilter_hour());
QueryWrapper<GnssCalcData> query = new QueryWrapper<>();
query.eq("deviceid", deviceId);
query.le("createtime", beforTime.format(dateFormatter));
query.ge("createtime", afterTime.format(dateFormatter));
query.le("createtime", beforTime);
query.ge("createtime", afterTime);
//query.eq("enabled",true); //好点坏点重新判断
query.orderByAsc("createtime");

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

@ -77,7 +77,10 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
//todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
FocusCalculator focusCalculator;
if(groupCalc!=null && groupCalc.getVer() == 3){
if(groupCalc!=null && groupCalc.getVer() == 4){
focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4());
}
else if(groupCalc!=null && groupCalc.getVer() == 3){
focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator3());
}
else if(groupCalc!=null && groupCalc.getVer() == 2){
@ -213,7 +216,16 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
// 记录本次位置，作为下次的参考
// 算法1：以上轮位置作为参考，跟随变化速度快
// 算法2：以滤波后的位置作为参考，跟随变化速度慢；如果倾角大于某一门限，则跟随上轮位置
if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.isShocked()) {
if(focusCalculator.getVer()==4) {
if(locationRecord.getRpose()!=null) {
focusCalculator.setReferPoint(
new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()});
}
else if(focusCalculator.getReferPoint() == null){
focusCalculator.setReferPoint(b562Result);
}
}
else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.isShocked()) {
focusCalculator.setReferPoint(b562Result);
}
else {

7
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/executor/D331RtcmMessageExecutor.java
View File

@ -7,7 +7,6 @@ import com.imdroid.sideslope.bd.Gga;
import com.imdroid.sideslope.message.D331RtcmMessage;
import com.imdroid.sideslope.sal.Device;
import com.imdroid.sideslope.sal.DeviceService;
import com.imdroid.sideslope.server.DeviceChannel;
import com.imdroid.sideslope.server.OnlineChannels;
import com.imdroid.sideslope.service.DataPersistService;
import io.netty.buffer.ByteBuf;
@ -53,7 +52,7 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
for (Device device : deviceList) {
if (device.getOpMode() != GnssDevice.OP_MODE_USE) continue;
String deviceId = device.getDeviceId();
if(deviceId.equals("2419385")){
/*if(deviceId.equals("2419385")){
DeviceChannel channel = OnlineChannels.INSTANCE.getConfigChannel(deviceId);
if(channel != null && channel.isOnline()){
if (logger.isDebugEnabled()) {
@ -64,7 +63,7 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
channel.writeAndFlush(buf);
}
}
else {
else {*/
OnlineChannels.INSTANCE.get(deviceId).ifPresent(deviceChannel -> {
if (logger.isDebugEnabled()) {
logger.debug("forward d331 rtcm message to device {}", deviceId);
@ -76,7 +75,7 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
buf.writeBytes(forwardBytes);
deviceChannel.writeAndFlush(buf);
});
}
//}
}
}

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

@ -36,6 +36,7 @@
<option value="1">1</option>
<option value="2">2</option>
<option value="3">3</option>
<option value="4">4</option>
</select>
</div>
</div>