Author: luc
Date: Tue Nov 29 19:36:52 2011
New Revision: 1208043
URL: http://svn.apache.org/viewvc?rev=1208043&view=rev
Log:
Updated tests and documentation for Kalman filter.
Patch provided by Thomas Neidhart.
Added:
commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml (with props)
Modified:
commons/proper/math/trunk/src/site/site.xml
commons/proper/math/trunk/src/site/xdoc/userguide/index.xml
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
Modified: commons/proper/math/trunk/src/site/site.xml
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/site.xml?rev=1208043&r1=1208042&r2=1208043&view=diff
==============================================================================
--- commons/proper/math/trunk/src/site/site.xml (original)
+++ commons/proper/math/trunk/src/site/site.xml Tue Nov 29 19:36:52 2011
@@ -65,6 +65,7 @@
<item name="Optimization"
href="/userguide/optimization.html"/>
<item name="Ordinary Differential Equations" href="/userguide/ode.html"/>
<item name="Genetic Algorithms" href="/userguide/genetics.html"/>
+ <item name="Filters" href="/userguide/filter.html"/>
</menu>
</body>
Added: commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml?rev=1208043&view=auto
==============================================================================
--- commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml (added)
+++ commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml Tue Nov 29
19:36:52 2011
@@ -0,0 +1,227 @@
+<?xml version="1.0"?>
+
+<!--
+ 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.
+ -->
+
+<?xml-stylesheet type="text/xsl" href="./xdoc.xsl"?>
+<!-- $Revision: 937893 $ $Date: 2010-04-25 23:42:47 +0200 (Sun, 25 Apr 2010) $
-->
+<document url="filter.html">
+ <properties>
+ <title>The Commons Math User Guide - Filters</title>
+ </properties>
+ <body>
+ <section name="15 Filters">
+ <subsection name="15.1 Overview" href="overview">
+ <p>
+ The filter package currently provides only an implementation of a
Kalman filter.
+ </p>
+ </subsection>
+ <subsection name="15.2 Kalman Filter" href="kalman">
+ <p>
+ <a
href="../apidocs/org/apache/commons/math/filter/KalmanFilter.html">
+ KalmanFilter</a> provides a discrete-time filter to estimate
+ a stochastic linear process.</p>
+
+ <p>A Kalman filter is initialized with a <a
href="../apidocs/org/apache/commons/math/filter/ProcessModel.html">
+ ProcessModel</a> and a <a
href="../apidocs/org/apache/commons/math/filter/MeasurementModel.html">
+ MeasurementModel</a>, 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:
+ <ul>
+ <li>A - state transition matrix</li>
+ <li>B - control input matrix</li>
+ <li>H - measurement matrix</li>
+ <li>Q - process noise covariance matrix</li>
+ <li>R - measurement noise covariance matrix</li>
+ <li>P - error covariance matrix</li>
+ </ul>
+ </p>
+ <p>
+ <dl>
+ <dt>Initialization</dt>
+ <dd> 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.
+ <source>
+// 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);
+ </source>
+ </dd>
+ <dt>Iteration</dt>
+ <dd>The following code illustrates how to perform the
predict/correct cycle:
+ <source>
+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
+}
+ </source>
+ </dd>
+ <dt>Constant Voltage Example</dt>
+ <dd>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.
+ <source>
+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];
+}
+ </source>
+ </dd>
+ <dt>Increasing Speed Vehicle Example</dt>
+ <dd>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.
+ <source>
+// 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];
+}
+ </source>
+ </dd>
+ </dl>
+ </p>
+ </subsection>
+ </section>
+ </body>
+</document>
Propchange: commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml
------------------------------------------------------------------------------
svn:eol-style = native
Propchange: commons/proper/math/trunk/src/site/xdoc/userguide/filter.xml
------------------------------------------------------------------------------
svn:keywords = "Author Date Id Revision"
Modified: commons/proper/math/trunk/src/site/xdoc/userguide/index.xml
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/xdoc/userguide/index.xml?rev=1208043&r1=1208042&r2=1208043&view=diff
==============================================================================
--- commons/proper/math/trunk/src/site/xdoc/userguide/index.xml (original)
+++ commons/proper/math/trunk/src/site/xdoc/userguide/index.xml Tue Nov 29
19:36:52 2011
@@ -142,6 +142,11 @@
<li><a href="genetics.html#a14.2_GA_Framework">14.2 GA
Framework</a></li>
<li><a href="genetics.html#a14.3_Implementation">14.3
Implementation and Examples</a></li>
</ul></li>
+ <li><a href="filter.html">15. Filters</a>
+ <ul>
+ <li><a href="filter.html#a15.1_Overview">15.1 Overview</a></li>
+ <li><a href="filter.html#a15.2_Kalman_Filter">15.2 Kalman
Filter</a></li>
+ </ul></li>
</ul>
</section>
Modified:
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java?rev=1208043&r1=1208042&r2=1208043&view=diff
==============================================================================
---
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
(original)
+++
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
Tue Nov 29 19:36:52 2011
@@ -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);
}
}
}
