Tom Tom / biquadFilter

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?

UserRevisionLine numberNew 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