BiQuad.h

00001 #ifndef BIQUAD_BIQUAD_H
00002 #define BIQUAD_BIQUAD_H
00003 
00004 #include <vector>
00005 #include <complex>
00006 
00007 class BiQuadChain;
00008 
00009 /** BiQuad class implements a single filter
00010  *
00011  * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl>
00012  *
00013  * Filters that - in the z domain - are the ratio of two quadratic functions. The general form is:
00014  *
00015  *        b0 + b1 z^-1 + b2 z^-2
00016  * H(z) = ----------------------
00017  *        a0 + a1 z^-1 + a2 z^-2
00018  *
00019  * Which is often normalized by dividing all coefficients by a0.
00020  *
00021  * Example:
00022  * @code
00023  * #include "mbed.h"
00024  * #include <complex>
00025  *
00026  * // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
00027  * BiQuad bq1( 4.16599e-04, 8.33198e-04, 4.16599e-04, -1.47967e+00, 5.55822e-01 );
00028  * BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.70096e+00, 7.88500e-01 );
00029  *
00030  * BiQuadChain bqc;
00031  *
00032  * int main() {
00033  *
00034  *    // Add the biquads to the chain
00035  *    bqc.add( &bq1 ).add( &bq2 );
00036  *
00037  *    // Find the poles of the filter
00038  *    std::cout << "Filter poles" << std::endl;
00039  *    std::vector< std::complex<double> > poles = bqc.poles();
00040  *    for( size_t i = 0; i < poles.size(); i++ )
00041  *        std::cout << "\t"  << poles[i] << std::endl;
00042  *
00043  *    // Find the zeros of the filter
00044  *    std::cout << "Filter zeros" << std::endl;
00045  *    std::vector< std::complex<double> > zeros = bqc.zeros();
00046  *    for( size_t i = 0; i < poles.size(); i++ )
00047  *        std::cout << "\t" << zeros[i] << std::endl;
00048  *
00049  *    // Is the filter stable?
00050  *    std::cout << "This filter is " << (bqc.stable() ? "stable" : "instable") << std::endl;
00051  *
00052  *    // Output the step-response of 20 samples
00053  *  std::cout << "Step response 20 samples" << std::endl;
00054  *  for( int i = 0; i < 20; i++ )
00055  *      std::cout << "\t" << bqc.step( 1.0 ) << std::endl;
00056  * }
00057  * @endcode
00058  *
00059  * https://github.com/tomlankhorst/biquad
00060  *
00061  */
00062 class BiQuad {
00063 
00064 private:
00065 
00066     double B[3];
00067     double A[2];
00068     double wz[2];
00069 
00070     bool resetStateOnGainChange;
00071 
00072     /**
00073      * Sets the gain parameters
00074      */
00075     void set( double b0, double b1, double b2, double a1, double a2 );
00076 
00077 public:
00078 
00079     /**
00080      * Initialize a unity TF biquad
00081      * @return BiQuad instance
00082      */
00083     BiQuad( );
00084 
00085     /**
00086      * Initialize a normalized biquad filter
00087      * @param b0
00088      * @param b1
00089      * @param b2
00090      * @param a1
00091      * @param a2
00092      * @return BiQuad instance
00093      */
00094     BiQuad( double b0, double b1, double b2, double a1, double a2 );
00095 
00096     /**
00097      * Initialize a biquad filter with all six coefficients
00098      * @param b0
00099      * @param b1
00100      * @param b2
00101      * @param a0
00102      * @param a1
00103      * @param a2
00104      * @return BiQuad instance
00105      */
00106     BiQuad( double b0, double b1, double b2, double a0, double a1, double a2 );
00107 
00108     /**
00109      * Initialize a PIDF biquad.
00110      * Based on Tustin-approx (trapezoidal) of the continous time version.
00111      * Behaviour equivalent to the PID controller created with the following MATLAB expression:
00112      *
00113      * C = pid( Kp, Ki, Kd, 1/N, Ts, 'IFormula', 'Trapezoidal', 'DFormula', 'Trapezoidal' );
00114      *
00115      * @param Kp    Proportional gain
00116      * @param Ki    Integral gain
00117      * @param Kd    Derivative gain
00118      * @param N     Filter coefficient ( N = 1/Tf )
00119      * @param Ts    Timestep
00120      */
00121     void PIDF( double Kp, double Ki, double Kd, double N, double Ts  );
00122 
00123     /**
00124      * Execute one digital timestep and return the result...
00125      * @param x input of the filer
00126      * @return output of the filter
00127      */
00128     double step( double x );
00129 
00130     /**
00131      * Return poles of the BiQuad filter
00132      * @return vector of std::complex poles
00133      */
00134     std::vector< std::complex<double> > poles( );
00135 
00136     /**
00137      * Return zeros of the BiQuad filter
00138      * @return vector of std::complex zeros
00139      */
00140     std::vector< std::complex<double> > zeros( );
00141 
00142     /**
00143      * Is this biquad stable?
00144      * Checks if all poles lie within the unit-circle
00145      * @return boolean whether the filter is stable or not
00146      */
00147     bool stable ();
00148 
00149     /**
00150      * Determines if the state variables are reset to zero on gain change.
00151      * Can be used for changing gain parameters on the fly.
00152      * @param v Value of the reset boolean
00153      */
00154     void setResetStateOnGainChange( bool v );
00155 
00156 };
00157 
00158 /**
00159  * The BiQuadChain class implements a chain of BiQuad filters
00160  */
00161 class BiQuadChain {
00162 
00163 private:
00164     std::vector< BiQuad* > biquads;
00165     std::vector< std::complex<double> > poles_zeros( bool zeros = false );
00166 
00167 public:
00168 
00169     /**
00170      * Add a BiQuad pointer to the list: bqc.add(&bq);
00171      * @param bq Pointer to BiQuad instance
00172      * @return Pointer to BiQuadChain
00173      */
00174     BiQuadChain &add( BiQuad *bq );
00175 
00176     /**
00177      * Execute a digital time step cascaded through all bq's
00178      * @param x Input of the filter chain
00179      * @return Output of the chain
00180      */
00181     double step(double x);
00182 
00183     /**
00184      * Return poles of the BiQuad filter
00185      * @return vector of std::complex poles
00186      */
00187     std::vector< std::complex<double> > poles( );
00188 
00189     /**
00190      * Return zeros of the BiQuad filter
00191      * @return vector of std::complex zeros
00192      */
00193     std::vector< std::complex<double> > zeros( );
00194 
00195     /**
00196      * Is this biquad-chain stable?
00197      * Checks if all poles lie within the unit-circle
00198      * @return boolean whether the chain is stable or not
00199      */
00200     bool stable ();
00201 
00202     /**
00203      * Appends a BiQuad to the chain
00204      * Shorthand for .add(&bq)
00205      * @param bq BiQuad
00206      * @return Pointer to BiQuadChain
00207      */
00208     BiQuadChain &operator*( BiQuad& bq );
00209 
00210 };
00211 
00212 /**
00213  * Multiply two BiQuads
00214  * ... which in fact means appending them into a BiQuadChain
00215  * @return BiQuadChain of the two BiQuads
00216  */
00217 BiQuadChain operator*( BiQuad&, BiQuad& );
00218 
00219 #endif //BIQUAD_BIQUAD_H