Apache
/
commons-math
mirror of https://github.com/apache/commons-math.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

MATH-485 

"KalmanFilter" and related classes updated by Thomas Neidhart according to
comments on JIRA.


git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@1138461 13f79535-47bb-0310-9956-ffa450edef68
This commit is contained in:
Gilles Sadowski 2011-06-22 13:42:39 +00:00
parent d778da77e0
commit a88ba6c1ca
7 changed files with 220 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/main/java/org/apache/commons/math/filter/DefaultMeasurementModel.java
View File

@ -27,35 +27,45 @@ import org.apache.commons.math.linear.RealMatrix;
*/
public class DefaultMeasurementModel implements MeasurementModel {
/**
* The measurement matrix, used to associate the measurement vector to the
* internal state estimation vector.
*/
private RealMatrix measurementMatrix;
/**
* The measurement noise covariance matrix.
*/
private RealMatrix measurementNoise;
/**
* Create a new {@link MeasurementModel}, taking double arrays as input
* parameters for the respective measurement matrix and noise.
*
* @param measurementMatrix
* @param measMatrix
* the measurement matrix
* @param measurementNoise
* @param measNoise
* the measurement noise matrix
*/
public DefaultMeasurementModel(final double[][] measurementMatrix,
final double[][] measurementNoise) {
this(new Array2DRowRealMatrix(measurementMatrix),
new Array2DRowRealMatrix(measurementNoise));
public DefaultMeasurementModel(final double[][] measMatrix,
final double[][] measNoise) {
this(new Array2DRowRealMatrix(measMatrix),
new Array2DRowRealMatrix(measNoise));
}
/**
* Create a new {@link MeasurementModel}, taking {@link RealMatrix} objects
* as input parameters for the respective measurement matrix and noise.
*
* @param measurementMatrix
* @param measurementNoise
* @param measMatrix
* the measurement matrix
* @param measNoise
* the measurement noise matrix
*/
public DefaultMeasurementModel(final RealMatrix measurementMatrix,
final RealMatrix measurementNoise) {
this.measurementMatrix = measurementMatrix;
this.measurementNoise = measurementNoise;
public DefaultMeasurementModel(final RealMatrix measMatrix,
final RealMatrix measNoise) {
this.measurementMatrix = measMatrix;
this.measurementNoise = measNoise;
}
/**

82
src/main/java/org/apache/commons/math/filter/DefaultProcessModel.java
View File

@ -28,20 +28,40 @@ import org.apache.commons.math.linear.RealVector;
* @version $Id$
*/
public class DefaultProcessModel implements ProcessModel {
/**
* The state transition matrix, used to advance the internal state
* estimation each time-step.
*/
private RealMatrix stateTransitionMatrix;
/**
* The control matrix, used to integrate a control input into the state
* estimation.
*/
private RealMatrix controlMatrix;
private RealMatrix processNoise;
private RealVector initialStateEstimate;
private RealMatrix initialErrorCovariance;
/**
* The process noise covariance matrix.
*/
private RealMatrix processNoiseCovMatrix;
/**
* The initial state estimation of the observed process.
*/
private RealVector initialStateEstimateVector;
/**
* The initial error covariance matrix of the observed process.
*/
private RealMatrix initialErrorCovMatrix;
/**
* Create a new {@link ProcessModel}, taking double arrays as input
* parameters.
*
* @param stateTransitionMatrix
* @param stateTransition
* the state transition matrix
* @param controlMatrix
* @param control
* the control matrix
* @param processNoise
* the process noise matrix
@ -50,15 +70,15 @@ public class DefaultProcessModel implements ProcessModel {
* @param initialErrorCovariance
* the initial error covariance matrix
*/
public DefaultProcessModel(final double[][] stateTransitionMatrix,
final double[][] controlMatrix, final double[][] processNoise,
public DefaultProcessModel(final double[][] stateTransition,
final double[][] control, final double[][] processNoise,
final double[] initialStateEstimate,
final double[][] initialErrorCovariance) {
this(new Array2DRowRealMatrix(stateTransitionMatrix),
new Array2DRowRealMatrix(controlMatrix),
new Array2DRowRealMatrix(processNoise), new ArrayRealVector(
initialStateEstimate), new Array2DRowRealMatrix(
initialErrorCovariance));
this(new Array2DRowRealMatrix(stateTransition),
new Array2DRowRealMatrix(control),
new Array2DRowRealMatrix(processNoise),
new ArrayRealVector(initialStateEstimate),
new Array2DRowRealMatrix(initialErrorCovariance));
}
/**
@ -66,17 +86,17 @@ public class DefaultProcessModel implements ProcessModel {
* parameters. The initial state estimate and error covariance are omitted
* and will be initialized by the {@link KalmanFilter} to default values.
*
* @param stateTransitionMatrix
* @param stateTransition
* the state transition matrix
* @param controlMatrix
* @param control
* the control matrix
* @param processNoise
* the process noise matrix
*/
public DefaultProcessModel(final double[][] stateTransitionMatrix,
final double[][] controlMatrix, final double[][] processNoise) {
this(new Array2DRowRealMatrix(stateTransitionMatrix),
new Array2DRowRealMatrix(controlMatrix),
public DefaultProcessModel(final double[][] stateTransition,
final double[][] control, final double[][] processNoise) {
this(new Array2DRowRealMatrix(stateTransition),
new Array2DRowRealMatrix(control),
new Array2DRowRealMatrix(processNoise), null, null);
}
@ -84,9 +104,9 @@ public class DefaultProcessModel implements ProcessModel {
* Create a new {@link ProcessModel}, taking double arrays as input
* parameters.
*
* @param stateTransitionMatrix
* @param stateTransition
* the state transition matrix
* @param controlMatrix
* @param control
* the control matrix
* @param processNoise
* the process noise matrix
@ -95,15 +115,15 @@ public class DefaultProcessModel implements ProcessModel {
* @param initialErrorCovariance
* the initial error covariance matrix
*/
public DefaultProcessModel(final RealMatrix stateTransitionMatrix,
final RealMatrix controlMatrix, final RealMatrix processNoise,
public DefaultProcessModel(final RealMatrix stateTransition,
final RealMatrix control, final RealMatrix processNoise,
final RealVector initialStateEstimate,
final RealMatrix initialErrorCovariance) {
this.stateTransitionMatrix = stateTransitionMatrix;
this.controlMatrix = controlMatrix;
this.processNoise = processNoise;
this.initialStateEstimate = initialStateEstimate;
this.initialErrorCovariance = initialErrorCovariance;
this.stateTransitionMatrix = stateTransition;
this.controlMatrix = control;
this.processNoiseCovMatrix = processNoise;
this.initialStateEstimateVector = initialStateEstimate;
this.initialErrorCovMatrix = initialErrorCovariance;
}
/**
@ -124,20 +144,20 @@ public class DefaultProcessModel implements ProcessModel {
* {@inheritDoc}
*/
public RealMatrix getProcessNoise() {
return processNoise;
return processNoiseCovMatrix;
}
/**
* {@inheritDoc}
*/
public RealVector getInitialStateEstimate() {
return initialStateEstimate;
return initialStateEstimateVector;
}
/**
* {@inheritDoc}
*/
public RealMatrix getInitialErrorCovariance() {
return initialErrorCovariance;
return initialErrorCovMatrix;
}
}

175
src/main/java/org/apache/commons/math/filter/KalmanFilter.java
View File

@ -17,7 +17,6 @@
package org.apache.commons.math.filter;
import org.apache.commons.math.exception.DimensionMismatchException;
import org.apache.commons.math.exception.NullArgumentException;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.ArrayRealVector;
import org.apache.commons.math.linear.CholeskyDecompositionImpl;
@ -27,27 +26,26 @@ import org.apache.commons.math.linear.MatrixUtils;
import org.apache.commons.math.linear.NonSquareMatrixException;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;
import org.apache.commons.math.linear.SingularMatrixException;
import org.apache.commons.math.util.MathUtils;
/**
* Implementation of a Kalman filter to estimate the state <i>x<sub>k</sub> of a
* discrete-time controlled process that is governed by the linear stochastic
* difference equation:
* Implementation of a Kalman filter to estimate the state <i>x<sub>k</sub></i>
* of a discrete-time controlled process that is governed by the linear
* stochastic difference equation:
*
* <pre>
* <i>x<sub>k</sub> = <b>A</b><i>x<sub>k-1</sub> + <b>B</b><i>u<sub>k-1</sub> + <i>w<sub>k-1</sub>
* <i>x<sub>k</sub></i> = <b>A</b><i>x<sub>k-1</sub></i> + <b>B</b><i>u<sub>k-1</sub></i> + <i>w<sub>k-1</sub></i>
* </pre>
*
* with a measurement <i>x<sub>k</sub> that is
* with a measurement <i>x<sub>k</sub></i> that is
*
* <pre>
* <i>z<sub>k</sub> = <b>H</b><i>x<sub>k</sub> + <i>v<sub>k</sub>.
* <i>z<sub>k</sub></i> = <b>H</b><i>x<sub>k</sub></i> + <i>v<sub>k</sub></i>.
* </pre>
*
* The random variables <i>w<sub>k</sub> and <i>v<sub>k</sub> represent the
* process and measurement noise and are assumed to be independent of each other
* and distributed with normal probability (white noise).
* The random variables <i>w<sub>k</sub></i> and <i>v<sub>k</sub></i> represent
* the process and measurement noise and are assumed to be independent of each
* other and distributed with normal probability (white noise).
* <p>
* The Kalman filter cycle involves the following steps:
* <ol>
@ -55,48 +53,64 @@ import org.apache.commons.math.util.MathUtils;
* <li>correct: adjust the projected estimate by an actual measurement</li>
* </ol>
* </p>
* <br/>
* <p>
* The Kalman filter is initialized with a {@link ProcessModel} and a
* {@link 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:
* <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>
*
* @see <a href="http://www.cs.unc.edu/~welch/kalman/">Kalman filter
* resources</a>
* @see <a href="http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf">An
* introduction to the Kalman filter by Greg Welch and Gary Bishop</a>
* @see <a
* href="http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf">Kalman
* filter example by Dan Simon</a>
*
* @see <a href="http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf">
* Kalman filter example by Dan Simon</a>
* @see ProcessModel
* @see MeasurementModel
* @version $Id$
*/
public class KalmanFilter {
/** Serializable version identifier. */
private static final long serialVersionUID = 4878026651422612760L;
/** The transition matrix, equivalent to A */
private transient RealMatrix transitionMatrix;
/** The transposed transition matrix */
private transient RealMatrix transitionMatrixT;
/** The control matrix, equivalent to B */
private transient RealMatrix controlMatrix;
/** The measurement matrix, equivalent to H */
private transient RealMatrix measurementMatrix;
/** The transposed measurement matrix */
private transient RealMatrix measurementMatrixT;
/** The internal state estimation vector, equivalent to x hat */
private transient RealVector stateEstimation;
/** The process noise covariance matrix, equivalent to Q */
private transient RealMatrix processNoise;
/** The measurement noise covariance matrix, equivalent to R */
private transient RealMatrix measurementNoise;
/** The error covariance matrix, equivalent to P */
private transient RealMatrix errorCovariance;
/** The process model used by this filter instance. */
private final ProcessModel processModel;
/** The measurement model used by this filter instance. */
private final MeasurementModel measurementModel;
/** The transition matrix, equivalent to A. */
private RealMatrix transitionMatrix;
/** The transposed transition matrix. */
private RealMatrix transitionMatrixT;
/** The control matrix, equivalent to B. */
private RealMatrix controlMatrix;
/** The measurement matrix, equivalent to H. */
private RealMatrix measurementMatrix;
/** The transposed measurement matrix. */
private RealMatrix measurementMatrixT;
/** The internal state estimation vector, equivalent to x hat. */
private RealVector stateEstimation;
/** The error covariance matrix, equivalent to P. */
private RealMatrix errorCovariance;
/**
* Creates a new Kalman filter with the given process and measurement
* models.
*
* @param processModel
* @param process
* the model defining the underlying process dynamics
* @param measurementModel
* @param measurement
* the model defining the given measurement characteristics
* @throws NullArgumentException
* @throws org.apache.commons.math.exception.NullArgumentException
* if any of the given inputs is null (except for the control
* matrix)
* @throws NonSquareMatrixException
@ -104,39 +118,46 @@ public class KalmanFilter {
* @throws MatrixDimensionMismatchException
* if the matrix dimensions do not fit together
*/
public KalmanFilter(final ProcessModel processModel,
final MeasurementModel measurementModel)
throws NullArgumentException, NonSquareMatrixException,
MatrixDimensionMismatchException {
public KalmanFilter(final ProcessModel process,
final MeasurementModel measurement) {
MathUtils.checkNotNull(processModel);
MathUtils.checkNotNull(measurementModel);
MathUtils.checkNotNull(process);
MathUtils.checkNotNull(measurement);
this.processModel = process;
this.measurementModel = measurement;
transitionMatrix = processModel.getStateTransitionMatrix();
MathUtils.checkNotNull(transitionMatrix);
transitionMatrixT = transitionMatrix.transpose();
// create an empty matrix if no control matrix was given
controlMatrix = (processModel.getControlMatrix() == null) ?
new Array2DRowRealMatrix() :
processModel.getControlMatrix();
if (processModel.getControlMatrix() == null) {
controlMatrix = new Array2DRowRealMatrix();
} else {
controlMatrix = processModel.getControlMatrix();
}
measurementMatrix = measurementModel.getMeasurementMatrix();
MathUtils.checkNotNull(measurementMatrix);
measurementMatrixT = measurementMatrix.transpose();
processNoise = processModel.getProcessNoise();
// check that the process and measurement noise matrices are not null
// they will be directly accessed from the model as they may change
// over time
RealMatrix processNoise = processModel.getProcessNoise();
MathUtils.checkNotNull(processNoise);
measurementNoise = measurementModel.getMeasurementNoise();
MathUtils.checkNotNull(measurementNoise);
RealMatrix measNoise = measurementModel.getMeasurementNoise();
MathUtils.checkNotNull(measNoise);
// set the initial state estimate to a zero vector if it is not
// available
stateEstimation = (processModel.getInitialStateEstimate() == null) ?
new ArrayRealVector(transitionMatrix.getColumnDimension()) :
processModel.getInitialStateEstimate();
MathUtils.checkNotNull(stateEstimation);
// available from the process model
if (processModel.getInitialStateEstimate() == null) {
stateEstimation =
new ArrayRealVector(transitionMatrix.getColumnDimension());
} else {
stateEstimation = processModel.getInitialStateEstimate();
}
if (transitionMatrix.getColumnDimension() != stateEstimation.getDimension()) {
throw new DimensionMismatchException(transitionMatrix.getColumnDimension(),
@ -144,10 +165,12 @@ public class KalmanFilter {
}
// initialize the error covariance to the process noise if it is not
// available
errorCovariance = (processModel.getInitialErrorCovariance() == null) ? processNoise
.copy() : processModel.getInitialErrorCovariance();
MathUtils.checkNotNull(errorCovariance);
// available from the process model
if (processModel.getInitialErrorCovariance() == null) {
errorCovariance = processNoise.copy();
} else {
errorCovariance = processModel.getInitialErrorCovariance();
}
// sanity checks, the control matrix B may be null
@ -181,10 +204,10 @@ public class KalmanFilter {
}
// row dimension of R must be equal to row dimension of H
if (measurementNoise.getRowDimension() != measurementMatrix.getRowDimension() ||
measurementNoise.getColumnDimension() != 1) {
throw new MatrixDimensionMismatchException(measurementNoise.getRowDimension(),
measurementNoise.getColumnDimension(),
if (measNoise.getRowDimension() != measurementMatrix.getRowDimension() ||
measNoise.getColumnDimension() != 1) {
throw new MatrixDimensionMismatchException(measNoise.getRowDimension(),
measNoise.getColumnDimension(),
measurementMatrix.getRowDimension(), 1);
}
}
@ -258,7 +281,7 @@ public class KalmanFilter {
* @throws DimensionMismatchException
* if the dimension of the control vector does not fit
*/
public void predict(final double[] u) throws DimensionMismatchException {
public void predict(final double[] u) {
predict(new ArrayRealVector(u));
}
@ -270,7 +293,7 @@ public class KalmanFilter {
* @throws DimensionMismatchException
* if the dimension of the control vector does not fit
*/
public void predict(final RealVector u) throws DimensionMismatchException {
public void predict(final RealVector u) {
// sanity checks
if (u != null &&
u.getDimension() != controlMatrix.getColumnDimension()) {
@ -290,7 +313,8 @@ public class KalmanFilter {
// project the error covariance ahead
// P(k)- = A * P(k-1) * A' + Q
errorCovariance = transitionMatrix.multiply(errorCovariance)
.multiply(transitionMatrixT).add(processNoise);
.multiply(transitionMatrixT)
.add(processModel.getProcessNoise());
}
/**
@ -300,11 +324,10 @@ public class KalmanFilter {
* the measurement vector
* @throws DimensionMismatchException
* if the dimension of the measurement vector does not fit
* @throws SingularMatrixException
* @throws org.apache.commons.math.linear.SingularMatrixException
* if the covariance matrix could not be inverted
*/
public void correct(final double[] z) throws DimensionMismatchException,
SingularMatrixException {
public void correct(final double[] z) {
correct(new ArrayRealVector(z));
}
@ -315,11 +338,10 @@ public class KalmanFilter {
* the measurement vector
* @throws DimensionMismatchException
* if the dimension of the measurement vector does not fit
* @throws SingularMatrixException
* @throws org.apache.commons.math.linear.SingularMatrixException
* if the covariance matrix could not be inverted
*/
public void correct(final RealVector z) throws DimensionMismatchException,
SingularMatrixException {
public void correct(final RealVector z) {
// sanity checks
if (z != null &&
z.getDimension() != measurementMatrix.getRowDimension()) {
@ -328,13 +350,14 @@ public class KalmanFilter {
}
// S = H * P(k) - * H' + R
RealMatrix S = measurementMatrix.multiply(errorCovariance)
.multiply(measurementMatrixT).add(measurementNoise);
RealMatrix s = measurementMatrix.multiply(errorCovariance)
.multiply(measurementMatrixT)
.add(measurementModel.getMeasurementNoise());
// invert S
// as the error covariance matrix is a symmetric positive
// semi-definite matrix, we can use the cholesky decomposition
DecompositionSolver solver = new CholeskyDecompositionImpl(S).getSolver();
DecompositionSolver solver = new CholeskyDecompositionImpl(s).getSolver();
RealMatrix invertedS = solver.getInverse();
// Inn = z(k) - H * xHat(k)-
@ -351,7 +374,7 @@ public class KalmanFilter {
// update covariance of prediction error
// P(k) = (I - K * H) * P(k)-
RealMatrix Identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
errorCovariance = Identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);
RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
errorCovariance = identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);
}
}

7
src/main/java/org/apache/commons/math/filter/MeasurementModel.java
View File

@ -32,9 +32,14 @@ public interface MeasurementModel {
RealMatrix getMeasurementMatrix();
/**
* Returns the measurement noise matrix.
* Returns the measurement noise matrix. This method is called by the
* {@link KalmanFilter} every correct step, so implementations of this
* interface may return a modified measurement noise depending on current
* iteration step.
*
* @return the measurement noise matrix
* @see KalmanFilter#correct(double[])
* @see KalmanFilter#correct(org.apache.commons.math.linear.RealVector)
*/
RealMatrix getMeasurementNoise();
}

8
src/main/java/org/apache/commons/math/filter/ProcessModel.java
View File

@ -40,9 +40,15 @@ public interface ProcessModel {
RealMatrix getControlMatrix();
/**
* Returns the process noise matrix.
* Returns the process noise matrix. This method is called by the
* {@link KalmanFilter} every predict step, so implementations of this
* interface may return a modified process noise depending on current
* iteration step.
*
* @return the process noise matrix
* @see KalmanFilter#predict()
* @see KalmanFilter#predict(double[])
* @see KalmanFilter#predict(RealVector)
*/
RealMatrix getProcessNoise();

19
src/main/java/org/apache/commons/math/filter/package.html Normal file
View File

@ -0,0 +1,19 @@
<html>
<!--
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.
-->
<body>Implementations of common discrete-time linear filters.</body>
</html>

29
src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
View File

@ -1,19 +1,17 @@
/*
* 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.
* 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.math.filter;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
@ -26,6 +24,11 @@ import org.apache.commons.math.util.MathUtils;
import org.junit.Assert;
import org.junit.Test;
/**
* Test for {@link KalmanFilter}.
*
* @version $Id$
*/
public class KalmanFilterTest {
@Test
public void testConstant() {