前言
发现做动态轨迹纠偏还是有点用的，记录一下
实现
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; }