This is a basic Kalman filter that uses a constant measurement noise and process noise. You can adjust these values to fine-tune the filter for your specific application. The filter takes a measurement (z_measured) and uses it to estimate the real value (z_real). The estimated value is then updated with each new measurement.
#define MEASUREMENT_NOISE 0.1
#define PROCESS_NOISE 0.05
double x_est_last = 0;
double P_last = 0;
double K;
double P;
double x_temp_est;
double x_est;
double z_measured = 0; // the measured value
double z_real = 0; // the real value
void KalmanFilter()
{
x_temp_est = x_est_last;
P = P_last + PROCESS_NOISE;
K = P / (P + MEASUREMENT_NOISE);
x_est = x_temp_est + K * (z_measured - x_temp_est);
P = (1 - K) * P;
x_est_last = x_est;
P_last = P;
}
No comments:
Post a Comment