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:

Post a Comment