fr.cnes.sirius.patrius.math.filter

Class KalmanFilter

java.lang.Object

fr.cnes.sirius.patrius.math.filter.KalmanFilter

public class KalmanFilter
extends Object

Implementation of a Kalman filter to estimate the state x_k 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 x_k that is

 zk = Hxk + vk.
 

The random variables w_k and v_k 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

Version:

$Id: KalmanFilter.java 18108 2017-10-04 06:45:27Z bignon $

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

Constructor Summary

Constructors

Constructor and Description
KalmanFilter(ProcessModel process, MeasurementModel measurement) Creates a new Kalman filter with the given process and measurement models.

Method Summary

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.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail

KalmanFilter

public KalmanFilter(ProcessModel process,
                    MeasurementModel measurement)

Creates a new Kalman filter with the given process and measurement models.

Parameters:

process - the model defining the underlying process dynamics

measurement - 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 square

DimensionMismatchException - if the column dimension of the transition matrix does not match the dimension of the initial state estimation vector

MatrixDimensionMismatchException - 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)

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)

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)

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

public void correct(RealVector z)

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