    001    /*
    002     * Licensed to the Apache Software Foundation (ASF) under one or more
    003     * contributor license agreements.  See the NOTICE file distributed with
    004     * this work for additional information regarding copyright ownership.
    005     * The ASF licenses this file to You under the Apache License, Version 2.0
    006     * (the "License"); you may not use this file except in compliance with
    007     * the License.  You may obtain a copy of the License at
    008     *
    009     *      http://www.apache.org/licenses/LICENSE-2.0
    010     *
    011     * Unless required by applicable law or agreed to in writing, software
    012     * distributed under the License is distributed on an "AS IS" BASIS,
    013     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    014     * See the License for the specific language governing permissions and
    015     * limitations under the License.
    016     */
    017    package org.apache.commons.math3.filter;
    018
    019    import org.apache.commons.math3.exception.DimensionMismatchException;
    020    import org.apache.commons.math3.exception.NullArgumentException;
    021    import org.apache.commons.math3.linear.Array2DRowRealMatrix;
    022    import org.apache.commons.math3.linear.ArrayRealVector;
    023    import org.apache.commons.math3.linear.CholeskyDecomposition;
    024    import org.apache.commons.math3.linear.DecompositionSolver;
    025    import org.apache.commons.math3.linear.MatrixDimensionMismatchException;
    026    import org.apache.commons.math3.linear.MatrixUtils;
    027    import org.apache.commons.math3.linear.NonSquareMatrixException;
    028    import org.apache.commons.math3.linear.RealMatrix;
    029    import org.apache.commons.math3.linear.RealVector;
    030    import org.apache.commons.math3.linear.SingularMatrixException;
    031    import org.apache.commons.math3.util.MathUtils;
    032
    033    /**
    034     * Implementation of a Kalman filter to estimate the state <i>x<sub>k</sub></i>
    035     * of a discrete-time controlled process that is governed by the linear
    036     * stochastic difference equation:
    037     *
    038     * <pre>
    039     * <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>
    040     * </pre>
    041     *
    042     * with a measurement <i>x<sub>k</sub></i> that is
    043     *
    044     * <pre>
    045     * <i>z<sub>k</sub></i> = <b>H</b><i>x<sub>k</sub></i> + <i>v<sub>k</sub></i>.
    046     * </pre>
    047     *
    048     * <p>
    049     * The random variables <i>w<sub>k</sub></i> and <i>v<sub>k</sub></i> represent
    050     * the process and measurement noise and are assumed to be independent of each
    051     * other and distributed with normal probability (white noise).
    052     * <p>
    053     * The Kalman filter cycle involves the following steps:
    054     * <ol>
    055     * <li>predict: project the current state estimate ahead in time</li>
    056     * <li>correct: adjust the projected estimate by an actual measurement</li>
    057     * </ol>
    058     * <p>
    059     * The Kalman filter is initialized with a {@link ProcessModel} and a
    060     * {@link MeasurementModel}, which contain the corresponding transformation and
    061     * noise covariance matrices. The parameter names used in the respective models
    062     * correspond to the following names commonly used in the mathematical
    063     * literature:
    064     * <ul>
    065     * <li>A - state transition matrix</li>
    066     * <li>B - control input matrix</li>
    067     * <li>H - measurement matrix</li>
    068     * <li>Q - process noise covariance matrix</li>
    069     * <li>R - measurement noise covariance matrix</li>
    070     * <li>P - error covariance matrix</li>
    071     * </ul>
    072     *
    073     * @see <a href="http://www.cs.unc.edu/~welch/kalman/">Kalman filter
    074     *      resources</a>
    075     * @see <a href="http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf">An
    076     *      introduction to the Kalman filter by Greg Welch and Gary Bishop</a>
    077     * @see <a href="http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf">
    078     *      Kalman filter example by Dan Simon</a>
    079     * @see ProcessModel
    080     * @see MeasurementModel
    081     * @since 3.0
    082     * @version $Id: KalmanFilter.java 1416643 2012-12-03 19:37:14Z tn $
    083     */
    084    public class KalmanFilter {
    085        /** The process model used by this filter instance. */
    086        private final ProcessModel processModel;
    087        /** The measurement model used by this filter instance. */
    088        private final MeasurementModel measurementModel;
    089        /** The transition matrix, equivalent to A. */
    090        private RealMatrix transitionMatrix;
    091        /** The transposed transition matrix. */
    092        private RealMatrix transitionMatrixT;
    093        /** The control matrix, equivalent to B. */
    094        private RealMatrix controlMatrix;
    095        /** The measurement matrix, equivalent to H. */
    096        private RealMatrix measurementMatrix;
    097        /** The transposed measurement matrix. */
    098        private RealMatrix measurementMatrixT;
    099        /** The internal state estimation vector, equivalent to x hat. */
    100        private RealVector stateEstimation;
    101        /** The error covariance matrix, equivalent to P. */
    102        private RealMatrix errorCovariance;
    103
    104        /**
    105         * Creates a new Kalman filter with the given process and measurement models.
    106         *
    107         * @param process
    108         *            the model defining the underlying process dynamics
    109         * @param measurement
    110         *            the model defining the given measurement characteristics
    111         * @throws NullArgumentException
    112         *             if any of the given inputs is null (except for the control matrix)
    113         * @throws NonSquareMatrixException
    114         *             if the transition matrix is non square
    115         * @throws DimensionMismatchException
    116         *             if the column dimension of the transition matrix does not match the dimension of the
    117         *             initial state estimation vector
    118         * @throws MatrixDimensionMismatchException
    119         *             if the matrix dimensions do not fit together
    120         */
    121        public KalmanFilter(final ProcessModel process, final MeasurementModel measurement)
    122                throws NullArgumentException, NonSquareMatrixException, DimensionMismatchException,
    123                       MatrixDimensionMismatchException {
    124
    125            MathUtils.checkNotNull(process);
    126            MathUtils.checkNotNull(measurement);
    127
    128            this.processModel = process;
    129            this.measurementModel = measurement;
    130
    131            transitionMatrix = processModel.getStateTransitionMatrix();
    132            MathUtils.checkNotNull(transitionMatrix);
    133            transitionMatrixT = transitionMatrix.transpose();
    134
    135            // create an empty matrix if no control matrix was given
    136            if (processModel.getControlMatrix() == null) {
    137                controlMatrix = new Array2DRowRealMatrix();
    138            } else {
    139                controlMatrix = processModel.getControlMatrix();
    140            }
    141
    142            measurementMatrix = measurementModel.getMeasurementMatrix();
    143            MathUtils.checkNotNull(measurementMatrix);
    144            measurementMatrixT = measurementMatrix.transpose();
    145
    146            // check that the process and measurement noise matrices are not null
    147            // they will be directly accessed from the model as they may change
    148            // over time
    149            RealMatrix processNoise = processModel.getProcessNoise();
    150            MathUtils.checkNotNull(processNoise);
    151            RealMatrix measNoise = measurementModel.getMeasurementNoise();
    152            MathUtils.checkNotNull(measNoise);
    153
    154            // set the initial state estimate to a zero vector if it is not
    155            // available from the process model
    156            if (processModel.getInitialStateEstimate() == null) {
    157                stateEstimation = new ArrayRealVector(transitionMatrix.getColumnDimension());
    158            } else {
    159                stateEstimation = processModel.getInitialStateEstimate();
    160            }
    161
    162            if (transitionMatrix.getColumnDimension() != stateEstimation.getDimension()) {
    163                throw new DimensionMismatchException(transitionMatrix.getColumnDimension(),
    164                                                     stateEstimation.getDimension());
    165            }
    166
    167            // initialize the error covariance to the process noise if it is not
    168            // available from the process model
    169            if (processModel.getInitialErrorCovariance() == null) {
    170                errorCovariance = processNoise.copy();
    171            } else {
    172                errorCovariance = processModel.getInitialErrorCovariance();
    173            }
    174
    175            // sanity checks, the control matrix B may be null
    176
    177            // A must be a square matrix
    178            if (!transitionMatrix.isSquare()) {
    179                throw new NonSquareMatrixException(
    180                        transitionMatrix.getRowDimension(),
    181                        transitionMatrix.getColumnDimension());
    182            }
    183
    184            // row dimension of B must be equal to A
    185            if (controlMatrix != null &&
    186                controlMatrix.getRowDimension() > 0 &&
    187                controlMatrix.getColumnDimension() > 0 &&
    188                (controlMatrix.getRowDimension() != transitionMatrix.getRowDimension() ||
    189                 controlMatrix.getColumnDimension() != 1)) {
    190                throw new MatrixDimensionMismatchException(controlMatrix.getRowDimension(),
    191                                                           controlMatrix.getColumnDimension(),
    192                                                           transitionMatrix.getRowDimension(), 1);
    193            }
    194
    195            // Q must be equal to A
    196            MatrixUtils.checkAdditionCompatible(transitionMatrix, processNoise);
    197
    198            // column dimension of H must be equal to row dimension of A
    199            if (measurementMatrix.getColumnDimension() != transitionMatrix.getRowDimension()) {
    200                throw new MatrixDimensionMismatchException(measurementMatrix.getRowDimension(),
    201                                                           measurementMatrix.getColumnDimension(),
    202                                                           measurementMatrix.getRowDimension(),
    203                                                           transitionMatrix.getRowDimension());
    204            }
    205
    206            // row dimension of R must be equal to row dimension of H
    207            if (measNoise.getRowDimension() != measurementMatrix.getRowDimension() ||
    208                measNoise.getColumnDimension() != 1) {
    209                throw new MatrixDimensionMismatchException(measNoise.getRowDimension(),
    210                                                           measNoise.getColumnDimension(),
    211                                                           measurementMatrix.getRowDimension(), 1);
    212            }
    213        }
    214
    215        /**
    216         * Returns the dimension of the state estimation vector.
    217         *
    218         * @return the state dimension
    219         */
    220        public int getStateDimension() {
    221            return stateEstimation.getDimension();
    222        }
    223
    224        /**
    225         * Returns the dimension of the measurement vector.
    226         *
    227         * @return the measurement vector dimension
    228         */
    229        public int getMeasurementDimension() {
    230            return measurementMatrix.getRowDimension();
    231        }
    232
    233        /**
    234         * Returns the current state estimation vector.
    235         *
    236         * @return the state estimation vector
    237         */
    238        public double[] getStateEstimation() {
    239            return stateEstimation.toArray();
    240        }
    241
    242        /**
    243         * Returns a copy of the current state estimation vector.
    244         *
    245         * @return the state estimation vector
    246         */
    247        public RealVector getStateEstimationVector() {
    248            return stateEstimation.copy();
    249        }
    250
    251        /**
    252         * Returns the current error covariance matrix.
    253         *
    254         * @return the error covariance matrix
    255         */
    256        public double[][] getErrorCovariance() {
    257            return errorCovariance.getData();
    258        }
    259
    260        /**
    261         * Returns a copy of the current error covariance matrix.
    262         *
    263         * @return the error covariance matrix
    264         */
    265        public RealMatrix getErrorCovarianceMatrix() {
    266            return errorCovariance.copy();
    267        }
    268
    269        /**
    270         * Predict the internal state estimation one time step ahead.
    271         */
    272        public void predict() {
    273            predict((RealVector) null);
    274        }
    275
    276        /**
    277         * Predict the internal state estimation one time step ahead.
    278         *
    279         * @param u
    280         *            the control vector
    281         * @throws DimensionMismatchException
    282         *             if the dimension of the control vector does not fit
    283         */
    284        public void predict(final double[] u) throws DimensionMismatchException {
    285            predict(new ArrayRealVector(u));
    286        }
    287
    288        /**
    289         * Predict the internal state estimation one time step ahead.
    290         *
    291         * @param u
    292         *            the control vector
    293         * @throws DimensionMismatchException
    294         *             if the dimension of the control vector does not match
    295         */
    296        public void predict(final RealVector u) throws DimensionMismatchException {
    297            // sanity checks
    298            if (u != null &&
    299                u.getDimension() != controlMatrix.getColumnDimension()) {
    300                throw new DimensionMismatchException(u.getDimension(),
    301                                                     controlMatrix.getColumnDimension());
    302            }
    303
    304            // project the state estimation ahead (a priori state)
    305            // xHat(k)- = A * xHat(k-1) + B * u(k-1)
    306            stateEstimation = transitionMatrix.operate(stateEstimation);
    307
    308            // add control input if it is available
    309            if (u != null) {
    310                stateEstimation = stateEstimation.add(controlMatrix.operate(u));
    311            }
    312
    313            // project the error covariance ahead
    314            // P(k)- = A * P(k-1) * A' + Q
    315            errorCovariance = transitionMatrix.multiply(errorCovariance)
    316                    .multiply(transitionMatrixT)
    317                    .add(processModel.getProcessNoise());
    318        }
    319
    320        /**
    321         * Correct the current state estimate with an actual measurement.
    322         *
    323         * @param z
    324         *            the measurement vector
    325         * @throws NullArgumentException
    326         *             if the measurement vector is {@code null}
    327         * @throws DimensionMismatchException
    328         *             if the dimension of the measurement vector does not fit
    329         * @throws SingularMatrixException
    330         *             if the covariance matrix could not be inverted
    331         */
    332        public void correct(final double[] z)
    333                throws NullArgumentException, DimensionMismatchException, SingularMatrixException {
    334            correct(new ArrayRealVector(z));
    335        }
    336
    337        /**
    338         * Correct the current state estimate with an actual measurement.
    339         *
    340         * @param z
    341         *            the measurement vector
    342         * @throws NullArgumentException
    343         *             if the measurement vector is {@code null}
    344         * @throws DimensionMismatchException
    345         *             if the dimension of the measurement vector does not fit
    346         * @throws SingularMatrixException
    347         *             if the covariance matrix could not be inverted
    348         */
    349        public void correct(final RealVector z)
    350                throws NullArgumentException, DimensionMismatchException, SingularMatrixException {
    351
    352            // sanity checks
    353            MathUtils.checkNotNull(z);
    354            if (z.getDimension() != measurementMatrix.getRowDimension()) {
    355                throw new DimensionMismatchException(z.getDimension(),
    356                                                     measurementMatrix.getRowDimension());
    357            }
    358
    359            // S = H * P(k) - * H' + R
    360            RealMatrix s = measurementMatrix.multiply(errorCovariance)
    361                .multiply(measurementMatrixT)
    362                .add(measurementModel.getMeasurementNoise());
    363
    364            // invert S
    365            // as the error covariance matrix is a symmetric positive
    366            // semi-definite matrix, we can use the cholesky decomposition
    367            DecompositionSolver solver = new CholeskyDecomposition(s).getSolver();
    368            RealMatrix invertedS = solver.getInverse();
    369
    370            // Inn = z(k) - H * xHat(k)-
    371            RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation));
    372
    373            // calculate gain matrix
    374            // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1
    375            // K(k) = P(k)- * H' * S^-1
    376            RealMatrix kalmanGain = errorCovariance.multiply(measurementMatrixT).multiply(invertedS);
    377
    378            // update estimate with measurement z(k)
    379            // xHat(k) = xHat(k)- + K * Inn
    380            stateEstimation = stateEstimation.add(kalmanGain.operate(innovation));
    381
    382            // update covariance of prediction error
    383            // P(k) = (I - K * H) * P(k)-
    384            RealMatrix identity = MatrixUtils.createRealIdentityMatrix(kalmanGain.getRowDimension());
    385            errorCovariance = identity.subtract(kalmanGain.multiply(measurementMatrix)).multiply(errorCovariance);
    386        }
    387    }