Basis aansturing projectgroep 3
Dependencies: Biquad HIDScope MODSERIAL QEI mbed
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
Generated on Thu Jul 14 2022 07:24:36 by 1.7.2