Go to the documentation of this file.00001 #include "IIRFilter.h"
00002 #include <numeric>
00003
00004 IIRFilter::IIRFilter(unsigned int dim, std::vector<double>& fb_coeffs, std::vector<double>& ff_coeffs, const std::string& error_prefix)
00005 {
00006 std::cerr << "This IIRFilter constructure is obsolated method." << std::endl;
00007 m_dimension = dim;
00008 m_error_prefix = error_prefix;
00009
00010
00011 if(fb_coeffs.size() != dim + 1|| ff_coeffs.size() != dim + 1){
00012 std::cout << "[" << m_error_prefix << "]" << "IIRFilter coefficients size error" << std::endl;
00013 return;
00014 }
00015 for(std::vector<double>::iterator it = fb_coeffs.begin(); it != fb_coeffs.end(); it++){
00016 m_fb_coefficients.push_back(*it);
00017 }
00018 for(std::vector<double>::iterator it = ff_coeffs.begin(); it != ff_coeffs.end(); it++){
00019 m_ff_coefficients.push_back(*it);
00020 }
00021
00022
00023 m_previous_values.assign(dim, 0.0);
00024 m_initialized = true;
00025 return;
00026 }
00027
00028 IIRFilter::IIRFilter(const std::string& error_prefix) :
00029 m_initialized(false) {
00030 m_error_prefix = error_prefix;
00031 }
00032
00033 bool IIRFilter::setParameter(int dim, std::vector<double>& A, std::vector<double>& B) {
00034 m_dimension = dim;
00035
00036
00037 if((A.size() != dim && A.size() != dim + 1) || B.size() != dim + 1) {
00038 std::cout << "[" << m_error_prefix << "]" << "IIRFilter coefficients size error" << std::endl;
00039 return false;
00040 }
00041
00042
00043 m_fb_coefficients.clear();
00044 m_ff_coefficients.clear();
00045
00046 if (A.size() == dim) {
00047 m_fb_coefficients.push_back(1.0);
00048 }
00049 for(std::vector<double>::iterator it = A.begin(); it != A.end(); it++){
00050 if (it == A.begin()) {
00051 if( *it != 1.0 ) {
00052 std::cout << "[" << m_error_prefix << "]" << "IIRFilter : parameter A[0] is not 1.0 !!!" << std::endl;
00053 }
00054 m_fb_coefficients.push_back(*it);
00055 } else {
00056 m_fb_coefficients.push_back(- *it);
00057 }
00058 }
00059 for(std::vector<double>::iterator it = B.begin(); it != B.end(); it++){
00060 m_ff_coefficients.push_back(*it);
00061 }
00062
00063
00064 m_previous_values.assign(dim, 0.0);
00065 m_initialized = true;
00066 return true;
00067 }
00068
00069 bool IIRFilter::setParameterAsBiquad(const double f_cutoff, const double Q, const double hz){
00070 std::vector<double> fb_coeffs(3), ff_coeffs(3);
00071 const double omega = 2 * 3.14159265 * f_cutoff / hz;
00072 const double alpha = std::sin(omega) / (2 * Q);
00073 const double denom = 1 + alpha;
00074 fb_coeffs[0] = 1;
00075 fb_coeffs[1] = -2 * std::cos(omega) / denom;
00076 fb_coeffs[2] = (1 - alpha) / denom;
00077 ff_coeffs[0] = (1 - std::cos(omega)) / 2 / denom;
00078 ff_coeffs[1] = (1 - std::cos(omega)) / denom;
00079 ff_coeffs[2] = (1 - std::cos(omega)) / 2 / denom;
00080 return this->setParameter(2, fb_coeffs, ff_coeffs);
00081 };
00082
00083
00084 void IIRFilter::getParameter(int &dim, std::vector<double>& A, std::vector<double>& B)
00085 {
00086 dim = m_dimension;
00087 B.resize(m_ff_coefficients.size());
00088 std::copy(m_ff_coefficients.begin(), m_ff_coefficients.end(), B.begin());
00089 A.resize(0);
00090 for(std::vector<double>::iterator it = m_fb_coefficients.begin();
00091 it != m_fb_coefficients.end(); it++) {
00092 if (it == m_fb_coefficients.begin()) {
00093 A.push_back(*it);
00094 } else {
00095 A.push_back(- *it);
00096 }
00097 }
00098 }
00099
00100 void IIRFilter::reset(double initial_input)
00101 {
00102
00103
00104
00105 double sum_ff_coeffs = std::accumulate(m_ff_coefficients.begin(), m_ff_coefficients.end(), 0.0);
00106 double reset_val = initial_input / sum_ff_coeffs;
00107 m_previous_values.assign(m_dimension, reset_val);
00108 }
00109
00110 double IIRFilter::passFilter(double input)
00111 {
00112
00113
00114 if (! m_initialized) {
00115 return 0.0;
00116 }
00117 double feedback, filtered;
00118
00119 feedback = m_fb_coefficients[0] * input;
00120 for (int i = 0; i < m_dimension; i++) {
00121 feedback += m_fb_coefficients[i + 1] * m_previous_values[i];
00122 }
00123 filtered = m_ff_coefficients[0] * feedback;
00124 for(int i = 0; i < m_dimension; i++) {
00125 filtered += m_ff_coefficients[i + 1] * m_previous_values[i];
00126 }
00127
00128 m_previous_values.push_front(feedback);
00129 m_previous_values.pop_back();
00130
00131 return filtered;
00132 }