public class KalmanFilterextends ObjectImplementation 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-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:
- 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
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