public class KalmanFilter
extends java.lang.Object
xk = Axk-1 + Buk-1 + wk-1with 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:
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:
Modifier and Type | Field and 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 and Description |
---|
KalmanFilter(ProcessModel process,
MeasurementModel measurement)
Creates a new Kalman filter with the given process and measurement models.
|
Modifier and Type | Method and 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.
|
private final ProcessModel processModel
private final MeasurementModel measurementModel
private RealMatrix transitionMatrix
private RealMatrix transitionMatrixT
private RealMatrix controlMatrix
private RealMatrix measurementMatrix
private RealMatrix measurementMatrixT
private RealVector stateEstimation
private RealMatrix errorCovariance
public KalmanFilter(ProcessModel process, MeasurementModel measurement) throws NullArgumentException, NonSquareMatrixException, DimensionMismatchException, MatrixDimensionMismatchException
process
- the model defining the underlying process dynamicsmeasurement
- the model defining the given measurement characteristicsNullArgumentException
- 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 togetherpublic int getStateDimension()
public int getMeasurementDimension()
public double[] getStateEstimation()
public RealVector getStateEstimationVector()
public double[][] getErrorCovariance()
public RealMatrix getErrorCovarianceMatrix()
public void predict()
public void predict(double[] u) throws DimensionMismatchException
u
- the control vectorDimensionMismatchException
- if the dimension of the control vector does not fitpublic void predict(RealVector u) throws DimensionMismatchException
u
- the control vectorDimensionMismatchException
- if the dimension of the control vector does not matchpublic void correct(double[] z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException
z
- the measurement vectorNullArgumentException
- if the measurement vector is null
DimensionMismatchException
- if the dimension of the measurement vector does not fitSingularMatrixException
- if the covariance matrix could not be invertedpublic void correct(RealVector z) throws NullArgumentException, DimensionMismatchException, SingularMatrixException
z
- the measurement vectorNullArgumentException
- if the measurement vector is null
DimensionMismatchException
- if the dimension of the measurement vector does not fitSingularMatrixException
- if the covariance matrix could not be inverted