KalmanFilter.java

package lejos.util;

/*
 * WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
 * DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
 */

public class KalmanFilter {
  private Matrix a, b, c, i, q, r, at, ct;
  private Matrix mu, sigma, muBar, sigmaBar, gain;
  
  public KalmanFilter(Matrix a, Matrix b, Matrix c, Matrix q, Matrix r) {
    this.a = a;
    this.b = b;
    this.c = c;
    this.q = q;
    this.r = r;
    this.at = a.transpose();
    this.ct = c.transpose();
  }
  
  public void setState(Matrix mean, Matrix covariance) {
    this.mu = mean;
    this.sigma = covariance;
    int n = mu.getColumnDimension();
    this.i = Matrix.identity(n, n);
  }
  
  public void update(Matrix control, Matrix measurement) {
    // Control update step 1: calculate the predicted mean
	muBar = a.times(mu).plus(b.times(control));
    
    // Control update step 2: calculate the predicted covariance
    sigmaBar = a.times(sigma).times(at).plus(r);
   
    // Calculate the Kalman Gain   
    gain = sigmaBar.times(ct).times(c.times(sigmaBar).times(ct).plus(q).inverse());
    
    // Measurement update: calculate the new mean
    mu = muBar.plus(gain.times(measurement.minus(c.times(muBar))));
    
    // Calculate the new covariance
    sigma = i.minus(gain.times(c)).times(sigmaBar);
  }
  
  public Matrix getMean() {
    return mu;
  }
  
  public Matrix getCovariance() {
    return sigma;
  }
  
  public Matrix getPredictedMean() {
	  return muBar;
  }
  
  public Matrix getPredictedCovariance() {
	  return sigmaBar;
  }
  
  public Matrix getGain() {
	  return gain;
  }
}