A question about Extended Kalman Filter

AI Thread Summary
The discussion revolves around implementing an Extended Kalman Filter (EKF) for vehicle tracking in a 3D model-based system. The user faced issues with deriving the matrices F and H and experienced unexpected behavior in the filter's predictions, particularly during vehicle turns. After receiving feedback, it was identified that the problem stemmed from inconsistent angle representations, where the vehicle's orientation was sometimes reported as negative, causing the EKF to diverge. The user acknowledged that representing rotational states can be challenging, even with quaternions, due to their mathematical properties. Ultimately, the user resolved the issue by correcting the angle representation in the localization module.
steven_mx
Messages
2
Reaction score
0
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, Vk and Qk, 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
 
Last edited by a moderator:
Mathematics news on Phys.org
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
 
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.
 
Last edited:
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!
 
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.
 
Suppose ,instead of the usual x,y coordinate system with an I basis vector along the x -axis and a corresponding j basis vector along the y-axis we instead have a different pair of basis vectors ,call them e and f along their respective axes. I have seen that this is an important subject in maths My question is what physical applications does such a model apply to? I am asking here because I have devoted quite a lot of time in the past to understanding convectors and the dual...
Insights auto threads is broken atm, so I'm manually creating these for new Insight articles. In Dirac’s Principles of Quantum Mechanics published in 1930 he introduced a “convenient notation” he referred to as a “delta function” which he treated as a continuum analog to the discrete Kronecker delta. The Kronecker delta is simply the indexed components of the identity operator in matrix algebra Source: https://www.physicsforums.com/insights/what-exactly-is-diracs-delta-function/ by...

Similar threads

Replies
1
Views
2K
Replies
1
Views
1K
Replies
4
Views
2K
Replies
2
Views
3K
Replies
24
Views
11K
Replies
1
Views
9K
Replies
24
Views
8K
Back
Top