KalmanFilter卡尔曼滤波对GPS轨迹数据清洗(JAVA实现)

1. 首先分析状态变量X(t)，由于GPS位移和速度都分为水平和垂直水平
   
2. 接着分析状态偏移矩阵
   
   接着将上述式子转化成矩阵
   
3. 分析观测矩阵
   
   将状态量带入得
   

package com.zz.meridian.KalmanFilter4;

import Jama.Matrix;

import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.AtomicReference;

public class KalmanGpsFilter4 {
    public static void main(String[] args) {
        List data=new ArrayList<>();
        data.add(new ShipTrajectory(1.,2.));
        data.add(new ShipTrajectory(3.,4.));
        data.add(new ShipTrajectory(5.,6.));
        dataClean(data);
    }

    static class ShipTrajectory{
        double JD;
        double WD;

        public ShipTrajectory(double JD, double WD) {
            this.JD = JD;
            this.WD = WD;
        }

        public double getJD() {
            return JD;
        }

        public void setJD(double JD) {
            this.JD = JD;
        }

        public double getWD() {
            return WD;
        }

        public void setWD(double WD) {
            this.WD = WD;
        }
    }
    //数据清洗-卡尔曼滤波对GPS轨迹数据清洗
    public static List  dataClean(List data){
        //时间间隔(时间间隔)
        double T = 1.0/900;
        //经度方差(水平位置)
        double JDFX = 5.8;
        //纬度方差(垂直位置)
        double WDFX = 1.3;
        //状态矩阵(初始值不重要,会自动更正)
        double[][] X_ARRAY = {
                {data.get(0).getJD()},
                {0},
                {data.get(0).getWD()},
                {0}
        };
        AtomicReference 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 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 removeShipList = new ArrayList<>();
        AtomicInteger i= new AtomicInteger();
        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=ship.getJD();
                //垂直位置
                double Py_tt=ship.getWD();
                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(ship.getWD() - X.get().get(2, 0));
                double abs1 = Math.abs(ship.getJD() - X.get().get(0, 0));
                //清除噪声
                if(abs1>=0.8||abs>=0.8){
                    removeShipList.add(ship);
                    System.out.println();
                    System.out.println("33[32;4m" + "======================================《《filter start》》 ===================================================" + "33[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("33[32;4m" + "=============================================================================================================" + "33[0m");
                    System.out.println();
                }else {
                    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 data;
    }
}