Skip to content

Kalman Filter

and their extensions

Extended Kalman Filter

State and transition model may be non-linear and differentiable functions.

\[x_t = f(x_{t-1}, u_t) + w_t\]
\[y_t = h(x_t) + v_t\]

Linearization via the Jacobian

The EKF linearizes the nonlinear functions \(f\) and \(h\) around the current estimate using a first-order Taylor expansion. The Jacobian matrices are the partial derivatives of each function with respect to the state, evaluated at the current estimate:

\[F_t = \frac{\partial f}{\partial x}\Big|_{x=\hat{x}_{t-1 \mid t-1}}, \qquad H_t = \frac{\partial h}{\partial x}\Big|_{x=\hat{x}_{t \mid t-1}}\]

where \(F_t\) is the state-transition Jacobian and \(H_t\) is the observation Jacobian. This gives the linearized model

\[x_t \approx F_t x_{t-1} + w_t, \qquad y_t \approx H_t x_t + v_t\]

Predict step

\[\hat{x}_{t \mid t-1} = f(\hat{x}_{t-1 \mid t-1}, u_t)\]
\[P_{t \mid t-1} = F_t P_{t-1 \mid t-1} F_t^\top + Q_t\]

Update step

\[K_t = P_{t \mid t-1} H_t^\top \left( H_t P_{t \mid t-1} H_t^\top + R_t \right)^{-1}\]
\[\hat{x}_{t \mid t} = \hat{x}_{t \mid t-1} + K_t \left( y_t - h(\hat{x}_{t \mid t-1}) \right)\]
\[P_{t \mid t} = \left( I - K_t H_t \right) P_{t \mid t-1}\]

Note the mean propagation uses the original nonlinear functions \(f\) and \(h\), while the covariance propagation uses the linearized Jacobians \(F_t\) and \(H_t\). The Jacobians are recomputed at every time step, which is what distinguishes the EKF from a fixed-point linearized Kalman filter.

Wikipedia - Extended Kalman Filter

Unscented Kalman Filter

The UKF avoids linearizing the nonlinear functions. Instead, it propagates a set of carefully chosen sigma points through the true nonlinear functions and reconstructs the mean and covariance from the transformed points. This captures the nonlinearity more accurately than the EKF's first-order Taylor expansion, without needing to compute Jacobians.

Sigma points

Given the state mean \(\hat{x}\) and covariance \(P\), we form \(2n+1\) sigma points (for an \(n\)-dimensional state) with spread parameter \(\lambda\):

\[\mathcal{X}^{(0)} = \hat{x}, \qquad \mathcal{X}^{(i)} = \hat{x} \pm \sqrt{(n+\lambda) P}_{\,i}\]

with weights \(W^{(0)}_m = \frac{\lambda}{n+\lambda}\), \(W^{(0)}_c = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta)\), and \(W^{(i)}_m = W^{(i)}_c = \frac{1}{2(n+\lambda)}\).

Predict step

Propagate each sigma point through the nonlinear dynamics, then recombine:

\[\mathcal{X}^{(i)}_{t \mid t-1} = f(\mathcal{X}^{(i)}_{t-1 \mid t-1}, u_t)\]
\[\hat{x}_{t \mid t-1} = \sum_i W^{(i)}_m \mathcal{X}^{(i)}_{t \mid t-1}\]
\[P_{t \mid t-1} = \sum_i W^{(i)}_c \left( \mathcal{X}^{(i)}_{t \mid t-1} - \hat{x}_{t \mid t-1} \right) \left( \mathcal{X}^{(i)}_{t \mid t-1} - \hat{x}_{t \mid t-1} \right)^\top + Q_t\]

Update step

Propagate the sigma points through the nonlinear observation, then compute the cross-covariance and Kalman gain:

\[\mathcal{Z}^{(i)} = h(\mathcal{X}^{(i)}_{t \mid t-1}), \qquad \hat{z} = \sum_i W^{(i)}_m \mathcal{Z}^{(i)}\]
\[P_{zz} = \sum_i W^{(i)}_c \left( \mathcal{Z}^{(i)} - \hat{z} \right) \left( \mathcal{Z}^{(i)} - \hat{z} \right)^\top + R_t\]
\[P_{xz} = \sum_i W^{(i)}_c \left( \mathcal{X}^{(i)}_{t \mid t-1} - \hat{x}_{t \mid t-1} \right) \left( \mathcal{Z}^{(i)} - \hat{z} \right)^\top\]
\[K_t = P_{xz} P_{zz}^{-1}\]
\[\hat{x}_{t \mid t} = \hat{x}_{t \mid t-1} + K_t (y_t - \hat{z})\]
\[P_{t \mid t} = P_{t \mid t-1} - K_t P_{zz} K_t^\top\]

The UKF propagates the true nonlinear functions (no Jacobians), so it is accurate to second order and avoids the derivative computations the EKF requires.

Wikipedia - Kalman Filter / Unscented Kalman Filter