Extended Kalman Filter

The extended Kalman filter (EKF) is a nonlinear variant of the standard Kalman filter. It applies to nonlinear dynamical systems where the state transition and/or measurement models are given by nonlinear functions rather than linear matrices. The EKF operates by linearizing these functions around the current estimate using first-order Taylor expansions (Jacobian matrices), then applying the standard Kalman filter mechanics to the linearized system.

The following notation is used throughout this document (see Notation and Conventions for a complete reference):

  • \(\mathbf{x}_k\) is the state vector at time step \(k\).

  • \(f(\cdot)\) and \(h(\cdot)\) are the nonlinear state transition and measurement functions, respectively.

Mathematical Formulation

The nonlinear system is described by

\[\mathbf{x}_{k} = f(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}) + \mathbf{w}_{k-1},\]
\[\mathbf{z}_{k} = h(\mathbf{x}_{k}) + \mathbf{v}_{k},\]

where

  • \(f(\cdot)\) is the nonlinear state transition function that propagates the state forward in time,

  • \(h(\cdot)\) is the nonlinear measurement function,

  • \(\mathbf{u}\) is the control vector (if present),

  • \(\mathbf{w}\) and \(\mathbf{v}\) are zero‑mean Gaussian noises with covariances \(\mathbf{Q}\) and \(\mathbf{R}\),

    \[\mathbf{w}_{k} \sim \mathcal{N}(0,\mathbf{Q}_{k}),\qquad \mathbf{v}_{k} \sim \mathcal{N}(0,\mathbf{R}_{k}),\]

where

  • \(\mathbf{P}\) denotes the state covariance matrix, \(\mathbf{Q}\) the process noise covariance and \(\mathbf{R}\) the measurement noise covariance.

The EKF linearizes these functions at the current operating point using first-order Taylor expansions. The Jacobian matrices are computed as

\[\mathbf{F}_{k-1} = \left. \frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}^+_{k-1}, \mathbf{u}_{k-1}},\]
\[\mathbf{H}_{k} = \left. \frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}^-_{k}}.,\]

where

  • \(\mathbf{F}\) and \(\mathbf{H}\) are the Jacobian matrices of \(f\) and \(h\) evaluated at the current estimate.

These Jacobians replace the constant linear matrices of the standard Kalman filter, allowing the algorithm to approximate nonlinear dynamics.

Prediction Step

The prior (predicted) state and covariance are computed as

\[\hat{\mathbf{x}}^-_{k} = f(\hat{\mathbf{x}}^+_{k-1}, \mathbf{u}_{k-1}),\]
\[\mathbf{P}^-_{k} = \mathbf{F}_{k-1} \, \mathbf{P}^+_{k-1} \, \mathbf{F}^T_{k-1} + \mathbf{Q}_{k-1}.\]

Note that the nonlinear function \(f\) is applied to obtain the predicted state, while the Jacobian \(\mathbf{F}\) is used for the covariance propagation (linearized dynamics).

Correction Step

After receiving a measurement \(\tilde{\mathbf{z}}_{k}\), the filter computes the innovation (residual) using the nonlinear measurement function

\[\tilde{\mathbf{y}}_{k} = \tilde{\mathbf{z}}_{k} - h(\hat{\mathbf{x}}^-_{k}),\]

and the Kalman gain

\[\mathbf{K}_{k} = \mathbf{P}^-_{k} \mathbf{H}^T_{k} (\mathbf{H}_{k} \mathbf{P}^-_{k} \mathbf{H}^T_{k} + \mathbf{R}_{k})^{-1}.\]

The posterior estimate and covariance are then updated by

\[\hat{\mathbf{x}}^+_{k} = \hat{\mathbf{x}}^-_{k} + \mathbf{K}_{k} \tilde{\mathbf{y}}_{k},\]
\[\mathbf{P}^+_{k} = (\mathbf{I} - \mathbf{K}_{k} \mathbf{H}_{k}) \mathbf{P}^-_{k}.\]

As in the linear filter, the posterior covariance reflects the reduction in uncertainty provided by the measurement.

Linearization and Approximation Error

The EKF replaces nonlinear functions with their first-order Taylor approximations. This linearization incurs approximation error that grows with the magnitude of nonlinearity and the size of the covariance (uncertainty region). For highly nonlinear systems or when nonlinearity matters significantly, higher-order filters (e.g., unscented Kalman filter, particle filters) may be required.

Motion Models and Process Noise

Many of the motion models described in Motion Models are nonlinear, such as the constant turn rate and velocity (CTRV) model. The EKF is the standard approach for estimating states under these models. Process and measurement noise covariances are designed using the same principles as for linear filters; see Motion Models for details on \(\mathbf{Q}\) design.

Code Documentation

The extended Kalman filter is seperated in prediction and correction step. The prediction step is implemented in the model_state_predictor.hpp using a model which inherits from non_linear_kalman_transition_model.hpp. The correction step is implemented in the model_state_updater.hpp usning a measurement model which inherits from non_linear_kalman_measurement_model.hpp.

Example Usage

Comming Soon

References

R. E. Kalman, “A new approach to linear filtering and prediction problems,” Journal of Basic Engineering, 1960

Y. Kim and H. Bang, “Introduction to Kalman filter and its applications,” Introduction and implementations of the Kalman filter, IntechOpen, 2018

H. A. P. Blom and Y. Bar-Shalom, “The interacting multiple model algorithm for systems with Markovian switching coefficients,” in IEEE Transactions on Automatic Control, vol. 33, no. 8, pp. 780-783, 1988