Package org.apache.commons.math3.filter
Class KalmanFilter
- java.lang.Object
-
- org.apache.commons.math3.filter.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 iszk = 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:
- predict: project the current state estimate ahead in time
- correct: adjust the projected estimate by an actual measurement
The Kalman filter is initialized with a
ProcessModel
and aMeasurementModel
, 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
-
-
Field Summary
Fields Modifier and Type Field Description private RealMatrix
controlMatrix
The control matrix, equivalent to B.private RealMatrix
errorCovariance
The error covariance matrix, equivalent to P.private RealMatrix
measurementMatrix
The measurement matrix, equivalent to H.private RealMatrix
measurementMatrixT
The transposed measurement matrix.private MeasurementModel
measurementModel
The measurement model used by this filter instance.private ProcessModel
processModel
The process model used by this filter instance.private RealVector
stateEstimation
The internal state estimation vector, equivalent to x hat.private RealMatrix
transitionMatrix
The transition matrix, equivalent to A.private RealMatrix
transitionMatrixT
The transposed transition matrix.
-
Constructor Summary
Constructors Constructor Description KalmanFilter(ProcessModel process, MeasurementModel measurement)
Creates a new Kalman filter with the given process and measurement models.
-
Method Summary
All Methods Instance Methods Concrete Methods Modifier and Type Method Description void
correct(double[] z)
Correct the current state estimate with an actual measurement.void
correct(RealVector z)
Correct the current state estimate with an actual measurement.double[][]
getErrorCovariance()
Returns the current error covariance matrix.RealMatrix
getErrorCovarianceMatrix()
Returns a copy of the current error covariance matrix.int
getMeasurementDimension()
Returns the dimension of the measurement vector.int
getStateDimension()
Returns the dimension of the state estimation vector.double[]
getStateEstimation()
Returns the current state estimation vector.RealVector
getStateEstimationVector()
Returns a copy of the current state estimation vector.void
predict()
Predict the internal state estimation one time step ahead.void
predict(double[] u)
Predict the internal state estimation one time step ahead.void
predict(RealVector u)
Predict the internal state estimation one time step ahead.
-
-
-
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.
-
-
Constructor Detail
-
KalmanFilter
public KalmanFilter(ProcessModel process, MeasurementModel measurement) throws NullArgumentException, NonSquareMatrixException, DimensionMismatchException, MatrixDimensionMismatchException
Creates a new Kalman filter with the given process and measurement models.- Parameters:
process
- the model defining the underlying process dynamicsmeasurement
- the model defining the given measurement characteristics- Throws:
NullArgumentException
- if any of the given inputs is null (except for the control matrix)NonSquareMatrixException
- if the transition matrix is non squareDimensionMismatchException
- if the column dimension of the transition matrix does not match the dimension of the initial state estimation vectorMatrixDimensionMismatchException
- if the matrix dimensions do not fit together
-
-
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
-
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
public void correct(double[] z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException
Correct the current state estimate with an actual measurement.- Parameters:
z
- the measurement vector- Throws:
NullArgumentException
- if the measurement vector isnull
DimensionMismatchException
- if the dimension of the measurement vector does not fitSingularMatrixException
- if the covariance matrix could not be inverted
-
correct
public void correct(RealVector z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException
Correct the current state estimate with an actual measurement.- Parameters:
z
- the measurement vector- Throws:
NullArgumentException
- if the measurement vector isnull
DimensionMismatchException
- if the dimension of the measurement vector does not fitSingularMatrixException
- if the covariance matrix could not be inverted
-
-