How to sumarize Kalman filter covariances for display?

Question

I'm implementing an extended Kalman filter and I'm facing a problem with showing the covariances to the user.

The covariance matrix estimate contains all the information we have about the current value estimate, but that is too much to display. I would like to have a single number that says "our estimate is really good" when close to 0 and "our estimate is not worth much" when large.

My intuitive simple solution would be to average all the values in the covariance estimate matrix (or maybe just the diagonal), except that in my case the values have different units and different ranges.

Is it possible to do something like this?

Answer 1

It's tough to say anything about a really large covariance matrix. This is because (presumably) the matrix includes the uncertainty about a whole bunch of features, not just the robot pose. Typically what matters in real life is the uncertainty in the position of the robot (and possibly orientation). So with only three spatial deimensions, you can easily display a 3D covariance ellipse which will tell you the expected position and uncertainty of the robot.

In literature there are several methods for displaying some statistics about an arbitrarily large state space. The most common is some measure of the eigenvalues of the matrix. (For a 2x2 or 3x3 matrix, this is often a covariance ellipse as mentioned above) For an nxn matrix, you can display the blockwise covariances for each feature, which will tell you which features are very, very roughly estimated, or you can output some function of the covariance itself.

For example, the determinant is a common measure of uncertainty used in GPS as the Dillution of Precision. Another common measure is the maximum eigenvalue, though that only really makes sense on a feature-by-feature basis.

Answer 2

You want to display $\frac{n(n+1)}{2}$ values of a symmetric $n \times n$ matrix, where $n=26$, which means you have 351 values.

It doesn't make too much sense to display all those numeric values. After all you want to display them to a human being, which is unlikely able to oversee all these values if displayed as numbers.

Then as you say, the values have different units and ranges. that means there's no automatic way of finding the "good" range. I suggest running the system for a while, disturbing it, gathering some rough estimates of where the values are and to what peaks they can rise. With these values you can provide a rough estimate of "good" and "bad", somewhat fuzzy if you will.

Back to the original problem: displaying all those values. A numerical display is out of the question. And that precision isn't even desired, so why not map the values to colors and display each value as a pixel? Make good green and the red bad. Now the job looks a lot simpler: display some 351 pixels¹. It's easy to see when some pixels go red if something goes wrong and what other pixels go "hot" at the same moment. It's also easy to see if some values are generally uncertain, constantly staying in the orange color.

Depending on your needs you can expand that debug facility further, allowing the user to only display a subset of the matrix. Or allow closer inspection, displaying the true values behind the colors and/or plot them to see how they change to the "bad" state.

¹of course you can display these values as large as you want. I'd start with a a 10x10px box for each number, resulting in a 260x260px box, which should be big enough to see clearly, yet small enough to add other debugging/observation tools around it.

Answer 3

Yes you can do so by converting the covariance matrix to a gray level image. For example, in the below picture the covariance matrix is represented as a gray image in which the gray intensity represents the correlation between the elements, the darker the stronger correlation.

More information see FastSLAM A Scalable Method for the Simultaneous Localization and Mapping Problem in Robotics

This shows the correlation. To show how the uncertainty of a quantity decreasing, then plot the $x\pm 3\sigma_{x}$ where $x$ the true value not the estimate. You can't describe the entire system with one single value. Averaging has no meaning in this context. In my opinion, pick an element of your state vector and its uncertainty and plot them according to $x\pm 3\sigma_{x}$. EKF is a sub-optimal filter which means converging to the true value might not be possible as the case in KF. This is due to the linearization step, however, EKF is a good choice and generally yields satisfactory results.