Kalman Filter

Optimal linear estimation under Gaussian noise — the workhorse of navigation.

  • #estimation
  • #filtering
  • #linear-systems

Overview

The Kalman filter provides the minimum-mean-square-error (MMSE) estimate of the state xk\mathbf{x}_k given a sequence of measurements z1:k\mathbf{z}_{1:k}, under the assumption that both process and measurement noise are Gaussian and that the system is linear.

State-space model

xk=Fk−1xk−1+Gk−1uk−1+wk−1\mathbf{x}_k = \mathbf{F}_{k-1} \mathbf{x}_{k-1} + \mathbf{G}_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1} zk=Hkxk+vk\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k

where wk−1∼N(0,Qk−1)\mathbf{w}_{k-1} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{k-1}) and vk∼N(0,Rk)\mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k).

Prediction step

x^k∣k−1=Fk−1x^k−1∣k−1\hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_{k-1} \hat{\mathbf{x}}_{k-1|k-1} Pk∣k−1=Fk−1Pk−1∣k−1Fk−1⊤+Qk−1\mathbf{P}_{k|k-1} = \mathbf{F}_{k-1} \mathbf{P}_{k-1|k-1} \mathbf{F}_{k-1}^\top + \mathbf{Q}_{k-1}

Update step

Kk=Pk∣k−1Hk⊤(HkPk∣k−1Hk⊤+Rk)−1\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \bigl(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top + \mathbf{R}_k\bigr)^{-1} x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1}) Pk∣k=(I−KkHk)Pk∣k−1\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}

See also