java - 使用 Apache Commons 卡尔曼滤波器进行 2D 定位估计

Question

我想提高我的室内定位框架的准确性，因此应用了卡尔曼滤波器。我发现 apache commons 数学库支持 Kalmanfilter，所以我尝试使用它并按照教程进行操作:https://commons.apache.org/proper/commons-math/userguide/filter.html我想我为 2D 定位正确设置了矩阵，而状态由位置和速度组成。我的问题在于方法 estimatePosition()。如何获得正确的 pNoise 和 mNoise 变量？为什么我必须指定它们。我认为这就是 Q 和 R 矩阵的用途......我感谢您的帮助!

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;
    //PO - error covariance matrix
    private RealMatrix PO;
    //x state
    private RealVector x;

    // discrete time interval (100ms) between to steps
    private final double dt = 0.1d;
    // position measurement noise (10 meter)
    private final double measurementNoise = 10d;
    // acceleration noise (meter/sec^2)
    private final double accelNoise = 0.2d;
    // constant control input, increase velocity by 0.1 m/s per cycle [vX, vY]
    private RealVector u = new ArrayRealVector(new double[] { 0.1d, 0.1d });
    private RealVector tmpPNoise = new ArrayRealVector(new double[] { Math.pow(dt, 2d) / 2d, dt });
    private RealVector mNoise = new ArrayRealVector(1);
    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) }
        });

        /*PO = new Array2DRowRealMatrix(new double[][] {
                                                        { 1d, 1d, 1d, 1d },
                                                        { 1d, 1d, 1d, 1d },
                                                        { 1d, 1d, 1d, 1d },
                                                        { 1d, 1d, 1d, 1d }
                                                     });*/

        // x = [ 0 0 0 0] state consists of position and velocity[pX, pY, vX, vY]
        //TODO: inititate with map center?
        x = new ArrayRealVector(new double[] { 0, 0, 0, 0 });

        ProcessModel pm = new DefaultProcessModel(A, B, Q, x, PO);
        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){
        RandomGenerator rand = new JDKRandomGenerator();

        double[] pos = position.toArray();
        // predict the state estimate one time-step ahead
        filter.predict(u);

        // noise of the process
        RealVector  pNoise = tmpPNoise.mapMultiply(accelNoise * pos[0]);

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

        // noise of the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise (measurement prediction)
        RealVector z = H.operate(x).add(mNoise);

        // 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);
    }
}

Answer 1

这些是他们的示例模拟的一部分。他们通过添加高斯噪声(如 pNoise 和 mNoise)来模拟真实世界条件来生成测试数据。在实际应用中，您不会在实际测量中添加任何噪声。