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