TKalmanFilter Class

Kalman filter component implements the standard (linear) Kalman filtering algorithm.

Recursive minimum-mean-square-error state estimator for the linear-Gaussian model x_k = A x_(k-1) + B u_(k-1) + w_(k-1) , z_k = H x_k + v_k , with independent zero-mean Gaussian process noise w ~ N(0,Q) and measurement noise v ~ N(0,R).

Each call to Update applies one time update (prediction) followed by one measurement update (correction):

Time update (a-priori): x_k^- = A x_(k-1) + B u_(k-1) P_k^- = A P_(k-1) A^T + Q

Measurement update (a-posteriori): K_k = P_k^- H^T(H P_k^- H^T + R)^(-1) x_k = x_k^- + K_k(z_k - H x_k^-) P_k = (I - K_k H) P_k^-

where K_k is the Kalman gain. After Update, the exposed P holds the a-posteriori covariance P_k. For a time-invariant model the covariance converges to the unique stabilising solution of the discrete algebraic Riccati equation. Domain: A is n * n, H is m * n, Q is n * n positive semdefinite, R is m * m positive definite; sizes must be consistent or Update raises. The process x(k) has a known mathematical model, and the filter fuses it with noisy measurements to reduce the estimation error.

Description of symbols:

s - number of parallel inputs (columns in x and z).

x(k) - size: n x s. vector state (value) of the process

A - size: n x n. Maps x(k-1) to x(k) without noise or system input

u(k) - control vector input (optional). size: l x s

B - size: n x l maps control input u(k-1) to x(k)

Q - process noise covariance

R - measurement noise covariance

z - size: m x s. Measurement vector

H - size: m x n. Relates x(k) to the measurement z(k)

w - process noise

v - measurement noise

All parameters can be modified by the user before each iteration of the filter. One iteration of the filter is achieved by calling the Update method. The process noise and measurement noise are assumed to be independent and gaussian.

[1] An Introduction to the Kalman Filter, Greg Welch and Gary Bishop