Oceanicsdotio

Algorithms — Kalman filter

December 01, 2019


The Kalman filter method is used to combine multiple data streams, and to drive control systems. The algorithm was described in:

The estimate of the value is the sum

X^=KkZk+(1Kk)X^k1\hat{X}=K_k \cdot Z_k + (1-K_k) \cdot \hat{X}_{k-1}

kk is states, like discrete time intervals XX is estimate zz, value

The gain (KK) is unknown, and the filter process finds the best averaging factor to minimize error. Fixing this at 0.5 results in simple averaging.

xk=Axk1+Buk+wk1x_k = Ax_{k-1} + Bu_k + w_{k-1}

xx is linear combination of the previous value, control signal (uku_k=0), and process noise (as independent Gaussian functions).

zk=Hxk+vkz_k = Hx_k + v_k

any measured value is a linear combination of signal and measurement noise.

noise terms are independent Gaussian functions

A,B,H are matrices, but may reduce to single values, and may be constants, maybe 1

The prediction step updates values: x^k=Ax^k1+Buk\hat{x}_k^-=A \hat{x}_{k-1}+Bu_k Pk=APk1AT+QP_k^-=AP_{k-1}A^T + Q

The correction step accounts for error: Kk=PkHT(HPkHT+R)1K_k=P_k^- H^T(HP_k^- H^T + R)^{-1} x^k=x^k+Kk(zkHk^k)\hat{x}_k = \hat{x}_k^- + K_k(z_k-H \hat{k}_k^-) Pk=(IKkH)PkP_k=(I-K_kH)P_k^-

determine R and Q

Kalman RE. 1960. A new approach to linear filtering and prediction problems. Journal of Basic Engineering 82(1):35–undefined.[links]