Sunday, December 6, 2009

Java implementation of the Kalman Filter using JAMA

This is a very clear and straight forward implementation of the Discrete Kalman Filter Algorithm in the Java language using the JAMA package. I wrote this code for testing and simulation purposes.

import Jama.Matrix;

/**
* This work is licensed under a Creative Commons Attribution 3.0 License.
*
* @author Ahmed Abdelkader
*/

public class KalmanFilter {
protected Matrix X, X0;
protected Matrix F, B, U, Q;
protected Matrix H, R;
protected Matrix P, P0;

public void predict() {
X0 = F.times(X).plus(B.times(U));

P0 = F.times(P).times(F.transpose()).plus(Q);
}

public void correct(Matrix Z) {
Matrix S = H.times(P0).times(H.transpose()).plus(R);

Matrix K = P0.times(H.transpose()).times(S.inverse());

X = X0.plus(K.times(Z.minus(H.times(X0))));

Matrix I = Matrix.identity(P0.getRowDimension(), P0.getColumnDimension());
P = (I.minus(K.times(H))).times(P0);
}

//getters and setters go here
}
To use this class you will need to create a new instance, set the system matrices (F, B, U, Q, H, R) and initialize the state and error covariance matrices (X, P). When you're done building your filter, you can start using it right away as you might expect. You will need to do the following for each step: 1) project ahead your state by calling predict. 2) update your prediction by creating a measurement matrix with the measurements you received at that step and passing it to the filter through a correct call. I am planning to post a tutorial of this in the next few days. (Update: tutorial posted here)

For more information about the Kalman Filter algorithm, I highly recommend you refer to the webpage maintained by Greg Welch and Gary Bishop. In particular, check their excellent introduction to this interesting topic.

4 comments:

Anonymous said...

Did you finally made this tutorial about your Kalman filter?
I would be very keen on some more explanations!
Regards,
Francois Baret

Anonymous said...
This comment has been removed by a blog administrator.
Ahmed Abdelkader said...

Yes. I updated the post with a link to the tutorial. Please let me know if you have any questions. Good luck.

Anonymous said...

i need all image processing classes in java
where can i find them please ?