telekyb_base: IIRFilter.cpp Source File

Go to the documentation of this file.
00001 // ----------------------------------------------------------------------------
00002 //
00003 // $Id$
00004 //
00005 // Copyright 2008, 2009, 2010, 2011  Antonio Franchi
00006 //
00007 // This file is part of TeleKyb.
00008 //
00009 // TeleKyb is free software: you can redistribute it and/or modify
00010 // it under the terms of the GNU General Public License as published by
00011 // the Free Software Foundation, either version 3 of the License, or
00012 // (at your option) any later version.
00013 //
00014 // TeleKyb is distributed in the hope that it will be useful,
00015 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 // GNU General Public License for more details.
00018 //
00019 // You should have received a copy of the GNU General Public License
00020 // along with TeleKyb. If not, see <http://www.gnu.org/licenses/>.
00021 //
00022 // Contact info: antonio.franchi@tuebingen.mpg.de 
00023 //
00024 // ----------------------------------------------------------------------------
00025 
00026 
00027 #include <telekyb_base/Filter/IIRFilter.hpp>
00028 
00029 #include <math.h>
00030 
00031 namespace TELEKYB_NAMESPACE
00032 {
00033 
00034 void IIRFilter::_kernelConstructor(std::vector< std::vector<double> > &inputCoeff,std::vector<double> &outputCoeff){
00035         _inCoeff  = inputCoeff;
00036         _inNum    = _inCoeff.size();
00037         _outCoeff = outputCoeff;
00038         _order    = _outCoeff.size();
00039         //Filter must be causal, future output will be dropped.
00040         for(int i=0;i<_inNum;i++){
00041                 _inCoeff[i].resize(_order + 1,0.0); // size is 1 more for the coeficient of the current input
00042         }
00043 
00044         _pastIns.resize(_inNum);
00045         for(int i=0;i<_inNum;i++){
00046                 _pastIns[i].resize(_order,0.0);
00047         }
00048         _pastOuts.resize(_order,0.0);
00049 }
00050 
00051 IIRFilter::IIRFilter(std::vector<double> &inputCoeff,std::vector<double> &outputCoeff){
00052         std::vector< std::vector<double> > iCs(1,inputCoeff);
00053         _kernelConstructor(iCs,outputCoeff);
00054 }
00055 
00056 IIRFilter::IIRFilter(std::vector< std::vector<double> > &inputCoeff,std::vector<double> &outputCoef){
00057         _kernelConstructor(inputCoeff,outputCoef);
00058 }
00059 
00060 IIRFilter::IIRFilter(const IIRFilter& t) {
00061         _inCoeff  = t._inCoeff;
00062         _outCoeff = t._outCoeff;
00063         _pastIns  = t._pastIns;
00064         _pastOuts = t._pastOuts;
00065         _order    = t._order;
00066         _inNum          = t._inNum;
00067 }
00068 
00069 IIRFilter& IIRFilter::operator=(const IIRFilter& t){
00070         if (this != &t){
00071                 _inCoeff  = t._inCoeff;
00072                 _outCoeff = t._outCoeff;
00073                 _pastIns  = t._pastIns;
00074                 _pastOuts = t._pastOuts;
00075                 _order    = t._order;
00076                 _inNum          = t._inNum;
00077         }
00078         return *this;
00079 }
00080 
00081 IIRFilter::IIRFilter(IIRLowPass dummyType, double wn, double csi, double ts){
00082 
00083         std::vector<double> inputCoeff(3, 0.0), outputCoeff(2, 0.0);
00084 
00085         inputCoeff[0]=wn*wn*ts*ts/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00086         inputCoeff[1]=2.0*wn*wn*ts*ts/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00087         inputCoeff[2]=wn*wn*ts*ts/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00088 
00089         outputCoeff[0]=(-4.0*csi*wn*ts+4+wn*wn*ts*ts)/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00090         outputCoeff[1]=(-8.0+2.0*wn*wn*ts*ts)/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00091 
00092         std::vector< std::vector<double> > iCs(1,inputCoeff);
00093 
00094         _kernelConstructor(iCs, outputCoeff);
00095 
00096 }
00097 
00098 IIRFilter::IIRFilter(IIRFiltDeriv dummyType, double wn, double csi, double ts){
00099 
00100         std::vector<double> inputCoeff(3, 0.0), outputCoeff(2, 0.0);
00101 
00102         inputCoeff[0]=-2.0*ts*wn*wn/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00103         inputCoeff[1]=0.0;
00104         inputCoeff[2]=2.0*ts*wn*wn/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00105 
00106         outputCoeff[0]=(-4.0*csi*wn*ts+4+wn*wn*ts*ts)/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00107         outputCoeff[1]=(-8.0+2.0*wn*wn*ts*ts)/(4.0+wn*wn*ts*ts+4.0*csi*wn*ts);
00108 
00109         std::vector< std::vector<double> > iCs(1,inputCoeff);
00110 
00111         _kernelConstructor(iCs, outputCoeff);
00112 
00113 }
00114 
00115 
00116 
00117 
00118 
00119 IIRFilter::IIRFilter(IIRLowPassButtW dummyType, double wn, double ts){
00120 
00121         std::vector<double> inputCoeff(5, 0.0), outputCoeff(4, 0.0);
00122 
00123         double a=1.0000;
00124         double b=2.61312592975275;
00125         double c=3.41421356237309;
00126         double d=2.61312592975275;
00127         double e=1.0000;
00128 
00129         double scale=e*pow(ts,4)*pow(wn,4) + 2*d*pow(ts,3)*pow(wn,3) + 4*c*pow(ts,2)*pow(wn,2) + 8*b*ts*wn + 16*a;
00130 
00131         inputCoeff[0]=(pow(ts,4)*pow(wn,4))/scale;
00132         inputCoeff[1]=(4*pow(ts,4)*pow(wn,4))/scale;
00133         inputCoeff[2]=(6*pow(ts,4)*pow(wn,4))/scale;
00134         inputCoeff[3]=(4*pow(ts,4)*pow(wn,4))/scale;
00135         inputCoeff[4]=(pow(ts,4)*pow(wn,4))/scale;
00136 
00137 
00138         outputCoeff[0]=(e*pow(ts,4)*pow(wn,4) - 2*d*pow(ts,3)*pow(wn,3) + 4*c*pow(ts,2)*pow(wn,2) - 8*b*ts*wn + 16*a)/scale;
00139         outputCoeff[1]=(4*e*pow(ts,4)*pow(wn,4) - 4*d*pow(ts,3)*pow(wn,3) + 16*b*ts*wn - 64*a)/scale;
00140         outputCoeff[2]=(6*e*pow(ts,4)*pow(wn,4) - 8*c*pow(ts,2)*pow(wn,2) + 96*a)/scale;
00141         outputCoeff[3]=(4*e*pow(ts,4)*pow(wn,4) + 4*d*pow(ts,3)*pow(wn,3) - 16*b*ts*wn - 64*a)/scale;
00142 
00143 
00144 /* MATLAB CODE
00145 a=1.0000;
00146 b=2.61312592975275;
00147 c=3.41421356237309;
00148 d=2.61312592975275;
00149 e=1.0000;
00150 
00151 
00152 numcoeff(1)=T^4*wn^4;
00153 numcoeff(2)=4*T^4*wn^4;
00154 numcoeff(3)=6*T^4*wn^4;
00155 numcoeff(4)=4*T^4*wn^4;
00156 numcoeff(5)=T^4*wn^4;
00157 
00158 dencoeff(1)=e*T^4*wn^4 + 2*d*T^3*wn^3 + 4*c*T^2*wn^2 + 8*b*T*wn + 16*a;
00159 dencoeff(2)=4*e*T^4*wn^4 + 4*d*T^3*wn^3 - 16*b*T*wn - 64*a;
00160 dencoeff(3)=6*e*T^4*wn^4 - 8*c*T^2*wn^2 + 96*a;
00161 dencoeff(4)=4*e*T^4*wn^4 - 4*d*T^3*wn^3 + 16*b*T*wn - 64*a;
00162 dencoeff(5)=e*T^4*wn^4 - 2*d*T^3*wn^3 + 4*c*T^2*wn^2 - 8*b*T*wn + 16*a;
00163 
00164 
00165 
00166 */
00167 
00168 
00169 
00170 
00171         std::vector< std::vector<double> > iCs(1,inputCoeff);
00172 
00173         _kernelConstructor(iCs, outputCoeff);
00174 
00175 }
00176 
00177 
00178 
00179 IIRFilter::IIRFilter(IIRFiltDerivButtW dummyType, double wn, double ts){
00180 
00181         std::vector<double> inputCoeff(6, 0.0), outputCoeff(5, 0.0);
00182 
00183         double a=1.0000;
00184         double b=2.61312592975275;
00185         double c=3.41421356237309;
00186         double d=2.61312592975275;
00187         double e=1.0000;
00188 
00189         double scale=ts*(e*pow(ts,4)*pow(wn,4) + 2*d*pow(ts,3)*pow(wn,3) + 4*c*pow(ts,2)*pow(wn,2) + 8*b*ts*wn + 16*a);
00190 
00191         inputCoeff[0]=(-2*pow(ts,4)*pow(wn,4))/scale;
00192         inputCoeff[1]=(-6*pow(ts,4)*pow(wn,4))/scale;
00193         inputCoeff[2]=(-4*pow(ts,4)*pow(wn,4))/scale;
00194         inputCoeff[3]=(4*pow(ts,4)*pow(wn,4))/scale;
00195         inputCoeff[4]=(6*pow(ts,4)*pow(wn,4))/scale;
00196         inputCoeff[5]=(2*pow(ts,4)*pow(wn,4))/scale;
00197 
00198 
00199         outputCoeff[0]=(ts*(e*pow(ts,4)*pow(wn,4) - 2*d*pow(ts,3)*pow(wn,3) + 4*c*pow(ts,2)*pow(wn,2) - 8*b*ts*wn + 16*a))/scale;
00200         outputCoeff[1]=(ts*(5*e*pow(ts,4)*pow(wn,4) - 6*d*pow(ts,3)*pow(wn,3) + 4*c*pow(ts,2)*pow(wn,2) + 8*b*ts*wn - 48*a))/scale;
00201         outputCoeff[2]=(ts*(10*e*pow(ts,4)*pow(wn,4) - 4*d*pow(ts,3)*pow(wn,3) - 8*c*pow(ts,2)*pow(wn,2) + 16*b*ts*wn + 32*a))/scale;
00202         outputCoeff[3]=(ts*(10*e*pow(ts,4)*pow(wn,4) + 4*d*pow(ts,3)*pow(wn,3) - 8*c*pow(ts,2)*pow(wn,2) - 16*b*ts*wn + 32*a))/scale;
00203         outputCoeff[4]=(ts*(5*e*pow(ts,4)*pow(wn,4) + 6*d*pow(ts,3)*pow(wn,3) + 4*c*pow(ts,2)*pow(wn,2) - 8*b*ts*wn - 48*a))/scale;
00204 
00205 
00206 /* MATLAB CODE
00207 a=1.0000;
00208 b=2.61312592975275;
00209 c=3.41421356237309;
00210 d=2.61312592975275;
00211 e=1.0000;
00212 
00213 
00214 numcoeff(1)=2*ts^4*wn^4;
00215 numcoeff(2)=6*ts^4*wn^4;
00216 numcoeff(3)=4*ts^4*wn^4;
00217 numcoeff(4)=-4*ts^4*wn^4;
00218 numcoeff(5)=-6*ts^4*wn^4;
00219 numcoeff(6)=-2*ts^4*wn^4;
00220 
00221 
00222 
00223 
00224 dencoeff(1)=ts*(e*ts^4*wn^4 + 2*d*ts^3*wn^3 + 4*c*ts^2*wn^2 + 8*b*ts*wn + 16*a);
00225 dencoeff(2)=ts*(5*e*ts^4*wn^4 + 6*d*ts^3*wn^3 + 4*c*ts^2*wn^2 - 8*b*ts*wn - 48*a);
00226 dencoeff(3)=ts*(10*e*ts^4*wn^4 + 4*d*ts^3*wn^3 - 8*c*ts^2*wn^2 - 16*b*ts*wn + 32*a);
00227 dencoeff(4)=ts*(10*e*ts^4*wn^4 - 4*d*ts^3*wn^3 - 8*c*ts^2*wn^2 + 16*b*ts*wn + 32*a);
00228 dencoeff(5)=ts*(5*e*ts^4*wn^4 - 6*d*ts^3*wn^3 + 4*c*ts^2*wn^2 + 8*b*ts*wn - 48*a);
00229 dencoeff(6)=ts*(e*ts^4*wn^4 - 2*d*ts^3*wn^3 + 4*c*ts^2*wn^2 - 8*b*ts*wn + 16*a);
00230 
00231 
00232 */
00233 
00234 
00235 
00236 
00237         std::vector< std::vector<double> > iCs(1,inputCoeff);
00238 
00239         _kernelConstructor(iCs, outputCoeff);
00240 
00241 }
00242 
00243 
00244 
00245 
00246 
00247 
00248 
00249 IIRFilter::~IIRFilter(){
00250 }
00251 
00252 
00253 void IIRFilter::step(double in,double& out){
00254         std::vector<double> inputs(_inNum,0.0);
00255         inputs[1] = in;
00256         step(inputs,out);
00257 }
00258 
00259 void IIRFilter::step(const std::vector<double> &in,double &out){
00260         out = 0.0;
00261         for(iCo=0, outIt=_pastOuts.begin() ;iCo<_order ; iCo++, outIt++){
00262                 out += -(*outIt)*_outCoeff[iCo];
00263         }
00264         for(iIn=0;iIn<_inNum;iIn++){
00265                 for(iCo=0,inIt=_pastIns[iIn].begin();iCo<_order;iCo++,inIt++){
00266                         out += (*inIt)*_inCoeff[iIn][iCo] ;
00267                 }
00268                 out += in[iIn] * _inCoeff[iIn][_order];
00269         }
00270         for(iIn=0;iIn<_inNum;iIn++){
00271                 _pastIns[iIn].pop_front();
00272                 _pastIns[iIn].push_back(in[iIn]);
00273         }
00274         _pastOuts.pop_front();
00275         _pastOuts.push_back(out);
00276 }
00277 
00278 } // namespace