Class KalmanFilter


  • public class KalmanFilter
    extends java.lang.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:
    Kalman filter resources, An introduction to the Kalman filter by Greg Welch and Gary Bishop, Kalman filter example by Dan Simon, ProcessModel, MeasurementModel
    • Field Detail

      • 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.
    • Method Detail

      • 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