Monday, June 10, 2013

Kalman Filter for Dummies

I found a simple 1-dimensional Kalman filter online.  It doesn't really look much better than an IIR filter, to be honest - no control input, no way of factoring in the "ideal" value into the estimate.  But as the math of Kalman filters eludes me, this will have to do for now.

I've ported it to Processing.  Should be easy to convert to Arduino or similar.

// simple Kalman filter
// adapted from C code by Adrian Boeing, www.adrianboeing.com
class KalmanFilter
{
  // initial values for the kalman filter
  float x_est_last = 0;
  float P_last = 0;
  // the noise in the system
  float Q;
  float R;
  float K;    // Kalman gain
  float P;
  float P_temp;
  float x_temp_est;
  float x_est;
  // constructor - initialize with first estimate
  KalmanFilter(float estimate)
  {
    Q = 0.022;
    R = 0.617;
    // initialize with a measurement
    x_est_last = estimate;
  }
  KalmanFilter(float estimate, float initQ, float initR)
  {
    Q = initQ;
    R = initR;
    // initialize with a measurement
    x_est_last = estimate;
  }
  // add a new measurement, return the Kalman-filtered value
  public float step(float z_measured)
  {
    // do a prediction
    x_temp_est = x_est_last;
    P_temp = P_last + R*Q;
    // calculate the Kalman gain
    K = P_temp * (1.0/(P_temp + R));
    // correct
    x_est = x_temp_est + K * (z_measured - x_temp_est);
    P = (1- K) * P_temp;
    // update our last's
    P_last = P;
    x_est_last = x_est;
    return (x_est);
  }
}
And to use it..

  // initial estimate, measurement noise, process noise
  KalmanFilter kf = new KalmanFilter(0, 0.05, 1);
  // update with a new measurement
  float data = kf.step(measurement);

No comments: