BiQuad.h

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