前言
发现做静态轨迹纠偏还是有点用的,记录一下
实现
LoucsCleanUtil.java
//数据清洗
public static List<LocusCoordinate> dataCleanWGS84(List<LocusCoordinate> data) {
//时间间隔(时间间隔)
double T = 1.0 / 900;
//经度方差(水平位置)
double JDFX = 5.8;
//纬度方差(垂直位置)
double WDFX = 1.3;
//状态矩阵(初始值不重要,会自动更正)
double[][] X_ARRAY = {
{Double.parseDouble(data.get(0).getLon())},
{0},
{Double.parseDouble(data.get(0).getLat())},
{0}
};
AtomicReference<Matrix> X = new AtomicReference<>(new Matrix(X_ARRAY));
//状态协方差矩阵(初始值不重要,会自动更正)
double[][] P_ARRAY = {
{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
};
AtomicReference<Matrix> P = new AtomicReference<>(new Matrix(P_ARRAY));
//状态转移矩阵
double[][] F_ARRAY = {
{1, T, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, T},
{0, 0, 0, 1}
};
Matrix F = new Matrix(F_ARRAY);
//状态转移协方差矩阵(对偏差忽略不计)
double[][] Q_ARRAY = {
{0.0001, 0, 0, 0},
{0, 0.0001, 0, 0},
{0, 0, 0.0001, 0},
{0, 0, 0, 0.0001}
};
Matrix Q = new Matrix(Q_ARRAY);
//观测矩阵(观测经度纬度)
double[][] H_ARRAY = {
{1, 0, 0, 0},
{0, 0, 1, 0}
};
Matrix H = new Matrix(H_ARRAY);
//观测噪声方差(对角线为各维度方差)
double[][] R_ARRAY = {
{JDFX, 0},
{0, WDFX},
};
Matrix R = new Matrix(R_ARRAY);
//I 维度为4
double[][] I_ARRAY = {
{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
};
Matrix I = new Matrix(I_ARRAY);
// List<LocusCoordinate> removeShipList = new ArrayList<>();
AtomicInteger i = new AtomicInteger();
List<LocusCoordinate> tgList = new ArrayList<>();
data.forEach(ship -> {
try {
i.getAndIncrement();
//==============第1条式子==============
Matrix X_ = F.times(X.get());
//==============第2条式子==============
Matrix P_ = F.times(P.get()).times(F.transpose()).plus(Q);
//==============第3条式子==============
Matrix K = P_.times(H.transpose()).times(H.times(P_).times(H.transpose()).plus(R).inverse());
//==============第4条式子==============
//水平位置
double Px_tt = Double.parseDouble(ship.getLon());
//垂直位置
double Py_tt = Double.parseDouble(ship.getLat());
double[][] Z_ARRAY = {
{Px_tt},
{Py_tt}
};
Matrix Z = new Matrix(Z_ARRAY);
X.set(X_.plus(K.times(Z.minus(H.times(X_)))));
//==============第5条式子==============
P.set(I.minus(K.times(H)).times(P_));
//==============数据清洗==============
double abs = Math.abs(Double.parseDouble(ship.getLat()) - X.get().get(2, 0));
double abs1 = Math.abs(Double.parseDouble(ship.getLon()) - X.get().get(0, 0));
//清除噪声
if (abs1 >= 0.8 || abs >= 0.8) {
// removeShipList.add(ship);
System.out.println();
System.out.println("\033[32;4m" + "======================================《《filter start》》 ===================================================" + "\033[0m");
System.out.println("序号: " + i.get() + " old-JD:" + Px_tt + " old-WD:" + Py_tt + " new-JD:" + X.get().get(0, 0) + " new-WD:" + X.get().get(2, 0));
System.out.println("\033[32;4m" + "=============================================================================================================" + "\033[0m");
System.out.println();
} else {
Double[] wgs84 = GCJ02ToWGS84Util.GCJ02ToWGS84(X.get().get(0, 0), X.get().get(2, 0));
ship.setLon(String.valueOf(wgs84[0]));
ship.setLat(String.valueOf(wgs84[1]));
tgList.add(ship);
System.out.println("序号: " + i.get() + " old-JD:" + Px_tt + " old-WD:" + Py_tt + " new-JD:" + X.get().get(0, 0) + " new-WD:" + X.get().get(2, 0));
}
} catch (Exception e) {
e.printStackTrace();
}
});
// System.out.println("count :" + removeShipList.size());
// data.removeAll(removeShipList);
return tgList;
}
public static List<LocusCoordinate> dataCleanGCJ02(List<LocusCoordinate> data) {
//时间间隔(时间间隔)
double T = 1.0 / 900;
//经度方差(水平位置)
double JDFX = 5.8;
//纬度方差(垂直位置)
double WDFX = 1.3;
//状态矩阵(初始值不重要,会自动更正)
double[][] X_ARRAY = {
{Double.parseDouble(data.get(0).getLon())},
{0},
{Double.parseDouble(data.get(0).getLat())},
{0}
};
AtomicReference<Matrix> X = new AtomicReference<>(new Matrix(X_ARRAY));
//状态协方差矩阵(初始值不重要,会自动更正)
double[][] P_ARRAY = {
{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
};
AtomicReference<Matrix> P = new AtomicReference<>(new Matrix(P_ARRAY));
//状态转移矩阵
double[][] F_ARRAY = {
{1, T, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, T},
{0, 0, 0, 1}
};
Matrix F = new Matrix(F_ARRAY);
//状态转移协方差矩阵(对偏差忽略不计)
double[][] Q_ARRAY = {
{0.0001, 0, 0, 0},
{0, 0.0001, 0, 0},
{0, 0, 0.0001, 0},
{0, 0, 0, 0.0001}
};
Matrix Q = new Matrix(Q_ARRAY);
//观测矩阵(观测经度纬度)
double[][] H_ARRAY = {
{1, 0, 0, 0},
{0, 0, 1, 0}
};
Matrix H = new Matrix(H_ARRAY);
//观测噪声方差(对角线为各维度方差)
double[][] R_ARRAY = {
{JDFX, 0},
{0, WDFX},
};
Matrix R = new Matrix(R_ARRAY);
//I 维度为4
double[][] I_ARRAY = {
{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
};
Matrix I = new Matrix(I_ARRAY);
// List<LocusCoordinate> removeShipList = new ArrayList<>();
AtomicInteger i = new AtomicInteger();
List<LocusCoordinate> tgList = new ArrayList<>();
data.forEach(ship -> {
try {
i.getAndIncrement();
//==============第1条式子==============
Matrix X_ = F.times(X.get());
//==============第2条式子==============
Matrix P_ = F.times(P.get()).times(F.transpose()).plus(Q);
//==============第3条式子==============
Matrix K = P_.times(H.transpose()).times(H.times(P_).times(H.transpose()).plus(R).inverse());
//==============第4条式子==============
//水平位置
double Px_tt = Double.parseDouble(ship.getLon());
//垂直位置
double Py_tt = Double.parseDouble(ship.getLat());
double[][] Z_ARRAY = {
{Px_tt},
{Py_tt}
};
Matrix Z = new Matrix(Z_ARRAY);
X.set(X_.plus(K.times(Z.minus(H.times(X_)))));
//==============第5条式子==============
P.set(I.minus(K.times(H)).times(P_));
//==============数据清洗==============
double abs = Math.abs(Double.parseDouble(ship.getLat()) - X.get().get(2, 0));
double abs1 = Math.abs(Double.parseDouble(ship.getLon()) - X.get().get(0, 0));
//清除噪声
if (abs1 >= 0.8 || abs >= 0.8) {
// removeShipList.add(ship);
System.out.println();
System.out.println("\033[32;4m" + "======================================《《filter start》》 ===================================================" + "\033[0m");
System.out.println("序号: " + i.get() + " old-JD:" + Px_tt + " old-WD:" + Py_tt + " new-JD:" + X.get().get(0, 0) + " new-WD:" + X.get().get(2, 0));
System.out.println("\033[32;4m" + "=============================================================================================================" + "\033[0m");
System.out.println();
} else {
ship.setLon(String.valueOf(X.get().get(0, 0)));
ship.setLat(String.valueOf(X.get().get(2, 0)));
tgList.add(ship);
System.out.println("序号: " + i.get() + " old-JD:" + Px_tt + " old-WD:" + Py_tt + " new-JD:" + X.get().get(0, 0) + " new-WD:" + X.get().get(2, 0));
}
} catch (Exception e) {
e.printStackTrace();
}
});
// System.out.println("count :" + removeShipList.size());
// data.removeAll(removeShipList);
return tgList;
}
jar
<!-- Java矩阵包 Jama -->
<dependency>
<groupId>gov.nist.math</groupId>
<artifactId>jama</artifactId>
<version>1.0.3</version>
</dependency>
**粗体** _斜体_ [链接](http://example.com) `代码` - 列表 > 引用
。你还可以使用@
来通知其他用户。