A question about Extended Kalman Filter

[steven_mx]

Hi,

I'm currently implementing a visual 3D model-based vehicle tracking system as my undergrad dissertation. I've implemented the vehicle localization algorithm and now have an estimate of the x, y location on the ground plane as well as the orientation angle of the vehicle which would like to track using an extended Kalman filter.

I have read the paper by Lou et al. which can be found at http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.132.8981&rep=rep1&type=pdf This should have pretty much what I need. However, I'm having problems understanding how to get the matrices F and H from f(X) and h(X) respectively. I also read the paper at http://academic.csuohio.edu/simond/pubs/ESDNonlinear.pdf which seems to be very similar to what I am implementing, however I'm getting funny results, notably:

- When the vehicle does a turn, the model orientation (predicted by Kalman filter) keeps going round and round in circles instead of following the vehicle motion.

To understand what I mean, take a look at what I've been doing so far. This is before I apply the EKF (my estimates from the vehicle localization module): and this is after I apply the EKF:

I am also unsure about the initial values I should set for the measurement and error covariances, V_k and Q_k, so I'm not sure whether the problem is with the way I derived F and H or whether I got the initial values for the covariances wrong.

I have already spent 2 weeks on this and have run out of ideas. I would really appreciate if someone could give me an idea on how to make it work. Thanks a lot in advance.

Steven

 

[chingkui]

I only read partially the first paper, it appears that f(x) could mean two different thing in the paper: in Eq 1, x_dot=f(x) and in Eq 6, x_k+1=f(x_k). You have to linearize the differential eq model of eq 1 to get x_dot=Ax, then relate this to the discrete version (difference equation: x_k+1= F x_k) through some exponential matrix. It appears the matrix H is already written out explicitly in Eq 2, isn't it?
When I debug Kalman Filter, I usually print out step by step how the covariance and gain are computed, and this usually would lead to insight as to why some terms are too sensitive or got overcorrected. I do not have the experience in your field so I really have no idea how u would get werid result. The only general rule is the obvious one: check your model and equations and make sure they are correct, and look at how covariance value influence the gain, try using different initial value and noise statistic

 

[D H]

You have three videos, one labeled before.avi, another kalman.avi, and the third "Visual Model-Based Vehicle Tracking using an Extended Kalman Filter". That third one with the long title looks pretty good. Is this third one not your work, something to show that the problem is tractable?

Anyhow, the first one looks like you have some Kalman-like methodology going on as the noise is strongest when the vehicle is smallish (further away from the camera) and is turning. There appears to be a good lock in that section where the vehicle is going straight.

The second one is obviously the problem. There is a basic problem with the EKF, and you may have run into it. When the filtered solution is far from the truth (and hence far from the measurements) the filter can diverge. You may need to adjust your tuning parameters. Take in a bit more or bit less measurement than your sensor model suggests, for example. Sometimes you have to lie to the filter about the performance of your sensors. Or lie to the filter about plant noise. Or both. Filter tuning can be a bit more art than science.

It also looks like you may have some problems with your state descriptions or your derivatives. The goofy behavior always appears to start with a step change in the filtered solution. It then rotates for a short while, makes another step change, rotates for a bit, ..., before finally locking on to something close to the truth. Are you using Euler angles for rotational state? If so, you may be running into a singularity/gimbal lock.

 

[steven_mx]

Hi,

Thank you very much for your replies. The third video is my work... I finally traced the problem to the inputs :) However I couldn't reply to this thread since it was still being accepted by a mentor.

The problem was pretty silly but for some reason I missed it: I am representing the vehicle orientation as the angle of the vehicle around an axis perpendicular to the ground plane. Conceptually, if the car is at 60 degrees in one frame and at -301 degrees in the other frame, the car has only moved by 1 degrees. However for the Kalman filter it would have moved by 361 degrees. Most of the time the vehicle localization module was giving out positive angles, but at random times it was giving out negative angles, causing the EKF to diverge. When drawn on screen, the model looks perfectly fine as you can see from before.avi, so probably that's why I didn't suspect the problem before.

Thank you again for the help!

 

[D H]

There will always be some problems with representing rotational state. Even quaternions are not immune. A unit quaternion used to represent rotations in 3-space and its additive inverse are indistinguishable insofar as rotation is concerned. A Kalman filter doesn't know that, and this can lead to all kinds of interesting, but goofy, filter behavior.