commons-commits mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From er...@apache.org
Subject svn commit: r1134779 - in /commons/proper/math/trunk/src: main/java/org/apache/commons/math/filter/ site/xdoc/ test/java/org/apache/commons/math/filter/
Date Sat, 11 Jun 2011 21:39:27 GMT
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



Mime
View raw message