I use the kalmanfilter implementation of the apache commons math library to improve the accuracy of my indoor positioning framework. I think I got the matrices setup correctly for 2D positioning while the state consists of the 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: Here is the output from my little JUnit test which calls the method estimatePosition() in a loop with the mocked position [20,20]:
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 is probably to set x to the initial state in your Kalman() constructor.
Practically speaking, when you initialize a Kalman filter, you will not always have an initial state that you know. In your own case, you happen to know that the initial position is 20,20, but what should you put in your initial velocity estimate?
One common starting point is to initialize to 0 (or whatever a reasonable mean value is) and set initial P to be "wide 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 with very large uncertainty. That would cause initial measurements to make large adjustments to x as P converges to the steady state after many measurements.