Class KalmanFilter

java.lang.Object
org.apache.commons.math3.filter.KalmanFilter

public class KalmanFilter extends Object
Implementation of a Kalman filter to estimate the state xk of a discrete-time controlled process that is governed by the linear stochastic difference equation:
 xk = Axk-1 + Buk-1 + wk-1
 
with a measurement xk that is
 zk = Hxk + vk.
 

The random variables wk and vk represent the process and measurement noise and are assumed to be independent of each other and distributed with normal probability (white noise).

The Kalman filter cycle involves the following steps:

  1. predict: project the current state estimate ahead in time
  2. correct: adjust the projected estimate by an actual measurement

The Kalman filter is initialized with a ProcessModel and a MeasurementModel, which contain the corresponding transformation and noise covariance matrices. The parameter names used in the respective models correspond to the following names commonly used in the mathematical literature:

  • A - state transition matrix
  • B - control input matrix
  • H - measurement matrix
  • Q - process noise covariance matrix
  • R - measurement noise covariance matrix
  • P - error covariance matrix
Since:
3.0
See Also:
  • Field Details

    • processModel

      private final ProcessModel processModel
      The process model used by this filter instance.
    • measurementModel

      private final MeasurementModel measurementModel
      The measurement model used by this filter instance.
    • transitionMatrix

      private RealMatrix transitionMatrix
      The transition matrix, equivalent to A.
    • transitionMatrixT

      private RealMatrix transitionMatrixT
      The transposed transition matrix.
    • controlMatrix

      private RealMatrix controlMatrix
      The control matrix, equivalent to B.
    • measurementMatrix

      private RealMatrix measurementMatrix
      The measurement matrix, equivalent to H.
    • measurementMatrixT

      private RealMatrix measurementMatrixT
      The transposed measurement matrix.
    • stateEstimation

      private RealVector stateEstimation
      The internal state estimation vector, equivalent to x hat.
    • errorCovariance

      private RealMatrix errorCovariance
      The error covariance matrix, equivalent to P.
  • Constructor Details

  • Method Details

    • getStateDimension

      public int getStateDimension()
      Returns the dimension of the state estimation vector.
      Returns:
      the state dimension
    • getMeasurementDimension

      public int getMeasurementDimension()
      Returns the dimension of the measurement vector.
      Returns:
      the measurement vector dimension
    • getStateEstimation

      public double[] getStateEstimation()
      Returns the current state estimation vector.
      Returns:
      the state estimation vector
    • getStateEstimationVector

      public RealVector getStateEstimationVector()
      Returns a copy of the current state estimation vector.
      Returns:
      the state estimation vector
    • getErrorCovariance

      public double[][] getErrorCovariance()
      Returns the current error covariance matrix.
      Returns:
      the error covariance matrix
    • getErrorCovarianceMatrix

      public RealMatrix getErrorCovarianceMatrix()
      Returns a copy of the current error covariance matrix.
      Returns:
      the error covariance matrix
    • predict

      public void predict()
      Predict the internal state estimation one time step ahead.
    • predict

      public void predict(double[] u) throws DimensionMismatchException
      Predict the internal state estimation one time step ahead.
      Parameters:
      u - the control vector
      Throws:
      DimensionMismatchException - if the dimension of the control vector does not fit
    • predict

      public void predict(RealVector u) throws DimensionMismatchException
      Predict the internal state estimation one time step ahead.
      Parameters:
      u - the control vector
      Throws:
      DimensionMismatchException - if the dimension of the control vector does not match
    • correct

      Correct the current state estimate with an actual measurement.
      Parameters:
      z - the measurement vector
      Throws:
      NullArgumentException - if the measurement vector is null
      DimensionMismatchException - if the dimension of the measurement vector does not fit
      SingularMatrixException - if the covariance matrix could not be inverted
    • correct

      Correct the current state estimate with an actual measurement.
      Parameters:
      z - the measurement vector
      Throws:
      NullArgumentException - if the measurement vector is null
      DimensionMismatchException - if the dimension of the measurement vector does not fit
      SingularMatrixException - if the covariance matrix could not be inverted