KALMAN FILTER IN ARDUINO without Header file kalman.h

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