差分定位使用EKF常加速度模型，在三维状态向量中添加测站到地心距离作为约束条件的JAVA代码

以下是使用EKF常加速度模型，在三维状态向量中添加测站到地心距离作为约束条件的JAVA代码：

public class EKF_Differential_Localization {
    private double[][] F; //状态转移矩阵
    private double[][] Q; //过程噪声协方差矩阵
    private double[][] H; //观测矩阵
    private double[][] R; //观测噪声协方差矩阵
    private double[][] P; //状态估计协方差矩阵
    private double[] x; //状态向量
    private double[] z; //观测值
    private double[] r; //测站到地心距离
    private int n; //状态向量维数
    private int m; //观测向量维数

    public EKF_Differential_Localization(double[][] F, double[][] Q, double[][] H, double[][] R, double[][] P, double[] x, double[] z, double[] r) {
        this.F = F;
        this.Q = Q;
        this.H = H;
        this.R = R;
        this.P = P;
        this.x = x;
        this.z = z;
        this.r = r;
        this.n = x.length;
        this.m = z.length;
    }

    public void predict() {
        x = Matrix.multiply(F, x);
        P = Matrix.add(Matrix.multiply(F, Matrix.multiply(P, Matrix.transpose(F))), Q);
    }

    public void correct() {
        double[][] K = Matrix.multiply(P, Matrix.multiply(Matrix.transpose(H), Matrix.inverse(Matrix.add(Matrix.multiply(H, Matrix.multiply(P, Matrix.transpose(H))), R))));
        double[] hx = new double[m];
        for (int i = 0; i < m; i++) {
            hx[i] = Math.sqrt(Math.pow(x[0] - z[3 * i], 2) + Math.pow(x[1] - z[3 * i + 1], 2) + Math.pow(x[2] - z[3 * i + 2], 2))) - r[i];
        }
        x = Matrix.add(x, Matrix.multiply(K, hx));
        P = Matrix.multiply(Matrix.subtract(Matrix.identity(n), Matrix.multiply(K, H)), P);
    }

    public void run() {
        predict();
        correct();
    }
}

与前一个问题的代码相比，唯一的区别在于观测矩阵和差分观测向量被替换为测站到地心距离向量 `r`，并且在校正时不需要再减去差分观测值 `dz`。在使用时，需要先初始化以上参数，然后使用 `run()` 方法进行预测和校正。