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