From d099c7c3436bd43094e7b3087c64e0552cfbec5b Mon Sep 17 00:00:00 2001 From: Luc Maisonobe Date: Tue, 29 Nov 2011 19:36:52 +0000 Subject: [PATCH] Updated tests and documentation for Kalman filter. Patch provided by Thomas Neidhart. git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@1208043 13f79535-47bb-0310-9956-ffa450edef68 --- src/site/site.xml | 1 + src/site/xdoc/userguide/filter.xml | 227 ++++++++++++++++++ src/site/xdoc/userguide/index.xml | 5 + .../commons/math/filter/KalmanFilterTest.java | 153 +++++++++++- 4 files changed, 382 insertions(+), 4 deletions(-) create mode 100644 src/site/xdoc/userguide/filter.xml diff --git a/src/site/site.xml b/src/site/site.xml index 1b9620108..32bd54da8 100644 --- a/src/site/site.xml +++ b/src/site/site.xml @@ -65,6 +65,7 @@ + diff --git a/src/site/xdoc/userguide/filter.xml b/src/site/xdoc/userguide/filter.xml new file mode 100644 index 000000000..0637941c5 --- /dev/null +++ b/src/site/xdoc/userguide/filter.xml @@ -0,0 +1,227 @@ + + + + + + + + + The Commons Math User Guide - Filters + + +
+ +

+ The filter package currently provides only an implementation of a Kalman filter. +

+ + +

+ + KalmanFilter provides a discrete-time filter to estimate + a stochastic linear process.

+ +

A Kalman filter is initialized with a + ProcessModel and a + MeasurementModel, which contain the corresponding transformation and noise covariance matrices. + The parameter names used in the respective models correspond to the following names commonly used + in the mathematical literature: +

    +
  • A - state transition matrix
    • +
  • B - control input matrix
    • +
  • H - measurement matrix
    • +
  • Q - process noise covariance matrix
    • +
  • R - measurement noise covariance matrix
    • +
  • P - error covariance matrix
    • +
+

+

+

+
Initialization
+
The following code will create a Kalman filter using the provided + DefaultMeasurementModel and DefaultProcessModel classes. To support dynamically changing + process and measurement noises, simply implement your own models. + +// A = [ 1 ] +RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d }); +// no control input +RealMatrix B = null; +// 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); +KalmanFilter filter = new KalmanFilter(pm, mm); + +
+
Iteration
+
The following code illustrates how to perform the predict/correct cycle: + +for (;;) { + // predict the state estimate one time-step ahead + // optionally provide some control input + filter.predict(); + + // obtain measurement vector z + RealVector z = getMeasurement(); + + // correct the state estimate with the latest measurement + filter.correct(z); + + double[] stateEstimate = filter.getStateEstimation(); + // do something with it +} + +
+
Constant Voltage Example
+
The following example creates a Kalman filter for a static process: a system with a + constant voltage as internal state. We observe this process with an artificially + imposed measurement noise of 0.1V and assume an internal process noise of 1e-5V. + +double constantVoltage = 10d; +double measurementNoise = 0.1d; +double processNoise = 1e-5d; + +// A = [ 1 ] +RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d }); +// B = null +RealMatrix B = null; +// H = [ 1 ] +RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d }); +// x = [ 10 ] +RealVector x = new ArrayRealVector(new double[] { constantVoltage }); +// Q = [ 1e-5 ] +RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise }); +// P = [ 1 ] +RealMatrix P0 = new Array2DRowRealMatrix(new double[] { 1d }); +// R = [ 0.1 ] +RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise }); + +ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0); +MeasurementModel mm = new DefaultMeasurementModel(H, R); +KalmanFilter filter = new KalmanFilter(pm, mm); + +// process and measurement noise vectors +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); + + double voltage = filter.getStateEstimation()[0]; +} + +
+
Increasing Speed Vehicle Example
+
The following example creates a Kalman filter for a simple linear process: a + vehicle driving along a street with a velocity increasing at a constant rate. The process + state is modeled as (position, velocity) and we only observe the position. A measurement + noise of 10m is imposed on the simulated measurement. + +// 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[][] { { Math.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[][] { + { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d }, + { Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } }); +// Q = [ dt^4/4 dt^3/2 ] +// [ dt^3/2 dt^2 ] +RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.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); + +RandomGenerator rand = new JDKRandomGenerator(); + +RealVector tmpPNoise = new ArrayRealVector(new double[] { Math.pow(dt, 2d) / 2d, dt }); +RealVector mNoise = new ArrayRealVector(1); + +// 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 + mNoise.setEntry(0, measurementNoise * rand.nextGaussian()); + + // z = H * x + m_noise + RealVector z = H.operate(x).add(mNoise); + + filter.correct(z); + + double position = filter.getStateEstimation()[0]; + double velocity = filter.getStateEstimation()[1]; +} + +
+
+

+ +
+ + diff --git a/src/site/xdoc/userguide/index.xml b/src/site/xdoc/userguide/index.xml index 2a441693d..e1ca0e689 100644 --- a/src/site/xdoc/userguide/index.xml +++ b/src/site/xdoc/userguide/index.xml @@ -142,6 +142,11 @@
  • 14.2 GA Framework
  • 14.3 Implementation and Examples
    • +
  • 15. Filters +
    • diff --git a/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java b/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java index b2108824b..a47372438 100644 --- a/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java +++ b/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java @@ -16,6 +16,7 @@ package org.apache.commons.math.filter; import org.apache.commons.math.linear.Array2DRowRealMatrix; import org.apache.commons.math.linear.ArrayRealVector; +import org.apache.commons.math.linear.MatrixDimensionMismatchException; import org.apache.commons.math.linear.RealMatrix; import org.apache.commons.math.linear.RealVector; import org.apache.commons.math.random.JDKRandomGenerator; @@ -25,13 +26,64 @@ import org.junit.Assert; import org.junit.Test; /** - * Test for {@link KalmanFilter}. + * Tests for {@link KalmanFilter}. * * @version $Id$ */ 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; @@ -86,7 +138,7 @@ public class KalmanFilterTest { filter.correct(z); - // state estimate should be larger than measurement noise + // state estimate shouldn't be larger than measurement noise double diff = Math.abs(constantValue - filter.getStateEstimation()[0]); // System.out.println(diff); Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0); @@ -97,12 +149,105 @@ public class KalmanFilterTest { 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[][] { { Math.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[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d }, + { Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } }); + + // Q = [ dt^4/4 dt^3/2 ] + // [ dt^3/2 dt^2 ] + RealMatrix Q = tmp.scalarMultiply(Math.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[] { Math.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[] { Math.pow(dt, 2d) / 2d, dt }); + + RealVector mNoise = new ArrayRealVector(1); + + // 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 + 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 the measurement noise + double diff = Math.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); + } + 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-15); + expected[i], result[i], 1.0e-6); } } @@ -114,7 +259,7 @@ public class KalmanFilterTest { 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-15); + + "]", expected[i][j], result[i][j], 1.0e-6); } } }