filters.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#ifndef __FILTERS_H__
#define __FILTERS_H__
 
#include <deque>
 
#include <yarp/sig/Vector.h>
#include <iCub/ctrl/math.h>
 
 
namespace iCub
{
 
namespace ctrl
{
 
class IFilter
{
public:
    virtual ~IFilter() { }
 
    virtual void init(const yarp::sig::Vector& y0) = 0;
 
    virtual const yarp::sig::Vector& filt(const yarp::sig::Vector& u) = 0;
 
    virtual const yarp::sig::Vector& output() const = 0;
};
 
 
class Filter : public IFilter
{
protected:
   yarp::sig::Vector b;
   yarp::sig::Vector a;
   yarp::sig::Vector y;
 
   std::deque<yarp::sig::Vector> uold;
   std::deque<yarp::sig::Vector> yold;
   size_t n;
   size_t m;
 
public:
   Filter(const yarp::sig::Vector &num, const yarp::sig::Vector &den,
          const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0));
 
   virtual void init(const yarp::sig::Vector &y0);
 
   virtual void init(const yarp::sig::Vector &y0, const yarp::sig::Vector &u0);
 
   void getCoeffs(yarp::sig::Vector &num, yarp::sig::Vector &den);
 
   void setCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den);
 
   bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den);
 
   void getStates(std::deque<yarp::sig::Vector> &u, std::deque<yarp::sig::Vector> &y);
 
   virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
 
   virtual const yarp::sig::Vector& output() const { return y; }
};
 
 
class RateLimiter : public IFilter
{
protected:
    yarp::sig::Vector uD;
    yarp::sig::Vector uLim;
    yarp::sig::Vector rateUpperLim;
    yarp::sig::Vector rateLowerLim;
 
    size_t n;
 
public:
    RateLimiter(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU);
 
    virtual void init(const yarp::sig::Vector &u0);
 
    void getLimits(yarp::sig::Vector &rL, yarp::sig::Vector &rU);
 
    void setLimits(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU);
 
    virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
 
    virtual const yarp::sig::Vector& output() const { return uLim; }
};
 
 
class FirstOrderLowPassFilter : public IFilter
{
protected:
    Filter *filter;         // low pass filter
    double fc;              // cut frequency
    double Ts;              // sample time
    yarp::sig::Vector y;    // filter current output
 
    void computeCoeff();
 
public:
    FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime,
                            const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0));
 
    virtual ~FirstOrderLowPassFilter();
 
    virtual void init(const yarp::sig::Vector &y0);
 
    bool setCutFrequency(const double cutFrequency);
 
    bool setSampleTime(const double sampleTime);
 
    double getCutFrequency() { return fc; }
 
    double getSampleTime() { return Ts; }
 
    virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
 
    virtual const yarp::sig::Vector& output() const { return y; }
};
 
 
class MedianFilter : public IFilter
{
protected:
   std::deque<std::deque<double> > uold;
   yarp::sig::Vector y;
   size_t n;
   size_t m;
 
   double median(std::deque<double>& v);
 
public:
   MedianFilter(const size_t n, const yarp::sig::Vector &y0=yarp::sig::Vector(1,0.0));
 
   virtual void init(const yarp::sig::Vector &y0);
 
   void setOrder(const size_t n);
 
   size_t getOrder() const { return n; }
 
   virtual const yarp::sig::Vector& filt(const yarp::sig::Vector &u);
 
   virtual const yarp::sig::Vector& output() const { return y; }
};
 
}
 
}
 
#endif