KalmanFilter.h

#pragma once
 
namespace audio_tools {
 
class KalmanFilter {
 public:
  KalmanFilter(float process_noise = 0.01f, float measurement_noise = 1.0f) {
    begin(process_noise, measurement_noise);
  }
 
  bool begin(float process_noise, float measurement_noise) {
    Q = process_noise;
    R = measurement_noise;
    P = 1.0;
    X = 0.0;
    return true;
  }
 
  bool begin() {
    X = 0;
    return true;
  }
 
  void end() { X = 0; }
 
  void addMeasurement(float measurement) {
    // Prediction update
    P = P + Q;
 
    // Measurement update
    K = P / (P + R);
    X = X + K * (measurement - X);
    P = (1 - K) * P;
  }
 
  float calculate() { return X; }
 
 protected:
  float Q;
  float R;
  float P;
  float X;
  float K;
};
 
}  // namespace audio_tools