Kalman Filter Implementation
Hi All,
I am looking to use a kalman filter to predict the motion of a throw ball. I was wondering if anybody knew of a java implementation of the broad outline of the equations involved in kalman filters, or any specific examples of a java app / set of classes that have a kalman filter.
I am new to this field so don't be too harsh if I say something stupid. I know that obviously the filter is specific to the system modelled, but I don't want to waste time and effort reinventing the wheel of the broader setup that kalman filter follow.
Adam
I can't answer for Java, but I have just got access to SAS at
work, and it does all the things you want--Kalman filter,
non-parametric regression splines etc.
Unfortunately, SAS is a very expensive package, but you could
try to find some documentation, it will tell you what sorts of
things to look for. Maybe the SAS website might help.
Heres the work of kalman http://www.cs.unc.edu/~welch/media/pdf/Kalman1960.pdfbit math heavy for some people but thats the way of the thing.matfud
30 seconds of time to search Google for this
http://www.google.com/search?as_q=java&num=100&hl=en&ie=ISO-8859-1&btnG=Google+Search&as_epq=kalman+filter&as_oq=&as_eq=&lr=lang_en&as_ft=i&as_filetype=&as_qdr=all&as_occt=any&as_dt=i&as_sitesearch=&safe=images
and you could have had your answer at 2:26 instead of 10:43 pm. :)
Hi There,
I have done some further reading, and now have some idea of what is
required. And yes it is easy to implement it in a programming language, now that I know the controlling equations.
My question now is, that I have created a simple setup, with the ball just travelling at a constant velocity in the 3 directions. I am for the moment ignoring air resistance, gravity etc.
What I need now to do is calculate / state the initial values in the Q,P and R matrices. What do I do for this, pick some values that a reasonable? OR is there a better way? For Instance, I don't know the measured error for the initial point(s), only that say the error isn't going to metres wrong, more like a few cm.
Adam
Hi,
I am trying to understand the relevance of a Kalman Filter to your problem! A Kalman Filter is used to get optimal estimates of states based on -
1) An assumed model of the dynamics of your process
2) A set of noise measurements
3) A set of statistics regarding the nature of the uncertainties (P, Q, R etc).
I see nothing in your origninal posting to indicate that you have a set of measurements. Do you?
Roger
Yes I do, I have written an object tracking application. I have used two cameras, and from these two views, I have found a set of 3d coordinates (one for each frame). The results I achieve are fairly accurate, however firstly I want to smooth these results to fit the known physical model, and then extrapolate these smoothed results to see where the ball would go in the future.
Like I said in my previous post, I have set up a simple physical model of situation, but I want to know what I should be putting in the P,Q and R matrices initially.
Sorry I failed to understand. The covariance matrixes should be setup to your initial guess as to the uncertainty in your values. If you know nothing about the error (noise) sources then start off with some large values for the diagonal elements and zero for the rest.
The orignal work by Kalman only applies when you you know the inital covariances but my experience is that it is not too critical.
I'm having to think back to work I did 5 years ago but I do remember that I used to run the simulations several times using different starting points for the initial states and covariances and then see which ones gave the best fit.
Roger
Ok I see. So it is a bit of trial and error. How do I work out the covariances through experiment? Say I know that my measured value of the position in each direction (x,y,z) is within 3cm, can I form the appropriate covariance matrix for one of the required matrices?
And can you tell me what each of Q,P and R relate to exactly? In terms of a real life example would be a great help.
Adam
Sorry should of said the initial estimate of position was 'within 3cm' at time = 0!Adam
Hi Again,
Assuming that the 3cm is 1 standard deviation then your P matrix and the initial velocity of the ball is zero then your P matrix (uncertainty of initial state) should be initialized at (0.03)^2 (mks units) for the position diagonal element and 0 for the velocity diagonal element. I would expect you to have at least one more state to deal with windage but you may be able to neglect this.
The Q matrix (uncertainty in input) can probably be initialed to zero since your input is essentially gravity so should be well known.
The R matrix (uncetrainty of measurement) I would expect to be 2 dimential (one for each camera) and probably be consant at (0.03)^2 for each diagonal element and zero for the rest. I assume that your measurements for the two cameras can be considered independent!
The only real problem I see is how to deal with the bounce discontinuity. Going back many years to my school days I remember that we modelled a bouncing ball as the vertical velocity changing sign and magnitude being scaled by some factor 'e' (I think it was called the coefficeint of restitution). The horizontal velocity was assumed to be constant) The Kalman filter deals with linear (or near linear) systems so I would be interested in how you are ging to deal with this discontinuity.
Are you modeling ball spin?
This sounds a very interesting project! Could you post the equations of your model?
Roger
Should have said that I am not tracking the ball from stationary. I am tracking the ball from about half way down the pitch for about 2.5m. I need then to predict about another 2m ahead to where the ball was going to finish.
I am neglecting before the bounce at the moment, and am only trying to fit and predict after the bounce, to eliminate the problem that you mentioned.
How does this effect the P,Q,R matrices?
At present my model is very very simple, constant velocity in all directions. However, this isn't my final plan. I have just purchased a book that is concerned with the physics of sport, and it has a chapter all about cricket physics. After a quick look, I can definitely add in gravity and air resistance with little problems. Would end you just considering postion and velocity in x and y direction as my X vector. As for spin and swing, at the moment I haven't got that far through the book, but I think that takes into consideration the rotational speed of the ball, which I don't know if I will be able to determine from my tracking.
You can check out my web site at www.dcs.warwick.ac.uk/~csvnd for more information on my project. I would appreciate any feedback and comments.
If you keep peeled to this forum and my web site, I will put up the equations and the resultant A matrix ASAP. Cos an extra pair of eyes looking at them would be a good check.
Adam
Hi Again,
I can't access your web site - it seems I am not allowed!
Thinking about your system I see now a minimum of 4 states. X and Y position and X and Y speed.
Assuming you are using some form of bowling machine then I would expect you to be able to predict with reasonable accuracy the initial position and velocity in both X and Y and these predications should be the initial values for your state variables.
The inital values for the P matrix (4x4?) will depend on how well you can predict the inital values. I would expect that it cannot be better than your 3cm in position but you will have to get information from the bowling machine manufacturer to get both the position and the speed uncertainty. There will almost certainly be a correlation between the position and speed uncertainty but I would bet that you won't be able to get any information. This will probably require a trial and error approach.
Assuming that you can only measure the position in X and Y then the R matrix it probably 2x2, constant and will most likely only have non-zero diagonal. These will probably have value 0.03^2 .
The Q matrix will be 4x4 and probably will be the NULL matrix since it just represents the uncertainty in stimulus and the values for gravity are pretty well known!
I do remember from the work I did that one of the best ways to measure the goodness of the fit was to look at the residuals. If you have a good fit then they should be 'white'.
When you add in spin, bounce etc then you have a much bigger 'challenge'. Be aware that adding in 2nd order effects may not improve your estimation!
Many years ago I did an MSc in Control Systems at Hatfield Polytechnic. Many of my work colleges did an MSc in Control based mainly at Warwick. We shared many guest lecturers - some good, some not so good.
Best of luck.
Roger
You web page is not publically visable.Who is your project supervisor? (I graduated from warwick DCS about 6years ago :)matfud
My web site is public, just the server has died today apparently. So if you check it out tomorrow it will be fine, I'm sure. I know many people who have accessed it from outside, so it is not a rights issues.Adam
My tracking is of real deliveries! Not from a bowling machine, so can't quite do what you said. I also have the 3d coordinates, rather than just 2D as you were suggesting.
I am just making an educated guess from the video clips and the 3d coordinates my tracking application is giving me that the error is only a few cm's, from marking on the picture that I can roughly determine where the ball really is.
It is my plan to go back and do some more filming in the next month or two, to really find out the sorts of errors I am getting due to calibration of the cameras, and from the object tracking algorithm.
Again, how do you think this will affect the matrices?
Arrh, another warwick graduate! You won't have been here for the new all singing all dancing dcs block (5 millions worth i think), well plus the new lake they have built out front (with flash fountains in it to!). Thank MR IBM, for the rather large donatations!
I have Roland Wilson supervising my project, an interesting choice I am sure you will say, if you know him. Certainly have lively project meeting!
>
> Arrh, another warwick graduate! You won't have been
No, I got my Masters from Hatfield Poly - we just shared some guest lecturers!
I shall have to think about the 3D version. You will certainly have at least 6 states (3 position and 3 velocity) but my initial reactions are the same as for 4 states. The addition two states changes little except the initial uncertainty.
I'll take a look at your site later.
Roger
ok sabre, cheers, I was talking to mathfub about warwick, he posted above about been fromw Warwick.
Oh other thing, just wanted to check that we are all using P,Q, and R for the same things
P - covariance matrix associated with the state
Q - process noise convariance matrix
R - measurement noise covariance matrix
Sounds right what ppl having been saying that the names I have have just checked with the equations I have found. Just want to be certain singing off the same hymn sheet!
Adam
Roland Wilson. Good luck. If I remember correctly he doesn't suffer
fools gladly :P
I think you should perhaps do, as was suggested earlier, the calculations
on a 2D (top down) representation of the model. Just to start of course.
This should remove the non-linear effect of the ball bounce while
still allowing you to model effects such as windage, spin (through
the air) and air resistance. These probably are not seperable effects
as I doubt you will
have enough information from your cameras to form a seperate model for
each. You can model their combined effects though and the effects should
be observable from position changes in the data that exceed the
estimated measurement errors. Might be worth a try.
From a top down view the ball should travel in a straight line at a
constant velocity. All deviation from that ideal is due to the second
order effects.
As for extending into 3D and handling the bounces. The simplest method is
to reset the model (reititialise it) after each bounce. You can use the
information contained in the model prior to the bounce to provide the
intitial values for the next segment of the balls journey. Remeber that
if the ball has spin it will curve through the air (slightly) and
change direction (quite sharply) when it hits the ground.
The major problem with that approach is that it does not allow
particularaly good predictions to be made about the future as you
can't predict accurately past the next bounce.
If the ball was bouncing a lot then I would suggest using a seperate
Kalman filter to model changes at each bounce. It would be provided with
information only at each bounce (not all the bits in between) an should
therefore act as a kind of meta data asto which bounce effects due to
interaction between the ball and the ground can be "seen".
Problem is I don't like that idea and you probably won't get enough
bounces to make it worth while (if this is cricket)
matfud
(Nope I havn't seen the new DCS building. Is the old one still around?)
> Oh other thing, just wanted to check that we are all
> using P,Q, and R for the same things
>
> P - covariance matrix associated with the state
> Q - process noise convariance matrix
> R - measurement noise covariance matrix
>
This is what I assumed!
Roger
Yes, you have Roland spot on!
Looked at your suggestion, not sure about it. You said for starter ball will travel with constant speed, which is won't because of air resistance.
I have found the following equation for the ball in 2d considering gravity and air resistance, and ignoring spin and swing for the moment. The air resistance is set to be proportion to velocity. I have a book about the physics of sports including cricket, and it says that air resistance is really proportional to velocity squared, but the resultant formulae are complex and not easily solved.
(d^2r/dt^2) = -g.Y - k.V
which can be transformed into
X = (xDot(t0)/E).(1-e^(-E.T))
Y = (yDot(t0)/E).(1-e^(-E.T)) + (1/E^2).(1-E.T-E^(-E.T))
where xDot(t0) is initial x velocity, same for yDot(t0).
e is expotential
E = (k.Vo)/g
I have transformed them into the following,
x(t+1) = x(t) + (xDot(t).((1/E).(1-e^(-E.FT)))
y(t+1) = y(t) + (yDot(t).((1/E).(1-e^(-E.FT))) + ((1/E^2).(1-E.FT-e^(-E.FT)))
xDot(t+1) = xDot(t).(e^(-E.FT))
yDot(t+1) = yDot(t).(e^(-E.FT)) + (1/E).(e^(-E.FT) - 1)
where FT is the time of one frame.
Only problem I have with this, is I will need a 5 dimension vector, as I need x,y,xDot,yDot,and 1. I think so anyway, I need the 1 column, to allow me to add the time based only varibles on, that aren't related to x,y,xDot or yDot. What do you guys think?
Next problem, should really model air resistance as proportional to velocity, should be velocity squared.
Therefore,
(d^2r/dt^2) = -g.Y - k.v^2.V
However, reading in this book can't just rearrange formulae to get solutions, have to use numerical technicals. They mentioned Runge-Kutta? Don't know how to get this equation into a matrix form for x,y,xDot,yDot or even if that is possible? Any suggestions!
Cheers,
Adam
>Looked at your suggestion, not sure about it. You said for starter
>ball will travel with constant speed, which is won't because of air
>resistance.
I was trying to say that if you discount things such as air resistance,
crosswinds and spin then in the topdown view the ideal ball would
travel at a constant velocity. (you also don't need to model gravity
in this limited representation)
matfud
> However, reading in this book can't just rearrange
> formulae to get solutions, have to use numerical
> technicals. They mentioned Runge-Kutta? Don't know how
> to get this equation into a matrix form for
> x,y,xDot,yDot or even if that is possible? Any
> suggestions!
>
You need to setup a 'state space' representation of the equations. Look in your library for this. With this Runge-Kutta (RK) is very easy because 2nd order RK only requires the current state and it's derivative which the 'state space' representation gives you.
As far as wind resistance is concerned. I suggest that you use the square law relationship for wind resistance and linearise the equations around some nominal velocity. Since wind resistance is likely to be a second order effect the errors introduced by linearization should be small.
Roger
I don't think I am going to worry about the effects of wind.
As for state space representations, I am having a look into it. However, how do you think it will convert the equation mentioned above i.e
(dx^2/dt^2) = - k.(dx/dt)^2
and
(dy^2/dt^2)= - g - k.(dy/dt)^2
Taking into consideration I need to calculate the x,y,xDot, and yDot for each state only using the values known for the previous state so that I have make the matrix A for the equation
x(t+1) = A x(t)
I have been looking into this problem for a long time, and I know there must be a solution to this, but still not been able to get there.
Adam
> Taking into consideration I need to calculate the
> x,y,xDot, and yDot for each state only using the
> values known for the previous state so that I have
> make the matrix A for the equation
>
> x(t+1) = A x(t)
>
> I have been looking into this problem for a long time,
> and I know there must be a solution to this, but still
> not been able to get there.
If you look at RK 2 you will quickly realise that it gives you exactly what you want!
In essence, given the continuous system dx/dt = A x where x is that state space vector and A the continuous square state transition matrix then the discrete version is
x(t+dt) = exp(-A dt) x(t) .
I think you should spend some time looking at state space for both continuous and discrete system. It would make all this clear.
Roger
What do you mean by RK2? Sorry if I'm being really dumb. Also could you recommend any website or books that I should read for the topic you mention.Adam
> What do you mean by RK2? Sorry if I'm being really
> dumb. Also could you recommend any website or books
> that I should read for the topic you mention.
>
> Adam
Second order Runge-Kutta. Check in your University library.
Within a forum it is almost impossible give details of the things you will need and, though I understand enough myself, I am not current enough to teach what you need to know. Most of the references I could give you are out of date.
Though you could treat the whole as a black box you will do a lot better to understand what you are doing.
As a minimum you will need to learn about -
1) State Space.
2) Linear Systems (continuous and sampled).
3) Solutions to linear matrix differential equations.
4) Solutions to linear matrix difference equations.
5) Numerical integration.
Time spent now in understanding these will pay vast dividends later.
Roger
Thanx for everybodies help on this topic, I have managed to get my kalman filter with a RK2 based A matrix working ok in 2D (Vertical Plane). A little of fudging to get the model to fit the real world situation, should now do it.
Only one small but painfully hard problem to solve now. How to deal the horizontal movement due to swing and spin. I have a book on the physics of projectiles in sport, and gives one the equations the these principles follow in theory. But you need a wind tunnel and a bowling machine for them to be really useful.
For starter you need to now the rate of rotation of the ball, something that I will never be able to calculate from my tracking application. So I suppose the question is, what I am going to do to fit a smooth path through a ball that may or may not be swinging, and then to predict the extent of this swing out of the range of the tracking.
I think the problem is that the majority of deleveries will be pretty much straight on, and a few where the lateral movement is linear-ish! Should be no problems with those. Then the third case seems to be the big swinging deliveries, especially though that swing late.
I can deal with all these deleveries in isolation, a best fit least squares linear approach for the first 2 cases. And a best fit least squares parabolic fit for the last case.
I have messed with this approach for the vertical plane, before splines and finally kalman filters, and they seemed ok.
Problem is how do I decided which fitting technique to use? Or is there a better way of fitting through the lateral movement and then predicting it in the near future?
Nearly there now, just this last major hurdle to overcome and I should have most of it sorted. However, could be a serious problem.
Adam
