testIIRFilter.cpp
Go to the documentation of this file.
00001 /* -*- coding:utf-8-unix; mode:c++; -*- */
00002 
00003 #include "IIRFilter.h"
00004 /* samples */
00005 #include <stdio.h>
00006 #include <stdlib.h>
00007 #include <iostream>
00008 #include <vector>
00009 #include <boost/shared_ptr.hpp>
00010 #include <hrpUtil/Eigen3d.h>
00011 
00012 template <class T, class FT>
00013 class testIIRFilter
00014 {
00015 protected:
00016     double dt; /* [s] */
00017     double input_freq; /* [Hz] */
00018     boost::shared_ptr<FT > filter;
00019     bool use_gnuplot;
00020     void fprintf_value (FILE* fp, const double _time, const T& _input, const T& _output);
00021     void fprintf_plot (FILE* gp_pos);
00022     void gen_pattern_and_plot (const std::vector<double>& time_vec, const std::vector<T>& input_vec)
00023     {
00024         std::string fname("/tmp/plot-iirfilter.dat");
00025         FILE* fp = fopen(fname.c_str(), "w");
00026         for (size_t i = 0; i < time_vec.size();i++) {
00027             fprintf_value(fp, time_vec[i], input_vec[i], filter->passFilter(input_vec[i]));
00028         }
00029         fclose(fp);
00030         if (use_gnuplot) {
00031             //   plot pos
00032             FILE* gp_pos = popen("gnuplot", "w");
00033             fprintf(gp_pos, "set xlabel 'Time [s]'\n");
00034             fprintf(gp_pos, "set ylabel 'var []'\n");
00035             fprintf_plot (gp_pos);
00036             fflush(gp_pos);
00037             double tmp;
00038             std::cin >> tmp;
00039             pclose(gp_pos);
00040         }
00041     };
00042     T init_value ();
00043     T test0_input_value (const size_t i);
00044     double calc_sin_value (const size_t i) { return std::sin(2*M_PI*i*dt*input_freq); };
00045 public:
00046     std::vector<std::string> arg_strs;
00047     testIIRFilter (const double _dt = 0.004) : dt(_dt), input_freq(1.0),
00048                                                use_gnuplot(true) { initialize(); };
00049     void initialize() {
00050         filter = boost::shared_ptr<FT >(new FT (4.0, dt, init_value()));
00051     }
00052     void test0 ()
00053     {
00054         std::cerr << "test0 : test" << std::endl;
00055         parse_params();
00056         double tm = 0.0, total_tm = 4.0;
00057         std::vector<double> time_vec;
00058         std::vector<T> input_vec;
00059         for (size_t i = 0; i < static_cast<size_t>(total_tm/dt);i++) {
00060             time_vec.push_back(tm);
00061             input_vec.push_back(test0_input_value(i));
00062             tm += dt;
00063         }
00064         gen_pattern_and_plot (time_vec,input_vec);
00065     };
00066     void parse_params ()
00067     {
00068       for (int i = 0; i < arg_strs.size(); ++ i) {
00069           if ( arg_strs[i]== "--use-gnuplot" ) {
00070               if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true");
00071           } else if ( arg_strs[i]== "--cutoff-freq" ) {
00072               if (++i < arg_strs.size()) filter->setCutOffFreq(atof(arg_strs[i].c_str()));
00073           } else if ( arg_strs[i]== "--input-freq" ) {
00074               if (++i < arg_strs.size()) input_freq = atof(arg_strs[i].c_str());
00075           }
00076       }
00077       std::cerr << "[testIIRFilter] params" << std::endl;
00078       std::cerr << "[testIIRFilter]   dt = " << dt << "[s], cutoff-freq = " << filter->getCutOffFreq() << "[Hz], input-freq = " << input_freq << "[Hz]" << std::endl;
00079     };
00080 };
00081 
00082 // Specialization for double
00083 template<> void testIIRFilter<double, FirstOrderLowPassFilter<double> >::fprintf_value (FILE* fp, const double _time, const double& _input, const double& _output)
00084 {
00085     fprintf(fp, "%f %f %f\n", _time, _input, _output);
00086 };
00087 template<> double testIIRFilter<double, FirstOrderLowPassFilter<double> >::init_value () { return 0;};
00088 template<> double testIIRFilter<double, FirstOrderLowPassFilter<double> >::test0_input_value (const size_t i) { return calc_sin_value(i);};
00089 template<> void testIIRFilter<double, FirstOrderLowPassFilter<double> >::fprintf_plot (FILE* gp_pos)
00090 {
00091     fprintf(gp_pos, "plot '/tmp/plot-iirfilter.dat' using 1:2 with lines title 'input' lw 4, '/tmp/plot-iirfilter.dat' using 1:3 with lines title 'filtered' lw 3\n");
00092 };
00093 
00094 // Specialization for hrp::Vector3
00095 template<> void testIIRFilter<hrp::Vector3, FirstOrderLowPassFilter<hrp::Vector3> >::fprintf_value (FILE* fp, const double _time, const hrp::Vector3& _input, const hrp::Vector3& _output)
00096 {
00097     fprintf(fp, "%f %f %f %f %f %f %f\n", _time, _input[0], _output[0], _input[1], _output[1], _input[2], _output[2]);
00098 };
00099 template<> hrp::Vector3 testIIRFilter<hrp::Vector3, FirstOrderLowPassFilter<hrp::Vector3> >::init_value () { return hrp::Vector3::Zero();};
00100 template<> hrp::Vector3 testIIRFilter<hrp::Vector3, FirstOrderLowPassFilter<hrp::Vector3> >::test0_input_value (const size_t i)
00101 {
00102     double tmp = calc_sin_value(i);
00103     return hrp::Vector3(tmp, 2*tmp, -0.5*tmp);
00104 };
00105 template<> void testIIRFilter<hrp::Vector3, FirstOrderLowPassFilter<hrp::Vector3> >::fprintf_plot (FILE* gp_pos)
00106 {
00107     fprintf(gp_pos, "plot '/tmp/plot-iirfilter.dat' using 1:2 with lines title 'input (0)' lw 4, '/tmp/plot-iirfilter.dat' using 1:3 with lines title 'filtered (0)' lw 3,");
00108     fprintf(gp_pos, "'/tmp/plot-iirfilter.dat' using 1:4 with lines title 'input (1)' lw 4, '/tmp/plot-iirfilter.dat' using 1:5 with lines title 'filtered (1)' lw 3,");
00109     fprintf(gp_pos, "'/tmp/plot-iirfilter.dat' using 1:6 with lines title 'input (2)' lw 4, '/tmp/plot-iirfilter.dat' using 1:7 with lines title 'filtered (2)' lw 3\n");
00110 };
00112 template<> void testIIRFilter<double, IIRFilter >::initialize () {
00113 #if 0 // use obsolated method
00114     int filter_dim = 0;
00115     std::vector<double> fb_coeffs, ff_coeffs;
00116     filter_dim = 2;
00117     fb_coeffs.resize(filter_dim+1);
00118     fb_coeffs[0] = 1.00000;
00119     fb_coeffs[1] = 1.88903;
00120     fb_coeffs[2] =-0.89487;
00121     ff_coeffs.resize(filter_dim+1);
00122     ff_coeffs[0] = 0.0014603;
00123     ff_coeffs[1] = 0.0029206;
00124     ff_coeffs[2] = 0.0014603;
00125     filter = boost::shared_ptr<IIRFilter >(new IIRFilter (filter_dim, fb_coeffs, ff_coeffs));
00126 #else
00127     filter = boost::shared_ptr<IIRFilter >(new IIRFilter ());
00128     int dim = 2;
00129     std::vector<double> A(dim+1);
00130     std::vector<double> B(dim+1);
00131     // octave
00132     // [B, A] = butter(2, 0.004 * 2 * 8) ;;; 2 * dt * cutoff_freq
00133     //b =
00134     //0.00882608666843131   0.01765217333686262   0.00882608666843131
00135     //a =
00136     //1.000000000000000  -1.717211834908084   0.752516181581809
00137     A[0] = 1.000000000000000;
00138     A[1] = -1.717211834908084;
00139     A[2] = 0.752516181581809;
00140     B[0] = 0.00882608666843131;
00141     B[1] = 0.01765217333686262;
00142     B[2] = 0.00882608666843131;
00143     filter->setParameter(dim, A, B);
00144 #endif
00145 };
00146 template<> void testIIRFilter<double, IIRFilter >::parse_params () {
00147     for (int i = 0; i < arg_strs.size(); ++ i) {
00148         if ( arg_strs[i]== "--use-gnuplot" ) {
00149             if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true");
00150 #if 0
00151         } else if ( arg_strs[i]== "--cutoff-freq" ) {
00152             if (++i < arg_strs.size()) filter->setCutOffFreq(atof(arg_strs[i].c_str()));
00153 #endif
00154         } else if ( arg_strs[i]== "--input-freq" ) {
00155             if (++i < arg_strs.size()) input_freq = atof(arg_strs[i].c_str());
00156         }
00157     }
00158     std::cerr << "[testIIRFilter] params" << std::endl;
00159     std::cerr << "[testIIRFilter]   dt = " << dt << "[s], cutoff-freq = " << 8 << "[Hz], input-freq = " << input_freq << "[Hz]" << std::endl;
00160 };
00161 template<> void testIIRFilter<double, IIRFilter >::fprintf_value (FILE* fp, const double _time, const double& _input, const double& _output)
00162 {
00163     fprintf(fp, "%f %f %f\n", _time, _input, _output);
00164 };
00165 template<> double testIIRFilter<double, IIRFilter >::init_value () { return 0;};
00166 template<> double testIIRFilter<double, IIRFilter >::test0_input_value (const size_t i) { return calc_sin_value(i);};
00167 template<> void testIIRFilter<double, IIRFilter >::fprintf_plot (FILE* gp_pos)
00168 {
00169     fprintf(gp_pos, "plot '/tmp/plot-iirfilter.dat' using 1:2 with lines title 'input' lw 4, '/tmp/plot-iirfilter.dat' using 1:3 with lines title 'filtered' lw 3\n");
00170 };
00171 
00172 void print_usage ()
00173 {
00174     std::cerr << "Usage : testIIRFilter [mode] [test-name] [option]" << std::endl;
00175     std::cerr << " [mode] should be: --double, --vector3" << std::endl;
00176     std::cerr << " [test-name] should be:" << std::endl;
00177     std::cerr << "  --test0 : test" << std::endl;
00178     std::cerr << " [option] should be:" << std::endl;
00179 };
00180 
00181 int main(int argc, char* argv[])
00182 {
00183     int ret = 0;
00184     if (argc >= 3) {
00185         if (std::string(argv[1]) == "--double") {
00186             testIIRFilter<double, FirstOrderLowPassFilter<double> > tiir;
00187             for (int i = 2; i < argc; ++ i) {
00188                 tiir.arg_strs.push_back(std::string(argv[i]));
00189             }
00190             if (std::string(argv[2]) == "--test0") {
00191                 tiir.test0();
00192             } else {
00193                 print_usage();
00194                 ret = 1;
00195             }
00196         } else if (std::string(argv[1]) == "--vector3") {
00197             testIIRFilter<hrp::Vector3, FirstOrderLowPassFilter<hrp::Vector3> > tiir;
00198             for (int i = 2; i < argc; ++ i) {
00199                 tiir.arg_strs.push_back(std::string(argv[i]));
00200             }
00201             if (std::string(argv[2]) == "--test0") {
00202                 tiir.test0();
00203             } else {
00204                 print_usage();
00205                 ret = 1;
00206             }
00207         } else if (std::string(argv[1]) == "--iir") {
00208             testIIRFilter<double, IIRFilter > tiir;
00209             for (int i = 2; i < argc; ++ i) {
00210                 tiir.arg_strs.push_back(std::string(argv[i]));
00211             }
00212             if (std::string(argv[2]) == "--test0") {
00213                 tiir.test0();
00214             } else {
00215                 print_usage();
00216                 ret = 1;
00217             }
00218         } else {
00219             print_usage();
00220             ret = 1;
00221         }
00222     } else {
00223         print_usage();
00224         ret = 1;
00225     }
00226     return ret;
00227 }
00228 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56