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.math4.legacy.filter; 018 019import org.apache.commons.math4.legacy.exception.DimensionMismatchException; 020import org.apache.commons.math4.legacy.exception.NullArgumentException; 021import org.apache.commons.math4.legacy.linear.Array2DRowRealMatrix; 022import org.apache.commons.math4.legacy.linear.ArrayRealVector; 023import org.apache.commons.math4.legacy.linear.CholeskyDecomposition; 024import org.apache.commons.math4.legacy.linear.MatrixDimensionMismatchException; 025import org.apache.commons.math4.legacy.linear.MatrixUtils; 026import org.apache.commons.math4.legacy.linear.NonSquareMatrixException; 027import org.apache.commons.math4.legacy.linear.RealMatrix; 028import org.apache.commons.math4.legacy.linear.RealVector; 029import org.apache.commons.math4.legacy.linear.SingularMatrixException; 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 */ 081public class KalmanFilter { 082 /** The process model used by this filter instance. */ 083 private final ProcessModel processModel; 084 /** The measurement model used by this filter instance. */ 085 private final MeasurementModel measurementModel; 086 /** The transition matrix, equivalent to A. */ 087 private RealMatrix transitionMatrix; 088 /** The transposed transition matrix. */ 089 private RealMatrix transitionMatrixT; 090 /** The control matrix, equivalent to B. */ 091 private RealMatrix controlMatrix; 092 /** The measurement matrix, equivalent to H. */ 093 private RealMatrix measurementMatrix; 094 /** The transposed measurement matrix. */ 095 private RealMatrix measurementMatrixT; 096 /** The internal state estimation vector, equivalent to x hat. */ 097 private RealVector stateEstimation; 098 /** The error covariance matrix, equivalent to P. */ 099 private RealMatrix errorCovariance; 100 101 /** 102 * Creates a new Kalman filter with the given process and measurement models. 103 * 104 * @param process 105 * the model defining the underlying process dynamics 106 * @param measurement 107 * the model defining the given measurement characteristics 108 * @throws NullArgumentException 109 * if any of the given inputs is null (except for the control matrix) 110 * @throws NonSquareMatrixException 111 * if the transition matrix is non square 112 * @throws DimensionMismatchException 113 * if the column dimension of the transition matrix does not match the dimension of the 114 * initial state estimation vector 115 * @throws MatrixDimensionMismatchException 116 * if the matrix dimensions do not fit together 117 */ 118 public KalmanFilter(final ProcessModel process, final MeasurementModel measurement) 119 throws NullArgumentException, NonSquareMatrixException, DimensionMismatchException, 120 MatrixDimensionMismatchException { 121 122 NullArgumentException.check(process); 123 NullArgumentException.check(measurement); 124 125 this.processModel = process; 126 this.measurementModel = measurement; 127 128 transitionMatrix = processModel.getStateTransitionMatrix(); 129 NullArgumentException.check(transitionMatrix); 130 transitionMatrixT = transitionMatrix.transpose(); 131 132 // create an empty matrix if no control matrix was given 133 if (processModel.getControlMatrix() == null) { 134 controlMatrix = new Array2DRowRealMatrix(); 135 } else { 136 controlMatrix = processModel.getControlMatrix(); 137 } 138 139 measurementMatrix = measurementModel.getMeasurementMatrix(); 140 NullArgumentException.check(measurementMatrix); 141 measurementMatrixT = measurementMatrix.transpose(); 142 143 // check that the process and measurement noise matrices are not null 144 // they will be directly accessed from the model as they may change 145 // over time 146 RealMatrix processNoise = processModel.getProcessNoise(); 147 NullArgumentException.check(processNoise); 148 RealMatrix measNoise = measurementModel.getMeasurementNoise(); 149 NullArgumentException.check(measNoise); 150 151 // set the initial state estimate to a zero vector if it is not 152 // available from the process model 153 if (processModel.getInitialStateEstimate() == null) { 154 stateEstimation = new ArrayRealVector(transitionMatrix.getColumnDimension()); 155 } else { 156 stateEstimation = processModel.getInitialStateEstimate(); 157 } 158 159 if (transitionMatrix.getColumnDimension() != stateEstimation.getDimension()) { 160 throw new DimensionMismatchException(transitionMatrix.getColumnDimension(), 161 stateEstimation.getDimension()); 162 } 163 164 // initialize the error covariance to the process noise if it is not 165 // available from the process model 166 if (processModel.getInitialErrorCovariance() == null) { 167 errorCovariance = processNoise.copy(); 168 } else { 169 errorCovariance = processModel.getInitialErrorCovariance(); 170 } 171 172 // sanity checks, the control matrix B may be null 173 174 // A must be a square matrix 175 if (!transitionMatrix.isSquare()) { 176 throw new NonSquareMatrixException( 177 transitionMatrix.getRowDimension(), 178 transitionMatrix.getColumnDimension()); 179 } 180 181 // row dimension of B must be equal to A 182 // if no control matrix is available, the row and column dimension will be 0 183 if (controlMatrix != null && 184 controlMatrix.getRowDimension() > 0 && 185 controlMatrix.getColumnDimension() > 0 && 186 controlMatrix.getRowDimension() != transitionMatrix.getRowDimension()) { 187 throw new MatrixDimensionMismatchException(controlMatrix.getRowDimension(), 188 controlMatrix.getColumnDimension(), 189 transitionMatrix.getRowDimension(), 190 controlMatrix.getColumnDimension()); 191 } 192 193 // Q must be equal to A 194 MatrixUtils.checkAdditionCompatible(transitionMatrix, processNoise); 195 196 // column dimension of H must be equal to row dimension of A 197 if (measurementMatrix.getColumnDimension() != transitionMatrix.getRowDimension()) { 198 throw new MatrixDimensionMismatchException(measurementMatrix.getRowDimension(), 199 measurementMatrix.getColumnDimension(), 200 measurementMatrix.getRowDimension(), 201 transitionMatrix.getRowDimension()); 202 } 203 204 // row dimension of R must be equal to row dimension of H 205 if (measNoise.getRowDimension() != measurementMatrix.getRowDimension()) { 206 throw new MatrixDimensionMismatchException(measNoise.getRowDimension(), 207 measNoise.getColumnDimension(), 208 measurementMatrix.getRowDimension(), 209 measNoise.getColumnDimension()); 210 } 211 } 212 213 /** 214 * Returns the dimension of the state estimation vector. 215 * 216 * @return the state dimension 217 */ 218 public int getStateDimension() { 219 return stateEstimation.getDimension(); 220 } 221 222 /** 223 * Returns the dimension of the measurement vector. 224 * 225 * @return the measurement vector dimension 226 */ 227 public int getMeasurementDimension() { 228 return measurementMatrix.getRowDimension(); 229 } 230 231 /** 232 * Returns the current state estimation vector. 233 * 234 * @return the state estimation vector 235 */ 236 public double[] getStateEstimation() { 237 return stateEstimation.toArray(); 238 } 239 240 /** 241 * Returns a copy of the current state estimation vector. 242 * 243 * @return the state estimation vector 244 */ 245 public RealVector getStateEstimationVector() { 246 return stateEstimation.copy(); 247 } 248 249 /** 250 * Returns the current error covariance matrix. 251 * 252 * @return the error covariance matrix 253 */ 254 public double[][] getErrorCovariance() { 255 return errorCovariance.getData(); 256 } 257 258 /** 259 * Returns a copy of the current error covariance matrix. 260 * 261 * @return the error covariance matrix 262 */ 263 public RealMatrix getErrorCovarianceMatrix() { 264 return errorCovariance.copy(); 265 } 266 267 /** 268 * Predict the internal state estimation one time step ahead. 269 */ 270 public void predict() { 271 predict((RealVector) null); 272 } 273 274 /** 275 * Predict the internal state estimation one time step ahead. 276 * 277 * @param u 278 * the control vector 279 * @throws DimensionMismatchException 280 * if the dimension of the control vector does not fit 281 */ 282 public void predict(final double[] u) throws DimensionMismatchException { 283 predict(new ArrayRealVector(u, false)); 284 } 285 286 /** 287 * Predict the internal state estimation one time step ahead. 288 * 289 * @param u 290 * the control vector 291 * @throws DimensionMismatchException 292 * if the dimension of the control vector does not match 293 */ 294 public void predict(final RealVector u) throws DimensionMismatchException { 295 // sanity checks 296 if (u != null && 297 u.getDimension() != controlMatrix.getColumnDimension()) { 298 throw new DimensionMismatchException(u.getDimension(), 299 controlMatrix.getColumnDimension()); 300 } 301 302 // project the state estimation ahead (a priori state) 303 // xHat(k)- = A * xHat(k-1) + B * u(k-1) 304 stateEstimation = transitionMatrix.operate(stateEstimation); 305 306 // add control input if it is available 307 if (u != null) { 308 stateEstimation = stateEstimation.add(controlMatrix.operate(u)); 309 } 310 311 // project the error covariance ahead 312 // P(k)- = A * P(k-1) * A' + Q 313 errorCovariance = transitionMatrix.multiply(errorCovariance) 314 .multiply(transitionMatrixT) 315 .add(processModel.getProcessNoise()); 316 } 317 318 /** 319 * Correct the current state estimate with an actual measurement. 320 * 321 * @param z 322 * the measurement vector 323 * @throws NullArgumentException 324 * if the measurement vector is {@code null} 325 * @throws DimensionMismatchException 326 * if the dimension of the measurement vector does not fit 327 * @throws SingularMatrixException 328 * if the covariance matrix could not be inverted 329 */ 330 public void correct(final double[] z) 331 throws NullArgumentException, DimensionMismatchException, SingularMatrixException { 332 correct(new ArrayRealVector(z, false)); 333 } 334 335 /** 336 * Correct the current state estimate with an actual measurement. 337 * 338 * @param z 339 * the measurement vector 340 * @throws NullArgumentException 341 * if the measurement vector is {@code null} 342 * @throws DimensionMismatchException 343 * if the dimension of the measurement vector does not fit 344 * @throws SingularMatrixException 345 * if the covariance matrix could not be inverted 346 */ 347 public void correct(final RealVector z) 348 throws NullArgumentException, DimensionMismatchException, SingularMatrixException { 349 350 // sanity checks 351 NullArgumentException.check(z); 352 if (z.getDimension() != measurementMatrix.getRowDimension()) { 353 throw new DimensionMismatchException(z.getDimension(), 354 measurementMatrix.getRowDimension()); 355 } 356 357 // S = H * P(k) * H' + R 358 RealMatrix s = measurementMatrix.multiply(errorCovariance) 359 .multiply(measurementMatrixT) 360 .add(measurementModel.getMeasurementNoise()); 361 362 // Inn = z(k) - H * xHat(k)- 363 RealVector innovation = z.subtract(measurementMatrix.operate(stateEstimation)); 364 365 // calculate gain matrix 366 // K(k) = P(k)- * H' * (H * P(k)- * H' + R)^-1 367 // K(k) = P(k)- * H' * S^-1 368 369 // instead of calculating the inverse of S we can rearrange the formula, 370 // and then solve the linear equation A x X = B with A = S', X = K' and B = (H * P)' 371 372 // K(k) * S = P(k)- * H' 373 // S' * K(k)' = H * P(k)-' 374 RealMatrix kalmanGain = new CholeskyDecomposition(s).getSolver() 375 .solve(measurementMatrix.multiply(errorCovariance.transpose())) 376 .transpose(); 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}