KalmanFilter

Kalman filtering, or linear quadratic estimation, is "an algorithm that uses a series of measurements observed over time, including statistical noise and other inaccuracies, and produces estimates of unknown variables for each timeframe" (Wikipedia). To be completely honest, I only have a very basic understanding of the math going on here, so the documentation might be... a little bit shaky. Kalman filters are commonly used in autonomous navigation and control software because they help to safeguard against unwanted and extraneous sensor input. In the case of robotics, these filters can be used for several purposes, including measuring the velocity of an encoder, fusing data from multiple sensors, or estimating the robot's position.

Note: A Kalman filter is designed to reduce noise, NOT increase the accuracy of measurements. The only way to increase the accuracy of those measurements is to increase the accuracy of the tools used in gathering those measurements. If you have values with high accuracy but low precision, one of these bad boys should help you a lot. If you values with low accuracy but high precision... well, I hate to break it to you, but you're really kind of out of luck.

A Kalman filter is based upon three matrices:

• State matrix
• Covariance matrix
• Projection matrix

Of those three matrices, the state and covariance matrices are the most important - the projection matrix can be disregarded for simple use cases. The state matrix is the filter's current state - if you're using an encoder, for example, this might be the filter's velocity. The covariance matrix determines how heavily each value from the state matrix is weighted - a higher covariance means that the value is less accurate, meaning it will not affect the output as much, while a lower covariance means the value is more accurate, and that value will impact the output more. The projection matrix is used for outputting values.

Deprecated: This class is deprecated because the author does not have sufficient mathematical understanding to fully support it. It is included for posterity but is not officially supported.

Methods

KalmanFilter(double[] initialState, double[] initialCovariance)

Constructs a new KalmanFilter with the given initial state and covariance arrays. A default projection matrix is generated.

KalmanFilter(double[] initialState, double[] initialCovariance, double[] initialProjection)

Constructs a new KalmanFilter with the given initial state, covariance, and projection arrays.

KalmanFilter(DMatrixRMaj stateTransitionMatrix, DMatrixRMaj plantNoiseMatrix, DMatrixRMaj projectionMatrix)

Constructs a new KalmanFilter with pre-defined EJML matrices for state transition, plant noise, and projection.

getStateMatrix(double[] state)

Static method. Creates an EJML DMatrixRMaj from a double array representing the state values.

getCovarianceMatrix(double[] covariance)

Static method. Creates an EJML DMatrixRMaj from a double array representing the covariance values. Higher covariance means less weighting, lower means more weighting.

getProjectionMatrix(double[] projection)

Static method. Creates an EJML DMatrixRMaj from a double array representing the projection values.

getProjectionMatrix(int size)

Static method. Creates a default EJML DMatrixRMaj for projection with all diagonal elements set to 1.

setState(DMatrixRMaj state, DMatrixRMaj covariance)

Sets the current state and covariance matrices of the filter.

predict()

Performs the prediction step of the Kalman filter, updating the state and covariance based on the system model.

update(DMatrixRMaj state, DMatrixRMaj covariance)

Performs the update step of the Kalman filter, incorporating new measurements (state and covariance) to refine the state estimate.

getState()

Returns the most recently calculated state matrix.

getCovariance()

Returns the most recently calculated covariance matrix.