Posted on by Eric

The Extended Kalman Filter

What’s better than a Kalman filter? An Extended Kalman Filter! [1] (I’m just kidding, this isn’t from the book, but wouldn’t it be awesome if it was? Most of the other stuff in here is, though.)

Up until now all of our estimators have necessarily required the system dynamics to be linear and time invariant. Unfortunately, most things in the real world are non-linear, which sucks. One of the most common ways of dealing with this when using Kalman filters is to employ the extended Kalman filter.

The main difference between the EKF and standard KF is that the EKF is able to handle non-linear dynamics by linearizing the non-linear system around the Kalman filter estimate, and then the Kalman filter estimate is based on the linearized system. It’s a bit of a mouthful, but essentially we are just linearizing our system around our estimate, and then feeding that back in to the filter. It’s bootstrapping.

Let’s get crackin’!

For the case of the discrete time extended Kalman filter, we have the usual suspects of system and measurement equations, but this time the state transition matrix F and the output matrix H can be nonlinear. 

\begin{aligned}  1) \displaystyle \qquad x_k &= F_{k-1}(x_{k-1}, u_{k-1}, w_{k-1})\\  y_k &= H_k(x_k, v_k) \\  w_k &\sim (0, Q_k) \\  v_k &\sim (0, R_k)\\  \end{aligned}

We then initialize the EKF as before:

\begin{aligned}  2) \displaystyle \qquad \hat{x}_0^+ &= E(x_0)\\  P_0^+ &= E \left[ \left(x_0 - \hat{x}_0^+ \right ) \left(x_0 - \hat{x}_0^+ \right)^T \right ] \\  \end{aligned}

Once we let the EKF start rolling in measurements there is a lot more work to be done now that we are linearizing the system. For each time step we then perform the following step to linearize the state transition matrix about the previous steps a posteriori state estimate:

\begin{aligned}  3) \displaystyle \qquad F_{k-1} &= \frac{\partial{f_{k-1}}}{\partial{x}} \bigg\rvert_{\hat{x}_{k-1}^+}\\  L_{k-1} &= \frac{\partial{f_{k-1}}}{\partial{w}} \bigg\rvert_{\hat{x}_{k-1}^+}\\  \end{aligned}

After computing the partial derivatives update the state estimate and estimation-error covariances:

\begin{aligned}  4) \displaystyle \qquad P_k^- &= F_{k-1}P_{k-1}^+F_{k-1}^T+L_{k-1}Q_{k-1}L_{k-1}^T\\  \hat{x}_k^- &= f_{k-1} \left ( \hat{x}_{k-1}^+, u_{k-1}, 0 \right)\\  \end{aligned}

The time update of the state estimate above just indicates to plug the previous steps a posteriori estimate back in to the original nonlinear system model.

With the a preori state estimate for the current time step calculated, we then linearize the output model and evaluate it about the a preori state estimate.

\begin{aligned}  5) \displaystyle \qquad H_{k} &= \frac{\partial{h_{k}}}{\partial{x}} \bigg\rvert_{\hat{x}_{k}^-}\\  M_{k} &= \frac{\partial{h_{k}}}{\partial{v}} \bigg\rvert_{\hat{x}_{k}^-}\\  \end{aligned}

We then use the linearized output model evaluated about the a preori state estimate to calculate the Kalman gain, and therefore the a posteriori state estimate and a posteriori estimation-error covariance.

\begin{aligned}  6) \displaystyle \qquad K_k &= P_k^- H_k^T \left( H_k P_k^- H_k^T + M_k R_k M_k^T \right ) ^{-1}\\  \hat{x}_k^+ &= \hat{x}_k^- + K-k \left [y_k - h_k(\hat{x}_k^-, 0) \right ]\\  P_k^+ &= \left (I - K_k H_k \right ) P_k^-  \end{aligned}

EKFs, The Popular Crowd Kalman Filter

The EKF is one of the most popular tools for state estimation that you’ll come across within the robotics community. As opposed to tools like the particle filter, the EKF is much more computationally efficient because it represents the belief by a multivariate Gaussian distribution. Just like the standard Kalman filter, each update of the EKF requires time O(k^{2.4} + n^2), where k is the dimension of the measurement vector, and n is the dimension of the state vector.[2]

You may be thinking, “Surely, Eric, this is as deep as it goes,” but you’d be wrong. Why? Well, one of the limitations of the EKF is that we are approximating our state transition and measurement matrices by Taylor expansions (that whole partial derivative thing above). In most real-world applications these matrices are nonlinear (duh, that’s why we were using the EKF in the first place!) but if they are highly nonlinear about our evaluation point this approximation may not be good enough.

How Much More Complicated Can This Get?

Well, we have to keep grad students and profs busy, so there are lots of more complicated variations. We can represent posteriors using mixtures or sums of Gaussians, such is the case of the multi-hypothesis extended Kalman filter, or MHEKF. Then there’s the
information filter, which propagates the inverse of the estimation-error covariance, which can be computationally more efficient if we have significantly more measurements than states. Then there’s the \boldsymbol{ \alpha - \beta} filter, which is actually less complicated because it’s been studied since the days of Newton (it involves a two state Newtonian dynamical system with a position measurement). We have fading memory Kalman filters for when we have a crap system model and we don’t want our estimate to diverge from the true state, The \boldsymbol{H_\infty} filter for when we need a very robust filter to minimize the worst-case estimation error, the hybrid extended Kalman filter, for when we want to combine continuous and discrete time dynamics, the second order extended Kalman filter where we employ a second order Taylor series expansion of our state transition and measurement matrices to reduce the linearization error of the standard EKF, the iterated extended Kalman filter where we perform our linearization process again (or as many times as we like) after getting our a posteriori state estimate. If we want to get even trickier there’s the unscented Kalman filter that aims to further reduce linearization errors present in the EKF by transforming our probability density functions through a nonlinear function (which is difficult) by means of deterministic vectors known as sigma points whose ensemble mean and covariance are equal to our state mean and estimation-error covariance. Finally, we have the very common particle filter that just brute forces its way through estimation, which is pretty awesome now that computation power is relatively cheap.

References

  1. Simon, D. (2006). Optimal State Estimation. Hoboken, New Jersey: Wiley. ISBN: 9780471708582
  2. Thrun, S., Burgard, W.,, Fox, D. (2005). Probabilistic robotics. Cambridge, Mass.: MIT Press. ISBN: 0262201623 9780262201629 

Leave a Reply

Your email address will not be published. Required fields are marked *