TExtendedKalmanFilter Class

Extended Kalman filter component for filtering a non-linear process.

Estimates the state of a non-linear model x_k = f(x_(k-1), u_(k-1), 0) , z_k = h(x_k, 0) by linearising f and h about the current estimate at each step. The Jacobians A_k = d f / d x and H_k = d h / d x (and the noise Jacobians W_k, V_k) are supplied by the user before each Update; the non-linear functions f and h are evaluated through the OnGenerateProcess and OnGenerateMeasurement events.

Time update:
x_k^- = f(x_(k-1), u_(k-1), 0) P_k^- = A_k P_(k-1) A_k^T + W_k Q W_k^T

Measurement update:
K_k = P_k^- H_k^T(H_k P_k^- H_k^T + V_k R V_k^T)^(-1) x_k = x_k^- + K_k(z_k - h(x_k^-, 0)) P_k = (I - K_k H_k) P_k^-

The linearisation is exact only to first order, so accuracy degrades as the nonlinearity over one step grows; for mild, smooth nonlinearities the estimate converges to the true state. OnGenerateProcess must fill x with f( * ) and OnGenerateMeasurement must fill z with h( * ), else Update raises. The process x(k) has a known mathematical model that the filter fuses with noisy measurements to reduce the estimation error.

Description of symbols:

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

f - non-linear function relating x(k-1) to x(k)

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

u(k) - control vector input. 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 - Non-linear function relates x(k) to the measurement z(k)

w(k) - process noise

v(k) - measurement noise

A(k) - size: n x n. matrix of partial derivates of f with respect to x

H(k) - matrix of partial derivates of h with respect to x

W(k) - matrix of partial derivates of f with respect to w

V(k) - matrix of partial derivates of h with respect to v

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