// H = [ 1 0 ]
RealMatrix H = new Array2DRowRealMatrix(new double[][] { { 1d, 0d } });
// x = [ 0 0 ]
RealVector x = new ArrayRealVector(new double[] { 0, 0 });
RealMatrix tmp = new Array2DRowRealMatrix(
new double[][] { { FastMath.pow(dt, 4d) / 4d, FastMath.pow(dt, 3d) / 2d },
{ FastMath.pow(dt, 3d) / 2d, FastMath.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
// [ dt^3/2 dt^2 ]
RealMatrix Q = tmp.scalarMultiply(FastMath.pow(accelNoise, 2));
// P0 = [ 1 1 ]
// [ 1 1 ]
RealMatrix P0 = new Array2DRowRealMatrix(new double[][] { { 1, 1 }, { 1, 1 } });
// R = [ measurementNoise^2 ]
RealMatrix R = new Array2DRowRealMatrix(
new double[] { FastMath.pow(measurementNoise, 2) });
// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });
ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0);
MeasurementModel mm = new DefaultMeasurementModel(H, R);
KalmanFilter filter = new KalmanFilter(pm, mm);
Assert.assertEquals(1, filter.getMeasurementDimension());
Assert.assertEquals(2, filter.getStateDimension());
assertMatrixEquals(P0.getData(), filter.getErrorCovariance());
// check the initial state
double[] expectedInitialState = new double[] { 0.0, 0.0 };
assertVectorEquals(expectedInitialState, filter.getStateEstimation());
RandomGenerator rand = new JDKRandomGenerator();
RealVector tmpPNoise = new ArrayRealVector(
new double[] { FastMath.pow(dt, 2d) / 2d, dt });
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);
// Simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());
// x = A * x + B * u + pNoise
x = A.operate(x).add(B.operate(u)).add(pNoise);
// Simulate the measurement
double mNoise = measurementNoise * rand.nextGaussian();
// z = H * x + m_noise
RealVector z = H.operate(x).mapAdd(mNoise);
filter.correct(z);
// state estimate shouldn't be larger than the measurement noise
double diff = FastMath.abs(x.getEntry(0) - filter.getStateEstimation()[0]);