// 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);