low_pass_filter.cc 1.2 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465
  1. #include "low_pass_filter.h"
  2. namespace one_euro_filter {
  3. void LowPassFilter::SetAlpha(double alpha) {
  4. if (alpha <= 0.0 || alpha > 1.0)
  5. a_ = 0.5;
  6. else
  7. a_ = alpha;
  8. }
  9. LowPassFilter::LowPassFilter() {}
  10. LowPassFilter::LowPassFilter(double alpha, double initval) {
  11. init_alpha_ = alpha;
  12. initval_ = initval;
  13. y_ = s_ = initval;
  14. SetAlpha(alpha);
  15. initialized_ = false;
  16. }
  17. double LowPassFilter::Filter(double value) {
  18. double result;
  19. if (initialized_)
  20. result = a_ * value + (1.0 - a_) * s_;
  21. else {
  22. result = value;
  23. initialized_ = true;
  24. }
  25. y_ = value;
  26. s_ = result;
  27. return result;
  28. }
  29. double LowPassFilter::FilterWithAlpha(double value, double alpha) {
  30. SetAlpha(alpha);
  31. return Filter(value);
  32. }
  33. bool LowPassFilter::HasLastRawValue(void) {
  34. return initialized_;
  35. }
  36. double LowPassFilter::LastRawValue(void) {
  37. return y_;
  38. }
  39. void LowPassFilter::Reset() {
  40. y_ = s_ = initval_;
  41. SetAlpha(init_alpha_);
  42. initialized_ = false;
  43. }
  44. LowPassFilter* LowPassFilter::Clone(void) {
  45. LowPassFilter* new_filter = new LowPassFilter();
  46. new_filter->y_ = y_;
  47. new_filter->a_ = a_;
  48. new_filter->s_ = s_;
  49. new_filter->initialized_ = initialized_;
  50. return new_filter;
  51. }
  52. } // namespace one_euro_filter