TorqueFilter/IIRFilter.h

Go to the documentation of this file.

// -*- C++ -*-

#ifndef IIR_FILTER_H
#define IIR_FILTER_H

#include <vector>
#include <deque>
#include <string>
#include <cmath>
#include <iostream>

class IIRFilter
{
public:
    IIRFilter(const std::string& error_prefix = "");
    IIRFilter(unsigned int dim, std::vector<double>& fb_coeffs, std::vector<double>& ff_coeffs, const std::string& error_prefix = "");
    ~IIRFilter() {};

    bool setParameter(int dim, std::vector<double>& A, std::vector<double>& B);

    bool setParameterAsBiquad(const double f_cutoff, const double Q, const double hz);

    void getParameter(int &dim, std::vector<double>&A, std::vector<double>& B);

    void reset(double initial_input = 0.0);

    double executeFilter(double input) {
        // std::cerr << "executeFilter will be obsolated." << std::endl;
        return passFilter(input);
    };
    double passFilter(double input);
    // double getCurrentValue () const { return m_prev_output; };
private:
    // Configuration variable declaration
    // <rtc-template block="config_declare">
    int m_dimension;
    std::vector<double> m_fb_coefficients; // fb parameters (dim must be m_dimension + 1, m_fb_coefficients[0] would be 1.0)
    std::vector<double> m_ff_coefficients; // ff parameters (dim must be m_dimension + 1)
    std::deque<double> m_previous_values;
    // double m_prev_output;
    bool m_initialized;
    std::string m_error_prefix;
};

template <class T> class FirstOrderLowPassFilter
{
private:
    T prev_value;
    double cutoff_freq, dt, const_param;
public:
    FirstOrderLowPassFilter (const double _cutoff_freq, const double _dt, const T init_value) : prev_value(init_value), dt(_dt)
    {
        setCutOffFreq(_cutoff_freq);
    };
    ~FirstOrderLowPassFilter()
    {
    };
    T passFilter (const T& value)
    {
        prev_value = 1.0/(1+const_param) * prev_value + const_param/(1+const_param) * value;
        return prev_value;
    };
    void reset (const T& value) { prev_value = value; };
    void setCutOffFreq (const double f)
    {
        cutoff_freq = f;
        const_param = 2 * M_PI * cutoff_freq * dt;
    };
    double getCutOffFreq () const { return cutoff_freq; };
    T getCurrentValue () const { return prev_value; };
};

#endif // IIRFilter_H