ExponentialMovingAverage.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 *
 * This file is inspired by Stephane's Caron original implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
#include <mc_filter/utils/clamp.h>
#include <mc_rtc/logging.h>
#include <algorithm>
#include <cmath>
 
namespace mc_filter
{
template<typename VectorT>
struct ExponentialMovingAverage
{
  ExponentialMovingAverage(double dt, double timeConstant, const VectorT & initValue = VectorT::Zero()) : dt_(dt)
  {
    this->reset(initValue);
    this->timeConstant(timeConstant);
  }
 
  void append(const VectorT & value)
  {
    average_ += alpha_ * (value - average_);
    if(saturation_ > 0.) { utils::clampInPlace(average_, -saturation_, saturation_); }
  }
 
  const VectorT & eval() const { return average_; }
 
  void saturation(double limit) { saturation_ = limit; }
 
  void reset(const VectorT & initVal) { average_ = initVal; }
 
  double timeConstant() const { return timeConstant_; }
 
  void timeConstant(double T)
  {
    if(T < 2 * dt_)
    {
      mc_rtc::log::warning("Time constant must be at least twice the timestep (Nyquist–Shannon sampling theorem)");
      T = 2 * dt_;
    }
    alpha_ = 1. - std::exp(-dt_ / T);
    timeConstant_ = T;
  }
 
protected:
  VectorT average_ = VectorT::Zero();
  double alpha_;
  double dt_;
  double timeConstant_;
  double saturation_ = -1.;
};
 
} // namespace mc_filter