Use the Apache Commons Kalman Filter to Evaluate 2D Positioning

Question

I want to improve the accuracy of my internal positioning structure and therefore apply kalmanfilter. I found that the apache commons math library supports Kalmanfilter, so I tried to use it and followed the tutorials: https://commons.apache.org/proper/commons-math/userguide/filter.html I think I configured the matrix correctly for 2D positioning, while the state consists of position and speed. My task is to evaluate the Position () method. How to get the correct pNoise and mNoise variable? And why should I point them out. I thought it was something for Q and R-matrices ... I help in every way!

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), . .

Answer 2

, Kalman math Kalman . , .

, u z - . :

public Position<Euclidean2D> esimatePosition(Measurement z){ 
        u = readControlInputFromSomeWhere();
        filter.predict(u);
        filter.correct(z);
        double pX = filter.getStateEstimation()[0];
        double pY = filter.getStateEstimation()[1];
        return new Position2D(pX, pY);
    }

http://commons.apache.org/proper/commons-math/userguide/filter.html Apache commons Paper