1-dimensional Kalman Filter, Arduino version

Converted the Processing code (which was a conversion of Adrian Boeing's C++ code) to Arduino.

kalman.ino
// simple Kalman filter
// adapted from C code by Adrian Boeing, www.adrianboeing.com 

KalmanFilter::KalmanFilter(float estimate, float initQ, float initR)
{
  Q = initQ;
  R = initR;

  // initial values for the kalman filter
  x_est_last = 0;
  P_last = 0;

  // initialize with a measurement
  x_est_last = estimate;
}


// add a new measurement, return the Kalman-filtered value
float KalmanFilter::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);
}

kalman.h
class KalmanFilter
{
public:
  KalmanFilter(float estimate, float initQ, float initR);
  float step(float measurement);
private:
  // initial values for the kalman filter
  float x_est_last;
  float P_last;

  // 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;
};

and how to use:

// simplistic Kalman filter for encoder readings
KalmanFilter kf(0, 0.01, 1.0);

float avg_err = kf.step(track_err);

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);