Java—Specifies the starting position for the Apache Commons Kalman filter 2D positioning estimate

I use the KalmanFilter implementation of the Apache public math library to improve the accuracy of my indoor positioning framework. I guess I set the matrix correctly for 2D positioning, while the state consists of position (x,y) and velocity (vx, vy). I set the state “x” with the new incoming position in the “estimatePosition()” method. The filter seems to work: this is the output of my little JUnit test, which calls the method estimatePosition() and mock position [20,20]:, in a loop

• First recursion: Location: {20; 20}
  Estimate: {0,0054987503; 0,0054987503
• …
• 100th recursion: Location: {20; 20}
  estimate: {20,054973733; 20,054973733}

I wonder why the initial Position seems to be at [0,0]. Where do I have to set the initial position of [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);
    }
}

The technical answer to your question may be to set x to the initial state in your Kalman() constructor.

In fact, when you initialize a Kalman filter, you don’t always have a known initial state. In your own example, you happen to know that the initial position is 20,20, but what should you enter in the initial speed estimate?

A common starting point is to initialize to 0 (or any reasonable average) and set the initial P to “fully open”. I don’t see how P is initialized in your code. You would set it to say that your initial position is 0,0 very uncertain. This will result in a large adjustment of x to the initial measurement because P converges to a steady state after multiple measurements.