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
This commit is contained in:
Luc Maisonobe
parent
d8e892a21f
commit
d099c7c343
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue