Use Apache Commons Kalman Filter for 2D Positioning Estmation

Question

I want to improve the accuracy of my indoor positioning framework and therefore apply the 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 got the matrices setup correctly for 2D positioning while the state consists of the position and velocity. My Problem lies in the Method estimatePosition(). How do I get the correct pNoise and mNoise variable? And why must i specify them. I thought this is what the Q and R matrices are for... I appreaciate every help!

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

The one dimensional car acceleration example provided in Apache commons math Kalman filter library is from this paper. That paper is programmer oriented and easy to follow to start programming.

In practice, u and z is from control and measure sensor data input during every iteration. so the estimatePosition will be like:

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

Answer 2

Those are part of their example simulation. They're generating test data by adding Gaussian noise (as pNoise and mNoise) to simulate real world conditions. In a real application you wouldn't add any noise to your real measurements.