Simple biquad filter
Dependents: EMG_Filter frdm_Motor_V2_2 frdm_Motor_V2_2 frdm_Motor_V2_3 ... more
BiQuad.h@7:26861979d305, 2016-10-02 (annotated)
- Committer:
- tomlankhorst
- Date:
- Sun Oct 02 21:27:00 2016 +0000
- Revision:
- 7:26861979d305
- Parent:
- 6:54dc8fd46e26
Update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tomlankhorst | 5:519e9002b10e | 1 | #ifndef BIQUAD_BIQUAD_H |
tomlankhorst | 5:519e9002b10e | 2 | #define BIQUAD_BIQUAD_H |
tomlankhorst | 5:519e9002b10e | 3 | |
tomlankhorst | 5:519e9002b10e | 4 | #include <vector> |
tomlankhorst | 5:519e9002b10e | 5 | #include <complex> |
tomlankhorst | 0:dca6a1d16911 | 6 | |
tomlankhorst | 5:519e9002b10e | 7 | /** BiQuad class implements a single filter |
tomlankhorst | 7:26861979d305 | 8 | * |
tomlankhorst | 5:519e9002b10e | 9 | * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> |
tomlankhorst | 5:519e9002b10e | 10 | * |
tomlankhorst | 5:519e9002b10e | 11 | * Filters that - in the z domain - are the ratio of two quadratic functions. The general form is: |
tomlankhorst | 5:519e9002b10e | 12 | * |
tomlankhorst | 5:519e9002b10e | 13 | * b0 + b1 z^-1 + b2 z^-2 |
tomlankhorst | 5:519e9002b10e | 14 | * H(z) = ---------------------- |
tomlankhorst | 5:519e9002b10e | 15 | * a0 + a1 z^-1 + a2 z^-2 |
tomlankhorst | 5:519e9002b10e | 16 | * |
tomlankhorst | 5:519e9002b10e | 17 | * Which is often normalized by dividing all coefficients by a0. |
tomlankhorst | 1:b9512f750fb6 | 18 | * |
tomlankhorst | 1:b9512f750fb6 | 19 | * Example: |
tomlankhorst | 1:b9512f750fb6 | 20 | * @code |
tomlankhorst | 1:b9512f750fb6 | 21 | * #include "mbed.h" |
tomlankhorst | 5:519e9002b10e | 22 | * #include <complex> |
tomlankhorst | 7:26861979d305 | 23 | * |
tomlankhorst | 7:26861979d305 | 24 | * // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist) |
tomlankhorst | 7:26861979d305 | 25 | * BiQuad bq1( 4.16599e-04, 8.33198e-04, 4.16599e-04, -1.47967e+00, 5.55822e-01 ); |
tomlankhorst | 7:26861979d305 | 26 | * BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.70096e+00, 7.88500e-01 ); |
tomlankhorst | 5:519e9002b10e | 27 | * |
tomlankhorst | 5:519e9002b10e | 28 | * BiQuadChain bqc; |
tomlankhorst | 5:519e9002b10e | 29 | * |
tomlankhorst | 5:519e9002b10e | 30 | * int main() { |
tomlankhorst | 5:519e9002b10e | 31 | * |
tomlankhorst | 5:519e9002b10e | 32 | * // Add the biquads to the chain |
tomlankhorst | 7:26861979d305 | 33 | * bqc.add( &bq1 ).add( &bq2 ); |
tomlankhorst | 1:b9512f750fb6 | 34 | * |
tomlankhorst | 5:519e9002b10e | 35 | * // Find the poles of the filter |
tomlankhorst | 5:519e9002b10e | 36 | * std::cout << "Filter poles" << std::endl; |
tomlankhorst | 5:519e9002b10e | 37 | * std::vector< std::complex<double> > poles = bqc.poles(); |
tomlankhorst | 5:519e9002b10e | 38 | * for( size_t i = 0; i < poles.size(); i++ ) |
tomlankhorst | 5:519e9002b10e | 39 | * std::cout << "\t" << poles[i] << std::endl; |
tomlankhorst | 5:519e9002b10e | 40 | * |
tomlankhorst | 5:519e9002b10e | 41 | * // Find the zeros of the filter |
tomlankhorst | 5:519e9002b10e | 42 | * std::cout << "Filter zeros" << std::endl; |
tomlankhorst | 5:519e9002b10e | 43 | * std::vector< std::complex<double> > zeros = bqc.zeros(); |
tomlankhorst | 5:519e9002b10e | 44 | * for( size_t i = 0; i < poles.size(); i++ ) |
tomlankhorst | 5:519e9002b10e | 45 | * std::cout << "\t" << zeros[i] << std::endl; |
tomlankhorst | 5:519e9002b10e | 46 | * |
tomlankhorst | 5:519e9002b10e | 47 | * // Is the filter stable? |
tomlankhorst | 5:519e9002b10e | 48 | * std::cout << "This filter is " << (bqc.stable() ? "stable" : "instable") << std::endl; |
tomlankhorst | 5:519e9002b10e | 49 | * |
tomlankhorst | 5:519e9002b10e | 50 | * // Output the step-response of 20 samples |
tomlankhorst | 5:519e9002b10e | 51 | * std::cout << "Step response 20 samples" << std::endl; |
tomlankhorst | 5:519e9002b10e | 52 | * for( int i = 0; i < 20; i++ ) |
tomlankhorst | 5:519e9002b10e | 53 | * std::cout << "\t" << bqc.step( 1.0 ) << std::endl; |
tomlankhorst | 1:b9512f750fb6 | 54 | * } |
tomlankhorst | 1:b9512f750fb6 | 55 | * @endcode |
tomlankhorst | 7:26861979d305 | 56 | * |
tomlankhorst | 7:26861979d305 | 57 | * https://github.com/tomlankhorst/biquad |
tomlankhorst | 7:26861979d305 | 58 | * |
tomlankhorst | 5:519e9002b10e | 59 | */ |
tomlankhorst | 5:519e9002b10e | 60 | class BiQuad { |
tomlankhorst | 5:519e9002b10e | 61 | |
tomlankhorst | 5:519e9002b10e | 62 | private: |
tomlankhorst | 5:519e9002b10e | 63 | |
tomlankhorst | 5:519e9002b10e | 64 | double B[3]; |
tomlankhorst | 5:519e9002b10e | 65 | double A[2]; |
tomlankhorst | 5:519e9002b10e | 66 | double wz[2]; |
tomlankhorst | 5:519e9002b10e | 67 | |
tomlankhorst | 5:519e9002b10e | 68 | bool resetStateOnGainChange; |
tomlankhorst | 5:519e9002b10e | 69 | |
tomlankhorst | 5:519e9002b10e | 70 | /** |
tomlankhorst | 5:519e9002b10e | 71 | * Sets the gain parameters |
tomlankhorst | 5:519e9002b10e | 72 | */ |
tomlankhorst | 5:519e9002b10e | 73 | void set( double b0, double b1, double b2, double a1, double a2 ); |
tomlankhorst | 5:519e9002b10e | 74 | |
tomlankhorst | 5:519e9002b10e | 75 | public: |
tomlankhorst | 5:519e9002b10e | 76 | |
tomlankhorst | 5:519e9002b10e | 77 | /** |
tomlankhorst | 5:519e9002b10e | 78 | * Initialize a unity TF biquad |
tomlankhorst | 5:519e9002b10e | 79 | * @return BiQuad instance |
tomlankhorst | 5:519e9002b10e | 80 | */ |
tomlankhorst | 5:519e9002b10e | 81 | BiQuad( ); |
tomlankhorst | 5:519e9002b10e | 82 | |
tomlankhorst | 5:519e9002b10e | 83 | /** |
tomlankhorst | 5:519e9002b10e | 84 | * Initialize a normalized biquad filter |
tomlankhorst | 5:519e9002b10e | 85 | * @param b0 |
tomlankhorst | 5:519e9002b10e | 86 | * @param b1 |
tomlankhorst | 5:519e9002b10e | 87 | * @param b2 |
tomlankhorst | 5:519e9002b10e | 88 | * @param a1 |
tomlankhorst | 5:519e9002b10e | 89 | * @param a2 |
tomlankhorst | 5:519e9002b10e | 90 | * @return BiQuad instance |
tomlankhorst | 5:519e9002b10e | 91 | */ |
tomlankhorst | 5:519e9002b10e | 92 | BiQuad( double b0, double b1, double b2, double a1, double a2 ); |
tomlankhorst | 1:b9512f750fb6 | 93 | |
tomlankhorst | 5:519e9002b10e | 94 | /** |
tomlankhorst | 5:519e9002b10e | 95 | * Initialize a biquad filter with all six coefficients |
tomlankhorst | 5:519e9002b10e | 96 | * @param b0 |
tomlankhorst | 5:519e9002b10e | 97 | * @param b1 |
tomlankhorst | 5:519e9002b10e | 98 | * @param b2 |
tomlankhorst | 5:519e9002b10e | 99 | * @param a0 |
tomlankhorst | 5:519e9002b10e | 100 | * @param a1 |
tomlankhorst | 5:519e9002b10e | 101 | * @param a2 |
tomlankhorst | 5:519e9002b10e | 102 | * @return BiQuad instance |
tomlankhorst | 5:519e9002b10e | 103 | */ |
tomlankhorst | 5:519e9002b10e | 104 | BiQuad( double b0, double b1, double b2, double a0, double a1, double a2 ); |
tomlankhorst | 5:519e9002b10e | 105 | |
tomlankhorst | 5:519e9002b10e | 106 | /** |
tomlankhorst | 5:519e9002b10e | 107 | * Initialize a PIDF biquad. |
tomlankhorst | 5:519e9002b10e | 108 | * Based on Tustin-approx (trapezoidal). of the continous time version |
tomlankhorst | 5:519e9002b10e | 109 | * @param Kp |
tomlankhorst | 5:519e9002b10e | 110 | * @param Ki |
tomlankhorst | 5:519e9002b10e | 111 | * @param Kd |
tomlankhorst | 5:519e9002b10e | 112 | * @param N |
tomlankhorst | 5:519e9002b10e | 113 | * @param Ts |
tomlankhorst | 5:519e9002b10e | 114 | */ |
tomlankhorst | 5:519e9002b10e | 115 | void PIDF( double Kp, double Ki, double Kd, double N, double Ts ); |
tomlankhorst | 5:519e9002b10e | 116 | |
tomlankhorst | 5:519e9002b10e | 117 | /** |
tomlankhorst | 5:519e9002b10e | 118 | * Execute one digital timestep and return the result... |
tomlankhorst | 5:519e9002b10e | 119 | * @param x input of the filer |
tomlankhorst | 5:519e9002b10e | 120 | * @return output of the filter |
tomlankhorst | 5:519e9002b10e | 121 | */ |
tomlankhorst | 5:519e9002b10e | 122 | double step( double x ); |
tomlankhorst | 5:519e9002b10e | 123 | |
tomlankhorst | 5:519e9002b10e | 124 | /** |
tomlankhorst | 5:519e9002b10e | 125 | * Return poles of the BiQuad filter |
tomlankhorst | 5:519e9002b10e | 126 | * @return vector of std::complex poles |
tomlankhorst | 5:519e9002b10e | 127 | */ |
tomlankhorst | 5:519e9002b10e | 128 | std::vector< std::complex<double> > poles( ); |
tomlankhorst | 5:519e9002b10e | 129 | |
tomlankhorst | 5:519e9002b10e | 130 | /** |
tomlankhorst | 5:519e9002b10e | 131 | * Return zeros of the BiQuad filter |
tomlankhorst | 5:519e9002b10e | 132 | * @return vector of std::complex zeros |
tomlankhorst | 5:519e9002b10e | 133 | */ |
tomlankhorst | 5:519e9002b10e | 134 | std::vector< std::complex<double> > zeros( ); |
tomlankhorst | 5:519e9002b10e | 135 | |
tomlankhorst | 5:519e9002b10e | 136 | /** |
tomlankhorst | 5:519e9002b10e | 137 | * Is this biquad stable? |
tomlankhorst | 5:519e9002b10e | 138 | * Checks if all poles lie within the unit-circle |
tomlankhorst | 5:519e9002b10e | 139 | * @return boolean whether the filter is stable or not |
tomlankhorst | 5:519e9002b10e | 140 | */ |
tomlankhorst | 5:519e9002b10e | 141 | bool stable (); |
tomlankhorst | 5:519e9002b10e | 142 | |
tomlankhorst | 5:519e9002b10e | 143 | /** |
tomlankhorst | 5:519e9002b10e | 144 | * Determines if the state variables are reset to zero on gain change. |
tomlankhorst | 5:519e9002b10e | 145 | * Can be used for changing gain parameters on the fly. |
tomlankhorst | 5:519e9002b10e | 146 | * @param v Value of the reset boolean |
tomlankhorst | 5:519e9002b10e | 147 | */ |
tomlankhorst | 5:519e9002b10e | 148 | void setResetStateOnGainChange( bool v ); |
tomlankhorst | 5:519e9002b10e | 149 | |
tomlankhorst | 0:dca6a1d16911 | 150 | }; |
tomlankhorst | 0:dca6a1d16911 | 151 | |
tomlankhorst | 5:519e9002b10e | 152 | /** |
tomlankhorst | 5:519e9002b10e | 153 | * The BiQuadChain class implements a chain of BiQuad filters |
tomlankhorst | 5:519e9002b10e | 154 | */ |
tomlankhorst | 5:519e9002b10e | 155 | class BiQuadChain { |
tomlankhorst | 5:519e9002b10e | 156 | |
tomlankhorst | 5:519e9002b10e | 157 | private: |
tomlankhorst | 5:519e9002b10e | 158 | std::vector< BiQuad* > biquads; |
tomlankhorst | 5:519e9002b10e | 159 | std::vector< std::complex<double> > poles_zeros( bool zeros = false ); |
tomlankhorst | 5:519e9002b10e | 160 | |
tomlankhorst | 5:519e9002b10e | 161 | public: |
tomlankhorst | 5:519e9002b10e | 162 | |
tomlankhorst | 5:519e9002b10e | 163 | /** |
tomlankhorst | 5:519e9002b10e | 164 | * Add a BiQuad pointer to the list: bqc.add(&bq); |
tomlankhorst | 5:519e9002b10e | 165 | * @param bq Pointer to BiQuad instance |
tomlankhorst | 5:519e9002b10e | 166 | * @return Pointer to BiQuadChain |
tomlankhorst | 5:519e9002b10e | 167 | */ |
tomlankhorst | 5:519e9002b10e | 168 | BiQuadChain &add( BiQuad *bq ); |
tomlankhorst | 5:519e9002b10e | 169 | |
tomlankhorst | 5:519e9002b10e | 170 | /** |
tomlankhorst | 5:519e9002b10e | 171 | * Execute a digital time step cascaded through all bq's |
tomlankhorst | 5:519e9002b10e | 172 | * @param x Input of the filter chain |
tomlankhorst | 5:519e9002b10e | 173 | * @return Output of the chain |
tomlankhorst | 5:519e9002b10e | 174 | */ |
tomlankhorst | 5:519e9002b10e | 175 | double step(double x); |
tomlankhorst | 5:519e9002b10e | 176 | |
tomlankhorst | 5:519e9002b10e | 177 | /** |
tomlankhorst | 5:519e9002b10e | 178 | * Return poles of the BiQuad filter |
tomlankhorst | 5:519e9002b10e | 179 | * @return vector of std::complex poles |
tomlankhorst | 5:519e9002b10e | 180 | */ |
tomlankhorst | 5:519e9002b10e | 181 | std::vector< std::complex<double> > poles( ); |
tomlankhorst | 5:519e9002b10e | 182 | |
tomlankhorst | 5:519e9002b10e | 183 | /** |
tomlankhorst | 5:519e9002b10e | 184 | * Return zeros of the BiQuad filter |
tomlankhorst | 5:519e9002b10e | 185 | * @return vector of std::complex zeros |
tomlankhorst | 5:519e9002b10e | 186 | */ |
tomlankhorst | 5:519e9002b10e | 187 | std::vector< std::complex<double> > zeros( ); |
tomlankhorst | 5:519e9002b10e | 188 | |
tomlankhorst | 5:519e9002b10e | 189 | /** |
tomlankhorst | 5:519e9002b10e | 190 | * Is this biquad-chain stable? |
tomlankhorst | 5:519e9002b10e | 191 | * Checks if all poles lie within the unit-circle |
tomlankhorst | 5:519e9002b10e | 192 | * @return boolean whether the chain is stable or not |
tomlankhorst | 5:519e9002b10e | 193 | */ |
tomlankhorst | 5:519e9002b10e | 194 | bool stable (); |
tomlankhorst | 5:519e9002b10e | 195 | }; |
tomlankhorst | 5:519e9002b10e | 196 | |
tomlankhorst | 5:519e9002b10e | 197 | |
tomlankhorst | 5:519e9002b10e | 198 | #endif //BIQUAD_BIQUAD_H |