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.