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