LowPass.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 implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
 
#include <mc_rtc/logging.h>
#include <algorithm>
 
namespace mc_filter
{
 
template<typename T>
struct LowPass
{
  LowPass(double dt, double period = 0) : cutoffPeriod_(period), dt_(dt) { reset(T::Zero()); }
 
  double cutoffPeriod() const { return cutoffPeriod_; }
 
  void cutoffPeriod(double period)
  {
    if(period < 2 * dt_)
    {
      mc_rtc::log::warning("Time constant must be at least twice the timestep (Nyquist–Shannon sampling theorem)");
      period = 2 * dt_;
    }
    cutoffPeriod_ = period;
  }
 
  void reset(const T & value) { eval_ = value; }
 
  void update(const T & newValue)
  {
    double x = (cutoffPeriod_ <= dt_) ? 1. : dt_ / cutoffPeriod_;
    eval_ = x * newValue + (1. - x) * eval_;
  }
 
  const T & eval() const { return eval_; }
 
  double dt() const { return dt_; }
 
  void dt(double dt)
  {
    dt_ = dt;
    cutoffPeriod(cutoffPeriod_);
  }
 
private:
  T eval_;
  double cutoffPeriod_ = 0.;
 
protected:
  double dt_ = 0.005; // [s]
};
 
} // namespace mc_filter