Author: erans
Date: Sat Jun 11 21:39:26 2011
New Revision: 1134779
URL: http://svn.apache.org/viewvc?rev=1134779&view=rev
Log:
MATH-485
New "filter" package. Initial implementation of Kalman filter provided
by Thomas Neidhart.
Added:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultMeasurementModel.java
(with props)
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultProcessModel.java
(with props)
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/KalmanFilter.java
(with props)
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/MeasurementModel.java
(with props)
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/ProcessModel.java
(with props)
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
(with props)
Modified:
commons/proper/math/trunk/src/site/xdoc/changes.xml
Added:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultMeasurementModel.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultMeasurementModel.java?rev=1134779&view=auto
==============================================================================
---
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultMeasurementModel.java
(added)
+++
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultMeasurementModel.java
Sat Jun 11 21:39:26 2011
@@ -0,0 +1,74 @@
+/*
+ * 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.math.filter;
+
+import org.apache.commons.math.linear.Array2DRowRealMatrix;
+import org.apache.commons.math.linear.RealMatrix;
+
+/**
+ * Default implementation of a {@link MeasurementModel} for the use with a
+ * {@link KalmanFilter}.
+ *
+ * @version $Id$
+ */
+public class DefaultMeasurementModel implements MeasurementModel {
+
+ private RealMatrix measurementMatrix;
+ private RealMatrix measurementNoise;
+
+ /**
+ * Create a new {@link MeasurementModel}, taking double arrays as input
+ * parameters for the respective measurement matrix and noise.
+ *
+ * @param measurementMatrix
+ * the measurement matrix
+ * @param measurementNoise
+ * the measurement noise matrix
+ */
+ public DefaultMeasurementModel(final double[][] measurementMatrix,
+ final double[][] measurementNoise) {
+ this(new Array2DRowRealMatrix(measurementMatrix),
+ new Array2DRowRealMatrix(measurementNoise));
+ }
+
+ /**
+ * Create a new {@link MeasurementModel}, taking {@link RealMatrix} objects
+ * as input parameters for the respective measurement matrix and noise.
+ *
+ * @param measurementMatrix
+ * @param measurementNoise
+ */
+ public DefaultMeasurementModel(final RealMatrix measurementMatrix,
+ final RealMatrix measurementNoise) {
+ this.measurementMatrix = measurementMatrix;
+ this.measurementNoise = measurementNoise;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public RealMatrix getMeasurementMatrix() {
+ return measurementMatrix;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public RealMatrix getMeasurementNoise() {
+ return measurementNoise;
+ }
+}
Propchange:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultMeasurementModel.java
------------------------------------------------------------------------------
svn:eol-style = native
Added:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultProcessModel.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultProcessModel.java?rev=1134779&view=auto
==============================================================================
---
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultProcessModel.java
(added)
+++
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultProcessModel.java
Sat Jun 11 21:39:26 2011
@@ -0,0 +1,143 @@
+/*
+ * 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.math.filter;
+
+import org.apache.commons.math.linear.Array2DRowRealMatrix;
+import org.apache.commons.math.linear.ArrayRealVector;
+import org.apache.commons.math.linear.RealMatrix;
+import org.apache.commons.math.linear.RealVector;
+
+/**
+ * Default implementation of a {@link ProcessModel} for the use with a
+ * {@link KalmanFilter}.
+ *
+ * @version $Id$
+ */
+public class DefaultProcessModel implements ProcessModel {
+
+ private RealMatrix stateTransitionMatrix;
+ private RealMatrix controlMatrix;
+ private RealMatrix processNoise;
+ private RealVector initialStateEstimate;
+ private RealMatrix initialErrorCovariance;
+
+ /**
+ * Create a new {@link ProcessModel}, taking double arrays as input
+ * parameters.
+ *
+ * @param stateTransitionMatrix
+ * the state transition matrix
+ * @param controlMatrix
+ * the control matrix
+ * @param processNoise
+ * the process noise matrix
+ * @param initialStateEstimate
+ * the initial state estimate vector
+ * @param initialErrorCovariance
+ * the initial error covariance matrix
+ */
+ public DefaultProcessModel(final double[][] stateTransitionMatrix,
+ final double[][] controlMatrix, 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));
+ }
+
+ /**
+ * Create a new {@link ProcessModel}, taking double arrays as input
+ * parameters. The initial state estimate and error covariance are omitted
+ * and will be initialized by the {@link KalmanFilter} to default values.
+ *
+ * @param stateTransitionMatrix
+ * the state transition matrix
+ * @param controlMatrix
+ * 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),
+ new Array2DRowRealMatrix(processNoise), null, null);
+ }
+
+ /**
+ * Create a new {@link ProcessModel}, taking double arrays as input
+ * parameters.
+ *
+ * @param stateTransitionMatrix
+ * the state transition matrix
+ * @param controlMatrix
+ * the control matrix
+ * @param processNoise
+ * the process noise matrix
+ * @param initialStateEstimate
+ * the initial state estimate vector
+ * @param initialErrorCovariance
+ * the initial error covariance matrix
+ */
+ public DefaultProcessModel(final RealMatrix stateTransitionMatrix,
+ final RealMatrix controlMatrix, final RealMatrix processNoise,
+ final RealVector initialStateEstimate,
+ final RealMatrix initialErrorCovariance) {
+ this.stateTransitionMatrix = stateTransitionMatrix;
+ this.controlMatrix = controlMatrix;
+ this.processNoise = processNoise;
+ this.initialStateEstimate = initialStateEstimate;
+ this.initialErrorCovariance = initialErrorCovariance;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public RealMatrix getStateTransitionMatrix() {
+ return stateTransitionMatrix;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public RealMatrix getControlMatrix() {
+ return controlMatrix;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public RealMatrix getProcessNoise() {
+ return processNoise;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public RealVector getInitialStateEstimate() {
+ return initialStateEstimate;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ public RealMatrix getInitialErrorCovariance() {
+ return initialErrorCovariance;
+ }
+}
Propchange:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/DefaultProcessModel.java
------------------------------------------------------------------------------
svn:eol-style = native
Added:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/KalmanFilter.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/KalmanFilter.java?rev=1134779&view=auto
==============================================================================
---
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/KalmanFilter.java
(added)
+++
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/KalmanFilter.java
Sat Jun 11 21:39:26 2011
@@ -0,0 +1,357 @@
+/*
+ * 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.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;
+import org.apache.commons.math.linear.DecompositionSolver;
+import org.apache.commons.math.linear.MatrixDimensionMismatchException;
+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:
+ *
+ * <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>
+ * </pre>
+ *
+ * with a measurement <i>x<sub>k</sub> that is
+ *
+ * <pre>
+ * <i>z<sub>k</sub> = <b>H</b><i>x<sub>k</sub> + <i>v<sub>k</sub>.
+ * </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).
+ * <p>
+ * The Kalman filter cycle involves the following steps:
+ * <ol>
+ * <li>predict: project the current state estimate ahead in time</li>
+ * <li>correct: adjust the projected estimate by an actual measurement</li>
+ * </ol>
+ * </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>
+ *
+ * @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;
+
+ /**
+ * Creates a new Kalman filter with the given process and measurement
+ * models.
+ *
+ * @param processModel
+ * the model defining the underlying process dynamics
+ * @param measurementModel
+ * the model defining the given measurement characteristics
+ * @throws NullArgumentException
+ * if any of the given inputs is null (except for the control
+ * matrix)
+ * @throws NonSquareMatrixException
+ * if the transition matrix is non square
+ * @throws MatrixDimensionMismatchException
+ * if the matrix dimensions do not fit together
+ */
+ public KalmanFilter(final ProcessModel processModel,
+ final MeasurementModel measurementModel)
+ throws NullArgumentException, NonSquareMatrixException,
+ MatrixDimensionMismatchException {
+
+ MathUtils.checkNotNull(processModel);
+ MathUtils.checkNotNull(measurementModel);
+
+ 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();
+
+ measurementMatrix = measurementModel.getMeasurementMatrix();
+ MathUtils.checkNotNull(measurementMatrix);
+ measurementMatrixT = measurementMatrix.transpose();
+
+ processNoise = processModel.getProcessNoise();
+ MathUtils.checkNotNull(processNoise);
+
+ measurementNoise = measurementModel.getMeasurementNoise();
+ MathUtils.checkNotNull(measurementNoise);
+
+ // 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);
+
+ if (transitionMatrix.getColumnDimension() !=
stateEstimation.getDimension()) {
+ throw new
DimensionMismatchException(transitionMatrix.getColumnDimension(),
+
stateEstimation.getDimension());
+ }
+
+ // initialize the error covariance to the process noise if it is not
+ // available
+ errorCovariance = (processModel.getInitialErrorCovariance() == null) ?
processNoise
+ .copy() : processModel.getInitialErrorCovariance();
+ MathUtils.checkNotNull(errorCovariance);
+
+ // sanity checks, the control matrix B may be null
+
+ // A must be a square matrix
+ if (!transitionMatrix.isSquare()) {
+ throw new NonSquareMatrixException(
+ transitionMatrix.getRowDimension(),
+ transitionMatrix.getColumnDimension());
+ }
+
+ // row dimension of B must be equal to A
+ if (controlMatrix != null &&
+ controlMatrix.getRowDimension() > 0 &&
+ controlMatrix.getColumnDimension() > 0 &&
+ (controlMatrix.getRowDimension() !=
transitionMatrix.getRowDimension() ||
+ controlMatrix.getColumnDimension() != 1)) {
+ throw new
MatrixDimensionMismatchException(controlMatrix.getRowDimension(),
+
controlMatrix.getColumnDimension(),
+
transitionMatrix.getRowDimension(), 1);
+ }
+
+ // Q must be equal to A
+ MatrixUtils.checkAdditionCompatible(transitionMatrix, processNoise);
+
+ // column dimension of H must be equal to row dimension of A
+ if (measurementMatrix.getColumnDimension() !=
transitionMatrix.getRowDimension()) {
+ throw new
MatrixDimensionMismatchException(measurementMatrix.getRowDimension(),
+
measurementMatrix.getColumnDimension(),
+
measurementMatrix.getRowDimension(),
+
transitionMatrix.getRowDimension());
+ }
+
+ // 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(),
+
measurementMatrix.getRowDimension(), 1);
+ }
+ }
+
+ /**
+ * Returns the dimension of the state estimation vector.
+ *
+ * @return the state dimension
+ */
+ public int getStateDimension() {
+ return stateEstimation.getDimension();
+ }
+
+ /**
+ * Returns the dimension of the measurement vector.
+ *
+ * @return the measurement vector dimension
+ */
+ public int getMeasurementDimension() {
+ return measurementMatrix.getRowDimension();
+ }
+
+ /**
+ * Returns the current state estimation vector.
+ *
+ * @return the state estimation vector
+ */
+ public double[] getStateEstimation() {
+ return stateEstimation.getData();
+ }
+
+ /**
+ * Returns a copy of the current state estimation vector.
+ *
+ * @return the state estimation vector
+ */
+ public RealVector getStateEstimationVector() {
+ return stateEstimation.copy();
+ }
+
+ /**
+ * Returns the current error covariance matrix.
+ *
+ * @return the error covariance matrix
+ */
+ public double[][] getErrorCovariance() {
+ return errorCovariance.getData();
+ }
+
+ /**
+ * Returns a copy of the current error covariance matrix.
+ *
+ * @return the error covariance matrix
+ */
+ public RealMatrix getErrorCovarianceMatrix() {
+ return errorCovariance.copy();
+ }
+
+ /**
+ * Predict the internal state estimation one time step ahead.
+ */
+ public void predict() {
+ predict((RealVector) null);
+ }
+
+ /**
+ * Predict the internal state estimation one time step ahead.
+ *
+ * @param u
+ * the control vector
+ * @throws DimensionMismatchException
+ * if the dimension of the control vector does not fit
+ */
+ public void predict(final double[] u) throws DimensionMismatchException {
+ predict(new ArrayRealVector(u));
+ }
+
+ /**
+ * Predict the internal state estimation one time step ahead.
+ *
+ * @param u
+ * the control vector
+ * @throws DimensionMismatchException
+ * if the dimension of the control vector does not fit
+ */
+ public void predict(final RealVector u) throws DimensionMismatchException {
+ // sanity checks
+ if (u != null &&
+ u.getDimension() != controlMatrix.getColumnDimension()) {
+ throw new DimensionMismatchException(u.getDimension(),
+
controlMatrix.getColumnDimension());
+ }
+
+ // project the state estimation ahead (a priori state)
+ // xHat(k)- = A * xHat(k-1) + B * u(k-1)
+ stateEstimation = transitionMatrix.operate(stateEstimation);
+
+ // add control input if it is available
+ if (u != null) {
+ stateEstimation = stateEstimation.add(controlMatrix.operate(u));
+ }
+
+ // project the error covariance ahead
+ // P(k)- = A * P(k-1) * A' + Q
+ errorCovariance = transitionMatrix.multiply(errorCovariance)
+ .multiply(transitionMatrixT).add(processNoise);
+ }
+
+ /**
+ * Correct the current state estimate with an actual measurement.
+ *
+ * @param z
+ * the measurement vector
+ * @throws DimensionMismatchException
+ * if the dimension of the measurement vector does not fit
+ * @throws SingularMatrixException
+ * if the covariance matrix could not be inverted
+ */
+ public void correct(final double[] z) throws DimensionMismatchException,
+ SingularMatrixException {
+ correct(new ArrayRealVector(z));
+ }
+
+ /**
+ * Correct the current state estimate with an actual measurement.
+ *
+ * @param z
+ * the measurement vector
+ * @throws DimensionMismatchException
+ * if the dimension of the measurement vector does not fit
+ * @throws SingularMatrixException
+ * if the covariance matrix could not be inverted
+ */
+ public void correct(final RealVector z) throws DimensionMismatchException,
+ SingularMatrixException {
+ // sanity checks
+ if (z != null &&
+ z.getDimension() != measurementMatrix.getRowDimension()) {
+ throw new DimensionMismatchException(z.getDimension(),
+
measurementMatrix.getRowDimension());
+ }
+
+ // S = H * P(k) - * H' + R
+ RealMatrix S = measurementMatrix.multiply(errorCovariance)
+ .multiply(measurementMatrixT).add(measurementNoise);
+
+ // 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();
+ RealMatrix invertedS = solver.getInverse();
+
+ // Inn = z(k) - H * xHat(k)-
+ RealVector innovation =
z.subtract(measurementMatrix.operate(stateEstimation));
+
+ // calculate gain matrix
+ // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1
+ // K(k) = P(k)- * H' * S^-1
+ RealMatrix kalmanGain =
errorCovariance.multiply(measurementMatrixT).multiply(invertedS);
+
+ // update estimate with measurement z(k)
+ // xHat(k) = xHat(k)- + K * Inn
+ stateEstimation = stateEstimation.add(kalmanGain.operate(innovation));
+
+ // 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);
+ }
+}
Propchange:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/KalmanFilter.java
------------------------------------------------------------------------------
svn:eol-style = native
Added:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/MeasurementModel.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/MeasurementModel.java?rev=1134779&view=auto
==============================================================================
---
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/MeasurementModel.java
(added)
+++
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/MeasurementModel.java
Sat Jun 11 21:39:26 2011
@@ -0,0 +1,40 @@
+/*
+ * 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.math.filter;
+
+import org.apache.commons.math.linear.RealMatrix;
+
+/**
+ * Defines the measurement model for the use with a {@link KalmanFilter}.
+ *
+ * @version $Id$
+ */
+public interface MeasurementModel {
+ /**
+ * Returns the measurement matrix.
+ *
+ * @return the measurement matrix
+ */
+ RealMatrix getMeasurementMatrix();
+
+ /**
+ * Returns the measurement noise matrix.
+ *
+ * @return the measurement noise matrix
+ */
+ RealMatrix getMeasurementNoise();
+}
Propchange:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/MeasurementModel.java
------------------------------------------------------------------------------
svn:eol-style = native
Added:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/ProcessModel.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/ProcessModel.java?rev=1134779&view=auto
==============================================================================
---
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/ProcessModel.java
(added)
+++
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/ProcessModel.java
Sat Jun 11 21:39:26 2011
@@ -0,0 +1,70 @@
+/*
+ * 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.math.filter;
+
+import org.apache.commons.math.linear.RealMatrix;
+import org.apache.commons.math.linear.RealVector;
+
+/**
+ * Defines the process dynamics model for the use with a {@link KalmanFilter}.
+ *
+ * @version $Id$
+ */
+public interface ProcessModel {
+ /**
+ * Returns the state transition matrix.
+ *
+ * @return the state transition matrix
+ */
+ RealMatrix getStateTransitionMatrix();
+
+ /**
+ * Returns the control matrix.
+ *
+ * @return the control matrix
+ */
+ RealMatrix getControlMatrix();
+
+ /**
+ * Returns the process noise matrix.
+ *
+ * @return the process noise matrix
+ */
+ RealMatrix getProcessNoise();
+
+ /**
+ * Returns the initial state estimation vector.
+ * <p>
+ * Note: if the return value is zero, the Kalman filter will initialize the
+ * state estimation with a zero vector.
+ * </p>
+ *
+ * @return the initial state estimation vector
+ */
+ RealVector getInitialStateEstimate();
+
+ /**
+ * Returns the initial error covariance matrix.
+ * <p>
+ * Note: if the return value is zero, the Kalman filter will initialize the
+ * error covariance with the process noise matrix.
+ * </p>
+ *
+ * @return the initial error covariance matrix
+ */
+ RealMatrix getInitialErrorCovariance();
+}
Propchange:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/filter/ProcessModel.java
------------------------------------------------------------------------------
svn:eol-style = native
Modified: commons/proper/math/trunk/src/site/xdoc/changes.xml
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/xdoc/changes.xml?rev=1134779&r1=1134778&r2=1134779&view=diff
==============================================================================
--- commons/proper/math/trunk/src/site/xdoc/changes.xml (original)
+++ commons/proper/math/trunk/src/site/xdoc/changes.xml Sat Jun 11 21:39:26 2011
@@ -52,6 +52,9 @@ The <action> type attribute can be add,u
If the output is not quite correct, check for invisible trailing spaces!
-->
<release version="3.0" date="TBD" description="TBD">
+ <action dev="erans" type="add" due-to="Thomas Neidhart">
+ New "filter" package. Initial implementation of Kalman filter.
+ </action>
<action dev="luc" type="fix" issue="MATH-584" due-to="Randall Scarberry">
Improved k-means++ clustering performances and initial cluster center
choice.
</action>
Added:
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=1134779&view=auto
==============================================================================
---
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
(added)
+++
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
Sat Jun 11 21:39:26 2011
@@ -0,0 +1,118 @@
+/*
+ * 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.math.filter;
+
+import org.apache.commons.math.linear.Array2DRowRealMatrix;
+import org.apache.commons.math.linear.ArrayRealVector;
+import org.apache.commons.math.linear.RealMatrix;
+import org.apache.commons.math.linear.RealVector;
+import org.apache.commons.math.random.JDKRandomGenerator;
+import org.apache.commons.math.random.RandomGenerator;
+import org.apache.commons.math.util.MathUtils;
+import org.junit.Assert;
+import org.junit.Test;
+
+public class KalmanFilterTest {
+ @Test
+ public void testConstant() {
+ 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 should be larger than measurement noise
+ double diff = Math.abs(constantValue -
filter.getStateEstimation()[0]);
+ // System.out.println(diff);
+ Assert.assertTrue(MathUtils.compareTo(diff, measurementNoise,
1e-6) < 0);
+ }
+
+ // error covariance should be already very low (< 0.02)
+
Assert.assertTrue(MathUtils.compareTo(filter.getErrorCovariance()[0][0],
+ 0.02d, 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);
+ }
+ }
+
+ 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-15);
+ }
+ }
+ }
+}
Propchange:
commons/proper/math/trunk/src/test/java/org/apache/commons/math/filter/KalmanFilterTest.java
------------------------------------------------------------------------------
svn:eol-style = native
