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