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

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

}

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

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