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.