指定 Apache Commons Kalman Filter 2D Positioning Estimation 的起始位置

Posted 2023-02-21

【中文标题】指定 Apache Commons Kalman Filter 2D Positioning Estimation 的起始位置

【英文标题】：Specify Start Position for Apache Commons Kalman Filter 2D Positioning Estmation

【发布时间】：2016-01-17 09:01:39

【问题描述】：

我使用 apache commons 数学库的 kalmanfilter 实现来提高我的室内定位框架的准确性。我想我为 2D 定位正确设置了矩阵，而状态由位置（x，y）和速度（vx，vy）组成。我在“estimatePosition()”方法中使用新的传入位置设置状态“x”。过滤器似乎工作：这是我的小 JUnit 测试的输出，它在模拟位置 [20,20] 的循环中调用方法 estimatePosition()：

第一次递归：位置：20; 20 估计：0,0054987503； 0,0054987503 ... 第 100 次递归：位置：20; 20 估计：20,054973733;20,054973733

我想知道为什么初始位置似乎在 [0,0]。我必须在哪里设置 [20,20] 的初始位置？

public class Kalman 

    //A - state transition matrix
    private RealMatrix A;
    //B - control input matrix
    private RealMatrix B;
    //H - measurement matrix
    private RealMatrix H;
    //Q - process noise covariance matrix (error in the process)
    private RealMatrix Q;
    //R - measurement noise covariance matrix (error in the measurement)
    private RealMatrix R;
    //x state
    private RealVector x;

    // discrete time interval (100ms) between to steps
    private final double dt = 0.1d;
    // position measurement noise (1 meter)
    private final double measurementNoise = 1d;

    // constant control input, increase velocity by 0.1 m/s per cycle
    private RealVector u = new ArrayRealVector(new double[]  0.1d );
    //private RealVector u = new ArrayRealVector(new double[]  10d );
    private KalmanFilter filter;

    public Kalman()
        //A and B describe the physic model of the user moving specified as matrices
        A = new Array2DRowRealMatrix(new double[][] 
                                                         1d, 0d, dt, 0d ,
                                                         0d, 1d, 0d, dt ,
                                                         0d, 0d, 1d, 0d ,
                                                         0d, 0d, 0d, 1d 
                                                    );
        B = new Array2DRowRealMatrix(new double[][] 
                                                         Math.pow(dt, 2d) / 2d ,
                                                         Math.pow(dt, 2d) / 2d ,
                                                         dt,
                                                         dt 
                                                    );
        //only observe first 2 values - the position coordinates
        H = new Array2DRowRealMatrix(new double[][] 
                                                         1d, 0d, 0d, 0d ,
                                                         0d, 1d, 0d, 0d ,
                                                    );
        Q = new Array2DRowRealMatrix(new double[][] 
                                                         Math.pow(dt, 4d)/4d, 0d, Math.pow(dt, 3d)/2d, 0d ,
                                                         0d, Math.pow(dt, 4d)/4d, 0d, Math.pow(dt, 3d)/2d ,
                                                         Math.pow(dt, 3d)/2d, 0d, Math.pow(dt, 2d), 0d ,
                                                         0d, Math.pow(dt, 3d)/2d, 0d, Math.pow(dt, 2d) 
                                                    );

        R = new Array2DRowRealMatrix(new double[][] 
                 Math.pow(measurementNoise, 2d), 0d ,
                 0d, Math.pow(measurementNoise, 2d) 
        );

        ProcessModel pm = new DefaultProcessModel(A, B, Q, x, null);
        MeasurementModel mm = new DefaultMeasurementModel(H, R);
        filter = new KalmanFilter(pm, mm);
    


    /**
     * Use Kalmanfilter to decrease measurement errors
     * @param position
     * @return
     */
    public Position<Euclidean2D> esimatePosition(Position<Euclidean2D> position)

        double[] pos = position.toArray();
        // x = [ 0 0 0 0] state consists of position and velocity[pX, pY, vX, vY]
        x = new ArrayRealVector(new double[]  pos[0], pos[1], 0, 0 );

        // predict the state estimate one time-step ahead
        filter.predict(u);

        // x = A * x + B * u (state prediction)
        x = A.operate(x).add(B.operate(u));

        // z = H * x  (measurement prediction)
        RealVector z = H.operate(x);

        // correct the state estimate with the latest measurement
        filter.correct(z);

        //get the corrected state - the position
        double pX = filter.getStateEstimation()[0];
        double pY = filter.getStateEstimation()[1];

        return new Position2D(pX, pY);
    

【参考方案1】：

您问题的技术答案可能是在您的Kalman() 构造函数中将x 设置为初始状态。

实际上，当您初始化卡尔曼滤波器时，您并不总是有一个您知道的初始状态。在您自己的情况下，您碰巧知道初始位置是20,20，但是您应该在初始速度估计中输入什么？

一个常见的起点是初始化为0（或任何合理的平均值）并将初始P设置为“全开”。我看不到 P 在您的代码中是如何初始化的。你会设置它说你的初始位置是0,0，不确定性很大。这将导致初始测量对x 进行较大调整，因为P 在多次测量后会收敛到稳定状态。

【讨论】：

你是对的@Ben...再一次。我忘记在初始化卡尔曼滤波器之前设置 x！然后开始位置如预期的那样是[20,20]。此外，我将速度初始化为 0。使用 Apache Math 实现，我不必初始化 P，因为它们似乎提供了某种默认矩阵。