卡尔曼滤波器（自用）

public class KalmanFilter {
    //维度确定
    int mp;//测量向量维数
    int dp;//状态向量维数
    int cp;//控制向量维度

     //卡尔曼滤波核心步骤
    //预测
    /** 初始值
     * 初始估计值：取第一次的观测值
     * 初始的估计值与真实值之间的误差协方差矩阵：可以取主对角线上的值均为一个较小值（跟据经验自己定义）
     * */
    Matrix state_pre;//预测状态矩阵（预测过程），基于过去时刻的状态以及控制量对当前时刻的状态进行估计，注意，数据是先验的: x(k)=A*x(k-1)+B*u(k)  （1）
    Matrix error_cov_pre;//先验误差估计协方差， 前一部分是由上次最优估计值自身引入的协方差，后一部分是本次估计产生的不确定性，P'(k)=A*P(k-1)*At + Q)  （2）
    //校正
    Matrix gain;//卡尔曼增益，K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  （3）
    Matrix state_post;//最优的状态估计，x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  （4）
    Matrix error_cov_post;//最优的状态估计的协方差，计算本次最优状态估计值的协方差矩阵，这个数据在下一次迭代的公式2中被用到，P(k)=(I-K(k)*H)*P'(k)  （5）

    //卡尔曼滤波相关参数
    Matrix transition_matrix;//A 状态转换矩阵
    Matrix measurement_matrix;//H 状态测量矩阵
    Matrix control_matrix;//B 控制矩阵
    Matrix process_noise_cov;//Q  预测噪声协方差矩阵
    Matrix measurement_noise_cov;//R  测量噪声协方差矩阵

    public Matrix getState_pre() {
        return state_pre;
    }

    public void setState_pre(Matrix state_pre) {
        this.state_pre = state_pre;
    }

    public Matrix getError_cov_pre() {
        return error_cov_pre;
    }

    public void setError_cov_pre(Matrix error_cov_pre) {
        this.error_cov_pre = error_cov_pre;
    }

    public Matrix getGain() {
        return gain;
    }

    public void setGain(Matrix gain) {
        this.gain = gain;
    }

    public Matrix getState_post() {
        return state_post;
    }

    public void setState_post(Matrix state_post) {
        this.state_post = state_post;
    }

    public Matrix getError_cov_post() {
        return error_cov_post;
    }

    public void setError_cov_post(Matrix error_cov_post) {
        this.error_cov_post = error_cov_post;
    }

    public Matrix getTransition_matrix() {
        return transition_matrix;
    }

    public void setTransition_matrix(Matrix transition_matrix) {
        this.transition_matrix = transition_matrix;
    }

    public Matrix getMeasurement_matrix() {
        return measurement_matrix;
    }

    public void setMeasurement_matrix(Matrix measurement_matrix) {
        this.measurement_matrix = measurement_matrix;
    }

    public Matrix getControl_matrix() {
        return control_matrix;
    }

    public void setControl_matrix(Matrix control_matrix) {
        this.control_matrix = control_matrix;
    }

    public Matrix getProcess_noise_cov() {
        return process_noise_cov;
    }

    public void setProcess_noise_cov(Matrix process_noise_cov) {
        this.process_noise_cov = process_noise_cov;
    }

    public Matrix getMeasurement_noise_cov() {
        return measurement_noise_cov;
    }

    public void setMeasurement_noise_cov(Matrix measurement_noise_cov) {
        this.measurement_noise_cov = measurement_noise_cov;
    }

    /** 临时矩阵 */
    Matrix temp1;
    Matrix temp2;
    Matrix temp3;
    Matrix temp4;
    Matrix temp5;

    /*构造函数，初始化相关参数*/
    public KalmanFilter(int dynam_params, int measure_params,int control_params)
                        throws Exception {
        if( dynam_params <= 0 || measure_params <= 0 ) {//异常处理，维度不为0
                throw new IllegalArgumentException("Kalman filter: Illegal dimensions.");
        }
        if( control_params < 0 ) {
            control_params = dynam_params;
        }
            // 初始化
         dp = dynam_params;
         mp= measure_params;
         cp = control_params;
         state_pre = new Matrix(dp, 1); // 初始值定义为 A*state_post(第一次的观测值)
         state_post = new Matrix(dp, 1); // 初始值定义为第一次的观测值
         transition_matrix = Matrix.identity(dp, dp,1); //初始值一般设为单位矩阵
         process_noise_cov = Matrix.identity(dp, dp, 1e-3); // 初始值一般设为单位矩阵
         measurement_matrix = Matrix.identity(mp, dp,1); // 初始值一般设为单位矩阵
         measurement_noise_cov = Matrix.identity(mp, mp, 1e-1); //初始值一般设为单位矩阵
         error_cov_pre = new Matrix(dp, dp); // 在预测中初始化
         error_cov_post = Matrix.identity(dp, dp,1); //初始值一般设为单位矩阵
         gain = new Matrix(dp, mp);

        if( cp > 0 )
        {
            control_matrix = new Matrix(dp, cp);
        }
        else {
            control_matrix = null;
        }

        /** 临时矩阵 */
         temp1 = new Matrix(dp, dp);
         temp2 = new Matrix(mp, dp);
         temp3 = new Matrix(mp, mp);
         temp4 = new Matrix(mp, dp);
         temp5 = new Matrix(mp, 1);
    }

    public KalmanFilter(int dynam_params, int measure_params) throws Exception {//无控制
        this(dynam_params, measure_params, 0);
    }

    public Matrix Predict(Object o) {//无控制预测
        return Predict(null);
    }

    //预测模块
    public Matrix Predict(Matrix control) {

        //(1)
        state_pre = transition_matrix.times(state_post);//x'(k) = A*x(k)
        if( control != null && cp > 0 ) {
            state_pre = control_matrix.gemm(control, state_pre, 1, 1);// x'(k) = x'(k) + B*u(k)
        }

        // (2)
        temp1 = transition_matrix.times(error_cov_post);//temp1 = A*P(k)
        error_cov_pre = temp1.gemm(transition_matrix.transpose(), process_noise_cov, 1, 1);// P'(k) = temp1*At + Q

        return state_pre;
    }

    //校正模块
    public Matrix Correct(final Matrix measurement) {

        // (3) Compute the Kalman gain
        temp2 = measurement_matrix.times(error_cov_pre);// temp2 = H*P'(k)
        temp3 = temp2.gemm(measurement_matrix.transpose(), measurement_noise_cov, 1, 1);// temp3 = temp2*Ht + R
        temp4 = temp3.svd().getU().times(temp2);// temp4 = inv(temp3)*temp2 = Kt(k)
        gain = temp4.transpose();// K(k)

        // (4) Update estimate with measurement z(k)
        temp5 = measurement_matrix.gemm(state_pre, measurement, -1, 1);// temp5 = z(k) - H*x'(k)
        state_post = gain.gemm(temp5, state_pre, 1, 1);// x(k) = x'(k) + K(k)*temp5

        // (5) Update the error covariance.
        error_cov_post = gain.gemm(temp2, error_cov_pre, -1, 1);// P(k) = P'(k) - K(k)*temp2

        return state_post;
    }
}