关于java:卡尔曼滤波算法

6次阅读

共计 5796 个字符，预计需要花费 15 分钟才能阅读完成。

前言

发现做动态轨迹纠偏还是有点用的，记录一下

实现

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;
    }
正文完
java
发表至： java
2022-03-22
 0
关于java:发愁没项目用Java开发一个停车场系统附源码
Java版-数据结构-队列（数组队列）
关于java:微信红包业务为什么采用轮询算法
关于java:RocketMQ是是如何管理消费进度的又是如何保证消息成功消费的

站内搜索

-「」