BiQuad.cpp

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