src/test/java/org/apache/commons/math3/filter/KalmanFilterTest.java - commons-math - Git at Google

 /*
  * Licensed to the Apache Software Foundation (ASF) under one or more
  * contributor license agreements. See the NOTICE file distributed with this
  * work for additional information regarding copyright ownership. The ASF
  * licenses this file to You under the Apache License, Version 2.0 (the
  * "License"); you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law
  * or agreed to in writing, software distributed under the License is
  * distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
  * KIND, either express or implied. See the License for the specific language
  * governing permissions and limitations under the License.
  */

 package org.apache.commons.math3.filter;

 import org.apache.commons.math3.distribution.NormalDistribution;
 import org.apache.commons.math3.linear.Array2DRowRealMatrix;
 import org.apache.commons.math3.linear.ArrayRealVector;
 import org.apache.commons.math3.linear.MatrixDimensionMismatchException;
 import org.apache.commons.math3.linear.MatrixUtils;
 import org.apache.commons.math3.linear.RealMatrix;
 import org.apache.commons.math3.linear.RealVector;
 import org.apache.commons.math3.random.JDKRandomGenerator;
 import org.apache.commons.math3.random.RandomGenerator;
 import org.apache.commons.math3.random.Well19937c;
 import org.apache.commons.math3.util.FastMath;
 import org.apache.commons.math3.util.Precision;
 import org.junit.Assert;
 import org.junit.Test;

 /**
  * Tests for {@link KalmanFilter}.
  *
  */
 public class KalmanFilterTest {

     @Test(expected=MatrixDimensionMismatchException.class)
     public void testTransitionMeasurementMatrixMismatch() {

         // A and H matrix do not match in dimensions

         // A = [ 1 ]
         RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
         // no control input
         RealMatrix B = null;
         // H = [ 1 1 ]
         RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d, 1d });
         // Q = [ 0 ]
         RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
         // R = [ 0 ]
         RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });

         ProcessModel pm
             = new DefaultProcessModel(A, B, Q,
                                       new ArrayRealVector(new double[] { 0 }), null);
         MeasurementModel mm = new DefaultMeasurementModel(H, R);
         new KalmanFilter(pm, mm);
         Assert.fail("transition and measurement matrix should not be compatible");
     }

     @Test(expected=MatrixDimensionMismatchException.class)
     public void testTransitionControlMatrixMismatch() {

         // A and B matrix do not match in dimensions

         // A = [ 1 ]
         RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
         // B = [ 1 1 ]
         RealMatrix B = new Array2DRowRealMatrix(new double[] { 1d, 1d });
         // H = [ 1 ]
         RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
         // Q = [ 0 ]
         RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
         // R = [ 0 ]
         RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });

         ProcessModel pm
             = new DefaultProcessModel(A, B, Q,
                                       new ArrayRealVector(new double[] { 0 }), null);
         MeasurementModel mm = new DefaultMeasurementModel(H, R);
         new KalmanFilter(pm, mm);
         Assert.fail("transition and control matrix should not be compatible");
     }

     @Test
     public void testConstant() {
         // simulates a simple process with a constant state and no control input

         double constantValue = 10d;
         double measurementNoise = 0.1d;
         double processNoise = 1e-5d;

         // A = [ 1 ]
         RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
         // no control input
         RealMatrix B = null;
         // H = [ 1 ]
         RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
         // x = [ 10 ]
         RealVector x = new ArrayRealVector(new double[] { constantValue });
         // Q = [ 1e-5 ]
         RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise });
         // R = [ 0.1 ]
         RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise });

         ProcessModel pm
             = new DefaultProcessModel(A, B, Q,
                                       new ArrayRealVector(new double[] { constantValue }), null);
         MeasurementModel mm = new DefaultMeasurementModel(H, R);
         KalmanFilter filter = new KalmanFilter(pm, mm);

         Assert.assertEquals(1, filter.getMeasurementDimension());
         Assert.assertEquals(1, filter.getStateDimension());

         assertMatrixEquals(Q.getData(), filter.getErrorCovariance());

         // check the initial state
         double[] expectedInitialState = new double[] { constantValue };
         assertVectorEquals(expectedInitialState, filter.getStateEstimation());

         RealVector pNoise = new ArrayRealVector(1);
         RealVector mNoise = new ArrayRealVector(1);

         RandomGenerator rand = new JDKRandomGenerator();
         // iterate 60 steps
         for (int i = 0; i < 60; i++) {
             filter.predict();

             // Simulate the process
             pNoise.setEntry(0, processNoise * rand.nextGaussian());

             // x = A * x + p_noise
             x = A.operate(x).add(pNoise);

             // Simulate the measurement
             mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

             // z = H * x + m_noise
             RealVector z = H.operate(x).add(mNoise);

             filter.correct(z);

             // state estimate shouldn't be larger than measurement noise
             double diff = FastMath.abs(constantValue - filter.getStateEstimation()[0]);
             // System.out.println(diff);
             Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
         }

         // error covariance should be already very low (< 0.02)
         Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
                                               0.02d, 1e-6) < 0);
     }

     @Test
     public void testConstantAcceleration() {
         // simulates a vehicle, accelerating at a constant rate (0.1 m/s)

         // discrete time interval
         double dt = 0.1d;
         // position measurement noise (meter)
         double measurementNoise = 10d;
         // acceleration noise (meter/sec^2)
         double accelNoise = 0.2d;

         // A = [ 1 dt ]
         //     [ 0  1 ]
         RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } });

         // B = [ dt^2/2 ]
         //     [ dt     ]
         RealMatrix B = new Array2DRowRealMatrix(
                 new double[][] { { FastMath.pow(dt, 2d) / 2d }, { dt } });

         // 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]);
             Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
         }

         // error covariance of the velocity should be already very low (< 0.1)
         Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
                                               0.1d, 1e-6) < 0);
     }

     /**
      * Represents an idealized Cannonball only taking into account gravity.
      */
     public static class Cannonball {

         private final double[] gravity = { 0, -9.81 };

         private final double[] velocity;
         private final double[] location;

         private double timeslice;

         public Cannonball(double timeslice, double angle, double initialVelocity) {
             this.timeslice = timeslice;

             final double angleInRadians = FastMath.toRadians(angle);
             this.velocity = new double[] {
                     initialVelocity * FastMath.cos(angleInRadians),
                     initialVelocity * FastMath.sin(angleInRadians)
             };

             this.location = new double[] { 0, 0 };
         }

         public double getX() {
             return location[0];
         }

         public double getY() {
             return location[1];
         }

         public double getXVelocity() {
             return velocity[0];
         }

         public double getYVelocity() {
             return velocity[1];
         }

         public void step() {
             // break gravitational force into a smaller time slice.
             double[] slicedGravity = gravity.clone();
             for ( int i = 0; i < slicedGravity.length; i++ ) {
                 slicedGravity[i] *= timeslice;
             }

             // apply the acceleration to velocity.
             double[] slicedVelocity = velocity.clone();
             for ( int i = 0; i < velocity.length; i++ ) {
                 velocity[i] += slicedGravity[i];
                 slicedVelocity[i] = velocity[i] * timeslice;
                 location[i] += slicedVelocity[i];
             }

             // cannonballs shouldn't go into the ground.
             if ( location[1] < 0 ) {
                 location[1] = 0;
             }
         }
     }

     @Test
     public void testCannonball() {
         // simulates the flight of a cannonball (only taking gravity and initial thrust into account)

         // number of iterations
         final int iterations = 144;
         // discrete time interval
         final double dt = 0.1d;
         // position measurement noise (meter)
         final double measurementNoise = 30d;
         // the initial velocity of the cannonball
         final double initialVelocity = 100;
         // shooting angle
         final double angle = 45;

         final Cannonball cannonball = new Cannonball(dt, angle, initialVelocity);

         final double speedX = cannonball.getXVelocity();
         final double speedY = cannonball.getYVelocity();

         // A = [ 1, dt, 0,  0 ]  =>  x(n+1) = x(n) + vx(n)
         //     [ 0,  1, 0,  0 ]  => vx(n+1) =        vx(n)
         //     [ 0,  0, 1, dt ]  =>  y(n+1) =              y(n) + vy(n)
         //     [ 0,  0, 0,  1 ]  => vy(n+1) =                     vy(n)
         final RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
                 { 1, dt, 0,  0 },
                 { 0,  1, 0,  0 },
                 { 0,  0, 1, dt },
                 { 0,  0, 0,  1 }
         });

         // The control vector, which adds acceleration to the kinematic equations.
         // 0          =>  x(n+1) =  x(n+1)
         // 0          => vx(n+1) = vx(n+1)
         // -9.81*dt^2 =>  y(n+1) =  y(n+1) - 1/2 * 9.81 * dt^2
         // -9.81*dt   => vy(n+1) = vy(n+1) - 9.81 * dt
         final RealVector controlVector =
                 MatrixUtils.createRealVector(new double[] { 0, 0, 0.5 * -9.81 * dt * dt, -9.81 * dt } );

         // The control matrix B only expects y and vy, see control vector
         final RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
                 { 0, 0, 0, 0 },
                 { 0, 0, 0, 0 },
                 { 0, 0, 1, 0 },
                 { 0, 0, 0, 1 }
         });

         // We only observe the x/y position of the cannonball
         final RealMatrix H = MatrixUtils.createRealMatrix(new double[][] {
                 { 1, 0, 0, 0 },
                 { 0, 0, 0, 0 },
                 { 0, 0, 1, 0 },
                 { 0, 0, 0, 0 }
         });

         // our guess of the initial state.
         final RealVector initialState = MatrixUtils.createRealVector(new double[] { 0, speedX, 0, speedY } );

         // the initial error covariance matrix, the variance = noise^2
         final double var = measurementNoise * measurementNoise;
         final RealMatrix initialErrorCovariance = MatrixUtils.createRealMatrix(new double[][] {
                 { var,    0,   0,    0 },
                 {   0, 1e-3,   0,    0 },
                 {   0,    0, var,    0 },
                 {   0,    0,   0, 1e-3 }
         });

         // we assume no process noise -> zero matrix
         final RealMatrix Q = MatrixUtils.createRealMatrix(4, 4);

         // the measurement covariance matrix
         final RealMatrix R = MatrixUtils.createRealMatrix(new double[][] {
                 { var,    0,   0,    0 },
                 {   0, 1e-3,   0,    0 },
                 {   0,    0, var,    0 },
                 {   0,    0,   0, 1e-3 }
         });

         final ProcessModel pm = new DefaultProcessModel(A, B, Q, initialState, initialErrorCovariance);
         final MeasurementModel mm = new DefaultMeasurementModel(H, R);
         final KalmanFilter filter = new KalmanFilter(pm, mm);

         final RandomGenerator rng = new Well19937c(1000);
         final NormalDistribution dist = new NormalDistribution(rng, 0, measurementNoise);

         for (int i = 0; i < iterations; i++) {
             // get the "real" cannonball position
             double x = cannonball.getX();
             double y = cannonball.getY();

             // apply measurement noise to current cannonball position
             double nx = x + dist.sample();
             double ny = y + dist.sample();

             cannonball.step();

             filter.predict(controlVector);
             // correct the filter with our measurements
             filter.correct(new double[] { nx, 0, ny, 0 } );

             // state estimate shouldn't be larger than the measurement noise
             double diff = FastMath.abs(cannonball.getY() - filter.getStateEstimation()[2]);
             Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
         }

         // error covariance of the x/y-position should be already very low (< 3m std dev = 9 variance)

         Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
                                               9, 1e-6) < 0);

         Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[2][2],
                                               9, 1e-6) < 0);
     }

     private void assertVectorEquals(double[] expected, double[] result) {
         Assert.assertEquals("Wrong number of rows.", expected.length,
                             result.length);
         for (int i = 0; i < expected.length; i++) {
             Assert.assertEquals("Wrong value at position [" + i + "]",
                                 expected[i], result[i], 1.0e-6);
         }
     }

     private void assertMatrixEquals(double[][] expected, double[][] result) {
         Assert.assertEquals("Wrong number of rows.", expected.length,
                             result.length);
         for (int i = 0; i < expected.length; i++) {
             Assert.assertEquals("Wrong number of columns.", expected[i].length,
                                 result[i].length);
             for (int j = 0; j < expected[i].length; j++) {
                 Assert.assertEquals("Wrong value at position [" + i + "," + j
                                     + "]", expected[i][j], result[i][j], 1.0e-6);
             }
         }
     }
 }
	/*
	* Licensed to the Apache Software Foundation (ASF) under one or more
	* contributor license agreements. See the NOTICE file distributed with this
	* work for additional information regarding copyright ownership. The ASF
	* licenses this file to You under the Apache License, Version 2.0 (the
	* "License"); you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	* http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law
	* or agreed to in writing, software distributed under the License is
	* distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
	* KIND, either express or implied. See the License for the specific language
	* governing permissions and limitations under the License.
	*/

	package org.apache.commons.math3.filter;

	import org.apache.commons.math3.distribution.NormalDistribution;
	import org.apache.commons.math3.linear.Array2DRowRealMatrix;
	import org.apache.commons.math3.linear.ArrayRealVector;
	import org.apache.commons.math3.linear.MatrixDimensionMismatchException;
	import org.apache.commons.math3.linear.MatrixUtils;
	import org.apache.commons.math3.linear.RealMatrix;
	import org.apache.commons.math3.linear.RealVector;
	import org.apache.commons.math3.random.JDKRandomGenerator;
	import org.apache.commons.math3.random.RandomGenerator;
	import org.apache.commons.math3.random.Well19937c;
	import org.apache.commons.math3.util.FastMath;
	import org.apache.commons.math3.util.Precision;
	import org.junit.Assert;
	import org.junit.Test;

	/**
	* Tests for {@link KalmanFilter}.
	*
	*/
	public class KalmanFilterTest {

	@Test(expected=MatrixDimensionMismatchException.class)
	public void testTransitionMeasurementMatrixMismatch() {

	// A and H matrix do not match in dimensions

	// A = [ 1 ]
	RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
	// no control input
	RealMatrix B = null;
	// H = [ 1 1 ]
	RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d, 1d });
	// Q = [ 0 ]
	RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
	// R = [ 0 ]
	RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });

	ProcessModel pm
	= new DefaultProcessModel(A, B, Q,
	new ArrayRealVector(new double[] { 0 }), null);
	MeasurementModel mm = new DefaultMeasurementModel(H, R);
	new KalmanFilter(pm, mm);
	Assert.fail("transition and measurement matrix should not be compatible");
	}

	@Test(expected=MatrixDimensionMismatchException.class)
	public void testTransitionControlMatrixMismatch() {

	// A and B matrix do not match in dimensions

	// A = [ 1 ]
	RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
	// B = [ 1 1 ]
	RealMatrix B = new Array2DRowRealMatrix(new double[] { 1d, 1d });
	// H = [ 1 ]
	RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
	// Q = [ 0 ]
	RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
	// R = [ 0 ]
	RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });

	ProcessModel pm
	= new DefaultProcessModel(A, B, Q,
	new ArrayRealVector(new double[] { 0 }), null);
	MeasurementModel mm = new DefaultMeasurementModel(H, R);
	new KalmanFilter(pm, mm);
	Assert.fail("transition and control matrix should not be compatible");
	}

	@Test
	public void testConstant() {
	// simulates a simple process with a constant state and no control input

	double constantValue = 10d;
	double measurementNoise = 0.1d;
	double processNoise = 1e-5d;

	// A = [ 1 ]
	RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
	// no control input
	RealMatrix B = null;
	// H = [ 1 ]
	RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
	// x = [ 10 ]
	RealVector x = new ArrayRealVector(new double[] { constantValue });
	// Q = [ 1e-5 ]
	RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise });
	// R = [ 0.1 ]
	RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise });

	ProcessModel pm
	= new DefaultProcessModel(A, B, Q,
	new ArrayRealVector(new double[] { constantValue }), null);
	MeasurementModel mm = new DefaultMeasurementModel(H, R);
	KalmanFilter filter = new KalmanFilter(pm, mm);

	Assert.assertEquals(1, filter.getMeasurementDimension());
	Assert.assertEquals(1, filter.getStateDimension());

	assertMatrixEquals(Q.getData(), filter.getErrorCovariance());

	// check the initial state
	double[] expectedInitialState = new double[] { constantValue };
	assertVectorEquals(expectedInitialState, filter.getStateEstimation());

	RealVector pNoise = new ArrayRealVector(1);
	RealVector mNoise = new ArrayRealVector(1);

	RandomGenerator rand = new JDKRandomGenerator();
	// iterate 60 steps
	for (int i = 0; i < 60; i++) {
	filter.predict();

	// Simulate the process
	pNoise.setEntry(0, processNoise * rand.nextGaussian());

	// x = A * x + p_noise
	x = A.operate(x).add(pNoise);

	// Simulate the measurement
	mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

	// z = H * x + m_noise
	RealVector z = H.operate(x).add(mNoise);

	filter.correct(z);

	// state estimate shouldn't be larger than measurement noise
	double diff = FastMath.abs(constantValue - filter.getStateEstimation()[0]);
	// System.out.println(diff);
	Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
	}

	// error covariance should be already very low (< 0.02)
	Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
	0.02d, 1e-6) < 0);
	}

	@Test
	public void testConstantAcceleration() {
	// simulates a vehicle, accelerating at a constant rate (0.1 m/s)

	// discrete time interval
	double dt = 0.1d;
	// position measurement noise (meter)
	double measurementNoise = 10d;
	// acceleration noise (meter/sec^2)
	double accelNoise = 0.2d;

	// A = [ 1 dt ]
	// [ 0 1 ]
	RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } });

	// B = [ dt^2/2 ]
	// [ dt ]
	RealMatrix B = new Array2DRowRealMatrix(
	new double[][] { { FastMath.pow(dt, 2d) / 2d }, { dt } });

	// 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]);
	Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
	}

	// error covariance of the velocity should be already very low (< 0.1)
	Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
	0.1d, 1e-6) < 0);
	}

	/**
	* Represents an idealized Cannonball only taking into account gravity.
	*/
	public static class Cannonball {

	private final double[] gravity = { 0, -9.81 };

	private final double[] velocity;
	private final double[] location;

	private double timeslice;

	public Cannonball(double timeslice, double angle, double initialVelocity) {
	this.timeslice = timeslice;

	final double angleInRadians = FastMath.toRadians(angle);
	this.velocity = new double[] {
	initialVelocity * FastMath.cos(angleInRadians),
	initialVelocity * FastMath.sin(angleInRadians)
	};

	this.location = new double[] { 0, 0 };
	}

	public double getX() {
	return location[0];
	}

	public double getY() {
	return location[1];
	}

	public double getXVelocity() {
	return velocity[0];
	}

	public double getYVelocity() {
	return velocity[1];
	}

	public void step() {
	// break gravitational force into a smaller time slice.
	double[] slicedGravity = gravity.clone();
	for ( int i = 0; i < slicedGravity.length; i++ ) {
	slicedGravity[i] *= timeslice;
	}

	// apply the acceleration to velocity.
	double[] slicedVelocity = velocity.clone();
	for ( int i = 0; i < velocity.length; i++ ) {
	velocity[i] += slicedGravity[i];
	slicedVelocity[i] = velocity[i] * timeslice;
	location[i] += slicedVelocity[i];
	}

	// cannonballs shouldn't go into the ground.
	if ( location[1] < 0 ) {
	location[1] = 0;
	}
	}
	}

	@Test
	public void testCannonball() {
	// simulates the flight of a cannonball (only taking gravity and initial thrust into account)

	// number of iterations
	final int iterations = 144;
	// discrete time interval
	final double dt = 0.1d;
	// position measurement noise (meter)
	final double measurementNoise = 30d;
	// the initial velocity of the cannonball
	final double initialVelocity = 100;
	// shooting angle
	final double angle = 45;

	final Cannonball cannonball = new Cannonball(dt, angle, initialVelocity);

	final double speedX = cannonball.getXVelocity();
	final double speedY = cannonball.getYVelocity();

	// A = [ 1, dt, 0, 0 ] => x(n+1) = x(n) + vx(n)
	// [ 0, 1, 0, 0 ] => vx(n+1) = vx(n)
	// [ 0, 0, 1, dt ] => y(n+1) = y(n) + vy(n)
	// [ 0, 0, 0, 1 ] => vy(n+1) = vy(n)
	final RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
	{ 1, dt, 0, 0 },
	{ 0, 1, 0, 0 },
	{ 0, 0, 1, dt },
	{ 0, 0, 0, 1 }
	});

	// The control vector, which adds acceleration to the kinematic equations.
	// 0 => x(n+1) = x(n+1)
	// 0 => vx(n+1) = vx(n+1)
	// -9.81dt^2 => y(n+1) = y(n+1) - 1/2 9.81 * dt^2
	// -9.81dt => vy(n+1) = vy(n+1) - 9.81 dt
	final RealVector controlVector =
	MatrixUtils.createRealVector(new double[] { 0, 0, 0.5 * -9.81 * dt * dt, -9.81 * dt } );

	// The control matrix B only expects y and vy, see control vector
	final RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
	{ 0, 0, 0, 0 },
	{ 0, 0, 0, 0 },
	{ 0, 0, 1, 0 },
	{ 0, 0, 0, 1 }
	});

	// We only observe the x/y position of the cannonball
	final RealMatrix H = MatrixUtils.createRealMatrix(new double[][] {
	{ 1, 0, 0, 0 },
	{ 0, 0, 0, 0 },
	{ 0, 0, 1, 0 },
	{ 0, 0, 0, 0 }
	});

	// our guess of the initial state.
	final RealVector initialState = MatrixUtils.createRealVector(new double[] { 0, speedX, 0, speedY } );

	// the initial error covariance matrix, the variance = noise^2
	final double var = measurementNoise * measurementNoise;
	final RealMatrix initialErrorCovariance = MatrixUtils.createRealMatrix(new double[][] {
	{ var, 0, 0, 0 },
	{ 0, 1e-3, 0, 0 },
	{ 0, 0, var, 0 },
	{ 0, 0, 0, 1e-3 }
	});

	// we assume no process noise -> zero matrix
	final RealMatrix Q = MatrixUtils.createRealMatrix(4, 4);

	// the measurement covariance matrix
	final RealMatrix R = MatrixUtils.createRealMatrix(new double[][] {
	{ var, 0, 0, 0 },
	{ 0, 1e-3, 0, 0 },
	{ 0, 0, var, 0 },
	{ 0, 0, 0, 1e-3 }
	});

	final ProcessModel pm = new DefaultProcessModel(A, B, Q, initialState, initialErrorCovariance);
	final MeasurementModel mm = new DefaultMeasurementModel(H, R);
	final KalmanFilter filter = new KalmanFilter(pm, mm);

	final RandomGenerator rng = new Well19937c(1000);
	final NormalDistribution dist = new NormalDistribution(rng, 0, measurementNoise);

	for (int i = 0; i < iterations; i++) {
	// get the "real" cannonball position
	double x = cannonball.getX();
	double y = cannonball.getY();

	// apply measurement noise to current cannonball position
	double nx = x + dist.sample();
	double ny = y + dist.sample();

	cannonball.step();

	filter.predict(controlVector);
	// correct the filter with our measurements
	filter.correct(new double[] { nx, 0, ny, 0 } );

	// state estimate shouldn't be larger than the measurement noise
	double diff = FastMath.abs(cannonball.getY() - filter.getStateEstimation()[2]);
	Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
	}

	// error covariance of the x/y-position should be already very low (< 3m std dev = 9 variance)

	Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
	9, 1e-6) < 0);

	Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[2][2],
	9, 1e-6) < 0);
	}

	private void assertVectorEquals(double[] expected, double[] result) {
	Assert.assertEquals("Wrong number of rows.", expected.length,
	result.length);
	for (int i = 0; i < expected.length; i++) {
	Assert.assertEquals("Wrong value at position [" + i + "]",
	expected[i], result[i], 1.0e-6);
	}
	}

	private void assertMatrixEquals(double[][] expected, double[][] result) {
	Assert.assertEquals("Wrong number of rows.", expected.length,
	result.length);
	for (int i = 0; i < expected.length; i++) {
	Assert.assertEquals("Wrong number of columns.", expected[i].length,
	result[i].length);
	for (int j = 0; j < expected[i].length; j++) {
	Assert.assertEquals("Wrong value at position [" + i + "," + j
	+ "]", expected[i][j], result[i][j], 1.0e-6);
	}
	}
	}
	}