Visualizing and debugging an EKF

Question

I am currently debugging and tuning an EKF (Extended Kalman Filter). The task is classical mobile robot pose tracking where landmarks are AR markers.

Sometimes I am surprised how some measurement affects the estimate. When I look at and calculate the numbers and matrices involved, I can work out how the update step got executed, what and why exactly happened, but this is very tedious.

So I wonder if anyone is using some technique, trick or clever visualization to get a better feel of what is happening in the EKF update step?

UPDATE #1 (will be more specific and show first approximation of what I have in mind)

What I am looking for, is some way to visualize one update step in a way that gives me a feel of how each component of the measurement affects each component of the state.

My very first idea is to plot the measurement and it's prediction together with some vectors taken from the K matrix. The vectors from K represent how the innovation vector (measurement - measurement prediction, not plotted) will affect each component of the state.

Currently I am working with an EKF where the state is 2D pose (x,y,angle) and the measurements are also 2D poses.

In the attached image(open it in new page/tab to see in full resolution), the (scaled) vector K(1,1:2) (MATLAB syntax to take a submatrix from 3x3 matrix) should give an idea how the first component of the EKF state will change with the current innovation vector, K(2,1:2) how the second component of EKF will change, etc. In this example, the innovation vector has a relatively large x component and it is aligned with vector K(2,1:2) - the second component of the state (y coordinate) will change the most.

One problem in this plot is that it does not give a feel of how the third component(angle) of the innovation vector affects the state. The first component of the state increases a bit, contrary to what K(1:1:2) indicates - the third component of the innovation causes this, but currently I can not visualize this.

First improvement would be to visualize how the third component of the innovation affects the state. Then it would be nice to add covariance data to get a feel how the K matrix is created.

UPDATE #2 Now the plot has vectors in state-space that show how each component of measurement changes the position. From this plot, I can see that the third component of the measurement changes the state most.

Answer 1

A very informative way to visualize the effect of measurements (for me) is to plot the state of the robot (mean, with covariance ellipse) before and after each measurement. Then, take the individual components of the measurement (bearing, range for AR markers), and apply them separately to get a feel for it.

To do this:

I use one or more of these functions to plot the ellipse. To find the constants $a,b$, note that they are the square roots of the eigenvalues of the covariance matrix. Then sample the angle $\theta$ over $[0, 2\pi]$ and find the range from the hypothesis. using the linked equation. I especially recommend using this equation:

$$ r(\theta) = \frac{ab}{\sqrt{b\cos^2\theta + a\sin^2\theta} } $$

Tracking the covariance of the prior hypothesis, measured state, and posterior hypothesis is usually sufficient to find if the equations of an EKF are being applied correctly.

Good luck, and don't update your question too frequently. Instead, come back with new questions.

Answer 2

Something that is often done is to plot the state variables over time as well as their 3-sigma intervals. Points where this interval shrinks are the updates, where you could possibly annotate the source of the measurements involved.

Besides errors in implementation which should be checked (not only wrong equations, but numerically unstable equations as well), the effect of updates is only directly affected by the difference between what is "expected" and "measured" and their respective uncertainties. So you might be interested in figuring out a way to visualize this balance in terms of the time progression in the first plot.