1、增加算法6
This commit is contained in:
weidong
parent
a7f8df9f97
commit
26625b4510
@ -27,3 +27,12 @@ beidou-rtcm 9904 9902(外),9903(外)
|
||||
beidou-fwd 9906
|
||||
ntrip-proxy 9910 11001(外)
|
||||
beidou-exapi 9908(外)
|
||||
|
||||
2024-9
|
||||
算法：
|
||||
1）固定值筛选：a）排序，取中间50%；b）取距离参考点最近的50%
|
||||
2）坏点判断：a）和前一个比较；b）和参考点比较
|
||||
3）幅度压缩：压缩值=上个周期平滑值+(单位周期解算值-上个周期平滑值)*幅度压缩系数，平滑值=滤波窗口内avg(压缩值)
|
||||
4）初值作为参考点的有效时长，缺省为1个周期。从初始值更新时间开始算起
|
||||
|
||||
小树林算法：1b+2b+3高程0.1+4的48小时
|
||||
@ -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++) {
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@ -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){
|
||||
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@ -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);
|
||||
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()});
|
||||
|
||||
@ -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>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user