BiQuad.cpp

00001 #include "BiQuad.h"
00002 
00003 BiQuad::BiQuad() {
00004     resetStateOnGainChange = true;
00005     set( 1.0, 0.0, 0.0, 0.0, 0.0 );
00006 }
00007 
00008 BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) {
00009     resetStateOnGainChange = true;
00010     set( b0, b1, b2, a1, a2 );
00011 }
00012 
00013 BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) {
00014     resetStateOnGainChange = true;
00015     set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 );
00016 }
00017 
00018 void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts  ) {
00019 
00020     double b0, b1, b2, bd, a1, a2;
00021 
00022     a1 = -4.0/(N*Ts+2.0);
00023     a2 = -(N*Ts-2.0)/(N*Ts+2.0);
00024 
00025     bd = ( N*Ts+2.0 );
00026 
00027     b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
00028     b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd;
00029     b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
00030 
00031     set( b0, b1, b2, a1, a2 );
00032 
00033 };
00034 
00035 void BiQuad::set(double b0, double b1, double b2, double a1, double a2) {
00036 
00037     B[0] = b0; B[1] = b1; B[2] = b2;
00038     A[0] = a1; A[1] = a2;
00039 
00040     if( resetStateOnGainChange )
00041         wz[0] = 0; wz[1] = 0;
00042 
00043 }
00044 
00045 double BiQuad::step(double x) {
00046 
00047     double y,w;
00048 
00049     /* Direct form II */
00050     w =      x - A[0]*wz[0] - A[1]*wz[1];
00051     y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
00052 
00053     /* Shift */
00054     wz[1] = wz[0];
00055     wz[0] = w;
00056 
00057     return y;
00058 
00059 }
00060 
00061 std::vector< std::complex<double> > BiQuad::poles() {
00062 
00063     std::vector< std::complex<double> > poles;
00064 
00065     std::complex<double> b2(A[0]*A[0],0);
00066     std::complex<double> ds = std::sqrt( b2-4*A[1] );
00067 
00068     poles.push_back( 0.5*(-A[0]+ds) );
00069     poles.push_back( 0.5*(-A[0]-ds) );
00070 
00071     return poles;
00072 
00073 }
00074 
00075 std::vector< std::complex<double> > BiQuad::zeros() {
00076 
00077     std::vector< std::complex<double> > zeros;
00078 
00079     std::complex<double> b2(B[1]*B[1],0);
00080     std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] );
00081 
00082     zeros.push_back( 0.5*(-B[1]+ds)/B[0] );
00083     zeros.push_back( 0.5*(-B[1]-ds)/B[0] );
00084 
00085     return zeros;
00086 
00087 }
00088 
00089 bool BiQuad::stable() {
00090     bool stable = true;
00091     std::vector< std::complex<double> > ps = poles();
00092     for( size_t i = 0; i < ps.size(); i++ )
00093         stable = stable & ( std::abs( ps[i] ) < 1 );
00094     return stable;
00095 }
00096 
00097 void BiQuad::setResetStateOnGainChange( bool v ){
00098     resetStateOnGainChange = v;
00099 }
00100 
00101 BiQuadChain &BiQuadChain::add(BiQuad *bq) {
00102     biquads.push_back( bq );
00103     return *this;
00104 }
00105 
00106 BiQuadChain operator*( BiQuad &bq1, BiQuad &bq2 ) {
00107     BiQuadChain bqc;
00108     bqc.add( &bq1 ).add( &bq2 );
00109     return bqc;
00110 }
00111 
00112 double BiQuadChain::step(double x) {
00113 
00114     int i;
00115     size_t bqs;
00116 
00117     bqs = biquads.size();
00118 
00119     for( i = 0; i < bqs; i++ )
00120         x = biquads[i]->step( x );
00121 
00122     return x;
00123 }
00124 
00125 std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) {
00126 
00127     std::vector< std::complex<double> > chain, bq;
00128     int i;
00129     size_t bqs;
00130 
00131     bqs = biquads.size();
00132 
00133     for( i = 0; i < bqs; i++ ){
00134         bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles();
00135         chain.insert( chain.end(), bq.begin(), bq.end() );
00136     }
00137 
00138     return chain;
00139 
00140 }
00141 
00142 std::vector< std::complex<double> > BiQuadChain::poles() {
00143     return poles_zeros( false );
00144 }
00145 
00146 std::vector< std::complex<double> > BiQuadChain::zeros() {
00147     return poles_zeros( true );
00148 }
00149 
00150 bool BiQuadChain::stable() {
00151     bool stable = true;
00152     for( size_t i = 0; i < biquads.size(); i++ )
00153         stable = stable & biquads[i]->stable();
00154     return stable;
00155 }
00156 
00157 BiQuadChain& BiQuadChain::operator*( BiQuad& bq ) {
00158     add( &bq );
00159     return *this;
00160 }