Derivation of Extended Kalman Filter (EKF)

The Kalman Filter heavily exploits the fact that our dynamics/measurements are linear. However, typical robotics problems have non-linear dynamics or non-linear observation model or both. Extended-Kalman Filter (EKF) is a modification of the KF to handle such situations. EKF linearizes the dynamics and observation equations of a non-linear system at each timestep and then plugs them into the Kalman Filter formulae.

Consider a non-linear dynamical system with non-linear observations, \begin{align*} x_{k+1} &= f(x_k, u_k) + \epsilon \tag{1.1}\\ y_k &= g(x_k) + \upsilon \tag{1.2} \end{align*} where noise vectors $\epsilon \sim N(0,R)$ and $\upsilon \sim N(0,Q)$ are defined similar to KF. The central idea of the Extended Kalman Filter (EKF) is to linearize a nonlinear system at each timestep $k$ using the Taylor series around the latest state estimate given by the Kalman Filter and use the resultant linearized dynamical system in the KF equations for the next timestep.

Step 1: Propagating the dynamics by one timestep

Just like KF, suppose we have the estimate $\hat{x}_{k|k}$. We can use the Taylor series by linearizing around the mean of this estimate $\mu_{k|k}$ to approximate the first and second moments as follows, \begin{align*} \hat{x}_{k+1|k} = f(\hat{x}_{k|k}, u_k) + \epsilon &\approx f(\mu_{k|k}, u_k) + \left.\frac{df}{dx}\right\vert_{x = \mu_{k|k}} (\hat{x}_{k|k} - \mu_{k|k}) + \epsilon \\ &= f(\mu_{k|k}, u_k) + A (\hat{x}_{k|k} - \mu_{k|k}) + \epsilon \\ \end{align*} where $A = \left.\frac{df}{dx}\right\vert_{x = \mu_{k|k}} \in \mathbb{R}^{p \times d}$ is the Jacobian matrix which is computed at every time step as it is a function of $\mu_{k|k}$. The mean of the EKF after the dynamics propagation step is given by, \begin{align*} \mu_{k+1|k} &= E[\hat{x}_{k+1|k}] = E[f(\mu_{k|k}, u_k) + A (\hat{x}_{k|k} - \mu_{k|k}) + \epsilon] \\ &= f(\mu_{k|k}, u_k) + A (\mu_{k|k} - \mu_{k|k}) \\ &= f(\mu_{k|k}, u_k) \tag{2}\\ \end{align*} Now, lets define $z = f(\mu_{k|k}, u_k) + A (\hat{x}_{k|k} - \mu_{k|k})$, then $E[z] = f(\mu_{k|k}, u_k)$. The covariance can be written as, \begin{align*} \Sigma_{k+1|k} &= \textrm{cov}(\hat{x}_{k+1|k}) = \textrm{cov}(f(\mu_{k|k}, u_k) + A (\hat{x}_{k|k} - \mu_{k|k}) + \epsilon) \\ &= \textrm{cov}(z) + R \\ &= E[(z - E[z])(z - E[z])^T] + R \\ &= E[A (\hat{x}_{k|k} - \mu_{k|k}) (\hat{x}_{k|k} - \mu_{k|k})^T A^T] + R\\ &= A \Sigma_{k|k} A^T + R \tag{3} \end{align*}

Step 2: Incorporating the observation

Now that we know $\mu_{k+1|k}$ after Step 1, we can linearize the nonlinear observations at this state, \begin{align*} y_{k+1} = g({x}_{k+1}) + \upsilon &\approx g(\mu_{k+1|k}) + \left.\frac{dg}{dx}\right\vert_{x = \mu_{k+1|k}} (x_{k+1|k} - \mu_{k+1|k}) + \upsilon \\ &= g(\mu_{k+1|k}) + C (x_{k+1|k} - \mu_{k+1|k}) + \upsilon \end{align*} where $C = \left.\frac{dg}{dx}\right\vert_{x = \mu_{k+1|k}}$ is again a Jacobian. This still has a non-linear form so Kalman Filter equations cannot be applied yet. We define a new variable $y'_{k+1}$ (a fake observation) by rearranging the above equation, \begin{align*} y'_{k+1} &= y_{k+1} - g(\mu_{k+1|k}) + C \mu_{k+1|k} \tag{4.1}\\ &= C x_{k+1|k} + \upsilon \tag{4.2} \end{align*} Now, this (Equation 4.2) is a linear observation model and we can combine $y'_{k+1}$ and $\hat{x}_{k+1|k}$ linearly to obtain the Kalman gain as, \begin{equation*} K_{k+1} = \Sigma_{k+1|k} C^T (C \Sigma_{k+1|k} C^T + Q)^{-1} \tag{5} \end{equation*} All subsequent steps are similar to Step 2 of Part 2. The new estimator is given as, \begin{equation*} \hat{x}_{k+1|k+1} = \hat{x}_{k+1|k} + K_{k+1} (y'_{k+1} - C\hat{x}_{k+1|k}) \end{equation*} The mean of this updated estimator (from equation 5 of Part 2) is given by, \begin{align*} \mu_{k+1|k+1} &= E [\hat{x}_{k+1|k+1}] = E[\hat{x}_{k+1|k} + K_{k+1} (y'_{k+1} - C\hat{x}_{k+1|k})] \\ &= \mu_{k+1|k} + K_{k+1} (y'_{k+1} - C \mu_{k+1|k}) \end{align*} Resubstituting the fake observation $y'_{k+1}$ in terms of the actual observation $y_{k+1}$ from Equation 4.1, \begin{align*} \mu_{k+1|k+1} &= \mu_{k+1|k} + K_{k+1}((y_{k+1} - g(\mu_{k+1|k}) + C \mu_{k+1|k}) - C \mu_{k+1|k}) \\ &= \mu_{k+1|k} + K_{k+1}(y_{k+1} - g(\mu_{k+1|k})) \tag{6} \end{align*} And the covariance can be written as (from Equation 6.2 of Part 2), \begin{equation} \Sigma_{k+1|k+1} = (I - K_{k+1} C) \Sigma_{k+1|k} \tag{7} \end{equation}

The EKF dramatically expands the applicability of the Kalman Filter. It is very commonly used in robotics and can handle nonlinear observations from complex sensors such as a LiDAR and camera easily. For instance, sophisticated augmented/virtual reality systems like Google AR Core or Snapchat uses EKF to track the motion of the phone or of the objects in the image. The EKF is a clever extension of KF to nonlinear systems but it no longer has the property of optimality (which the KF had for linear systems). There do exist filters for nonlinear systems that will have a smaller mean-squared error than the EKF which we will talk about in the next blog post!