Extended Kalman Filter

Topics: State Estimation

Introduction

The Extended Kalman Filter (EKF) is one of the nonlinear versions of the Kalman Filter. It can handle nonlinear state and measurement equations. It works by linearizing the nonlinear model around the current best guess using Jacobians.

Algorithm

A nonlinear model is represented by the following equations:

$$\begin{gather} x_k=f(x_{k-1}, u_{k-1}) + w_{k-1} \\ y_k = h(x_k) + n_k \end{gather}$$

where $f$ and $h$ are nonlinear functions. They don't have to be both nonlinear; one of them can be linear.

Similarly to the linear Kalman Filter, the EKF includes a prediction phase and an update phase. The equations are similar except that the state transition matrix $A$ and the observation matrix $C$ of the linear model are changed to be the Jacobians $F$ and $H$ of $f$ and $h$ of the nonlinear model.

The detailed EKF algorithm is presented below. The - superscript is used for predicted values and the + superscript is used for estimated values.

Jacobians

The Jacobians of the nonlinear equations need to be computed at every step:

$$\begin{gather} F_k = \frac{\partial f}{\partial x} \Bigg \vert _{\hat{x}_{k-1}^+, \; u_{k-1}} \\ \\ H_k = \frac{\partial h}{\partial x} \Bigg \vert _{\hat{x}_k} \end{gather}$$

I. Prediction

1. Predict the state $\hat{x}_k^-$ using the state equation, previous estimate, and the input

$$\hat{x}_k^- = f(\hat{x}_{k-1}^+, u_{k-1})$$

2. Calculate the Jacobian matrix $F_k$
3. Predict the error covariance
   $P_{k}^{-} = F_k \; P_{k-1}^{+} \; F_k^{\top} + Q$
4. Predict the system output $\hat{y}_k$ using the output equation
   $\hat{y}_k=h(\hat{x}_k^-)$

II. Update

1. Calculate the Jacobian matrix $H_k$
2. Calculate the Kalman gain
   $L_{k} = P_{k}^{-} \; H_k^{\top}[H_k \; P_{k}^{-} \; H_k^{\top} + R] ^ {-1}$
3. Correct the state prediction based on the output error to get the new estimate $\hat{x}_{k}^{+} = \hat{x}_{k}^{-} + L_{k}(y_k - \hat{y}_k)$
4. Update the error covariance matrix
   $P_{k}^{+} = (I - L_{k} \; H_{k}) P_{k}^{-}$

References

1. https://en.wikipedia.org/wiki/Kalman_filter