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 }