前言发现静态轨迹校正还是有用的，记录一下实现LoucsCleanUtil.java//数据清洗publicstaticListdataCleanWGS84(Listdata){//Timeinterval(timeinterval)双T=1.0/900;//经度方差（水平位置）doubleJDFX=5.8;//纬度方差（垂直位置）doubleWDFX=1.3;//状态矩阵（初始值不重要，会自动修正）double[][]X_ARRAY={{Double.parseDouble(data.get(0).getLon())},{0},{Double.parseDouble(data.get(0).getLat())},{0}};AtomicReferenceX=newAtomicReference<>(newMatrix(X_ARRAY));//状态协方差矩阵（初始值不重要，会自动修正）double[][]P_ARRAY={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};AtomicReferenceP=newAtomicReference<>(newMatrix(P_ARRAY));//状态转换矩阵double[][]F_ARRAY={{1,T,0,0},{0,1,0,0},{0,0,1,T},{0,0,0,1}};矩阵F=新矩阵（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}};矩阵Q=新矩阵（Q_ARRAY）；//观测矩阵（观测经纬度）double[][]H_ARRAY={{1,0,0,0},{0,0,1,0}};矩阵H=新矩阵（H_ARRAY）；//观测噪声方差（对角线为各维度的方差）double[][]R_ARRAY={{JDFX,0},{0,WDFX},};矩阵R=新矩阵(R_ARRAY);//I维度为4double[][]I_ARRAY={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};MatrixI=newMatrix(I_ARRAY);//ListremoveShipList=新数组列表<>();AtomicIntegeri=newAtomicInteger();列表<轨迹坐标>tgList=newArrayList<>();data.forEach(ship->{try{i.getAndIncrement();//===============第1条===============MatrixX_=F.times(X.get());//===============第二条===============MatrixP_=F.times(P.get()).times(F.transpose()).plus(Q);//===============第三条===============矩阵K=P_.times(H.transpose()).times(H.times(P_).times(H.transpose()).plus(R).inverse());//===============第四条==============//水平位置doublePx_tt=Double.parseDouble(ship.getLon());//垂直位置doublePy_tt=Double.parseDouble(ship.getLat());double[][]Z_ARRAY={{Px_tt},{Py_tt}};矩阵Z=新矩阵（Z_ARRAY）；X.set(X_.plus(K.times(Z.minus(H.times(X_)))));//===============第五条==============P.set(I.minus(K.times(H)).times(P_));//==============数据清理===============doubleabs=Math.abs(Double.parseDouble(ship.getLat())-X.get().get(2,0));双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"+"========================================》================================================="+"\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(Exceptione){e.printStackTrace();}});//System.out.println("count:"+removeShipList.size());//data.removeAll(removeShipList);返回列表；}publicstaticListdataCleanGCJ02(Listdata){//时间间隔（timeinterval）doubleT=1.0/900;//经度方差（水平位置）doubleJDFX=5.8;//纬度的方差（垂直位置）doubleWDFX=1.3;//状态矩阵（初始值不重要，会自动修正）double[][]X_ARRAY={{Double.parseDouble(data.get(0).getLon())},{0},{Double.parseDouble(data.get(0).getLat())},{0}};AtomicReferenceX=newAtomicReference<>(newMatrix(X_ARRAY));//状态协方差矩阵（初始值不重要，会自动修正）double[][]P_ARRAY={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};AtomicReferenceP=newAtomicReference<>(newMatrix(P_ARRAY));//状态转换矩阵double[][]F_ARRAY={{1,T,0,0},{0,1,0,0},{0,0,1,T},{0,0,0,1}};矩阵F=新矩阵（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}};矩阵Q=新矩阵（Q_ARRAY）；//观测矩阵（观测经度纬度）double[][]H_ARRAY={{1,0,0,0},{0,0,1,0}};矩阵H=新矩阵（H_ARRAY）；//观测噪声方差（对角线为各维度的方差）double[][]R_ARRAY={{JDFX,0},{0,WDFX},};矩阵R=新矩阵（R_ARRAY）；//I维度为4double[][]I_ARRAY={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};MatrixI=newMatrix(I_ARRAY);//ListremoveShipList=newArrayList<>();AtomicIntegeri=newAtomicInteger();ListtgList=newArrayList<>();data.forEach(ship->{try{i.getAndIncrement();//===============第一个公式==============MatrixX_=F.times(X.get());//===============第二个公式==============MatrixP_=F.times(P.get()).times(F.transpose()).plus(Q);//===============3个公式===============矩阵K=P_.times(H.transpose()).times(H.times(P_).times(H.transpose()).plus(R).inverse());//===============第4条公式===============//水平位置设置双Px_tt=Double.parseDouble(ship.getLon());//垂直位置doublePy_tt=Double.parseDouble(ship.getLat());双[][]Z_ARRAY={{Px_tt},{Py_tt}};矩阵Z=新矩阵（Z_ARRAY）；X.set(X_.plus(K.times(Z.minus(H.times(X_)))));//===============第五条==============P.set(I.minus(K.times(H)).times(P_));//==============数据清理===============doubleabs=Math.abs(Double.parseDouble(ship.getLat())-X.get().get(2,0));双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"+"======================================》================================================="+"\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(Exceptione){e.printStackTrace();}});//System.out.println("count:"+removeShipList.size());//data.removeAll(removeShipList);返回列表；}jargov.nist.mathjama1.0.3
