Tom Tom / biquadFilter

Simple biquad filter

Dependents:   EMG_Filter frdm_Motor_V2_2 frdm_Motor_V2_2 frdm_Motor_V2_3 ... more

BiQuad.h@5:519e9002b10e, 2016-10-02 (annotated)

Committer:
tomlankhorst
Date:
Sun Oct 02 20:45:07 2016 +0000
Revision:
5:519e9002b10e
Parent:
biquadFilter.h@4:e3bf917ae0a3
Child:
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 5:519e9002b10e 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 5:519e9002b10e 23 * #include "BiQuad.h"
tomlankhorst 5:519e9002b10e 24 *
tomlankhorst 5:519e9002b10e 25 * BiQuadChain bqc;
tomlankhorst 5:519e9002b10e 26 * BiQuad pidf;
tomlankhorst 5:519e9002b10e 27 *
tomlankhorst 5:519e9002b10e 28 * int main() {
tomlankhorst 5:519e9002b10e 29 * // Create a biquad filter based on PIDF parameters
tomlankhorst 5:519e9002b10e 30 * pidf.PIDF(1,1,1,1,1);
tomlankhorst 5:519e9002b10e 31 *
tomlankhorst 5:519e9002b10e 32 * // Add the biquads to the chain
tomlankhorst 5:519e9002b10e 33 * bqc.add( &pidf );
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 5:519e9002b10e 56 */
tomlankhorst 5:519e9002b10e 57 class BiQuad {
tomlankhorst 5:519e9002b10e 58
tomlankhorst 5:519e9002b10e 59 private:
tomlankhorst 5:519e9002b10e 60
tomlankhorst 5:519e9002b10e 61 double B[3];
tomlankhorst 5:519e9002b10e 62 double A[2];
tomlankhorst 5:519e9002b10e 63 double wz[2];
tomlankhorst 5:519e9002b10e 64
tomlankhorst 5:519e9002b10e 65 bool resetStateOnGainChange;
tomlankhorst 5:519e9002b10e 66
tomlankhorst 5:519e9002b10e 67 /**
tomlankhorst 5:519e9002b10e 68 * Sets the gain parameters
tomlankhorst 5:519e9002b10e 69 */
tomlankhorst 5:519e9002b10e 70 void set( double b0, double b1, double b2, double a1, double a2 );
tomlankhorst 5:519e9002b10e 71
tomlankhorst 5:519e9002b10e 72 public:
tomlankhorst 5:519e9002b10e 73
tomlankhorst 5:519e9002b10e 74 /**
tomlankhorst 5:519e9002b10e 75 * Initialize a unity TF biquad
tomlankhorst 5:519e9002b10e 76 * @return BiQuad instance
tomlankhorst 5:519e9002b10e 77 */
tomlankhorst 5:519e9002b10e 78 BiQuad( );
tomlankhorst 5:519e9002b10e 79
tomlankhorst 5:519e9002b10e 80 /**
tomlankhorst 5:519e9002b10e 81 * Initialize a normalized biquad filter
tomlankhorst 5:519e9002b10e 82 * @param b0
tomlankhorst 5:519e9002b10e 83 * @param b1
tomlankhorst 5:519e9002b10e 84 * @param b2
tomlankhorst 5:519e9002b10e 85 * @param a1
tomlankhorst 5:519e9002b10e 86 * @param a2
tomlankhorst 5:519e9002b10e 87 * @return BiQuad instance
tomlankhorst 5:519e9002b10e 88 */
tomlankhorst 5:519e9002b10e 89 BiQuad( double b0, double b1, double b2, double a1, double a2 );
tomlankhorst 1:b9512f750fb6 90
tomlankhorst 5:519e9002b10e 91 /**
tomlankhorst 5:519e9002b10e 92 * Initialize a biquad filter with all six coefficients
tomlankhorst 5:519e9002b10e 93 * @param b0
tomlankhorst 5:519e9002b10e 94 * @param b1
tomlankhorst 5:519e9002b10e 95 * @param b2
tomlankhorst 5:519e9002b10e 96 * @param a0
tomlankhorst 5:519e9002b10e 97 * @param a1
tomlankhorst 5:519e9002b10e 98 * @param a2
tomlankhorst 5:519e9002b10e 99 * @return BiQuad instance
tomlankhorst 5:519e9002b10e 100 */
tomlankhorst 5:519e9002b10e 101 BiQuad( double b0, double b1, double b2, double a0, double a1, double a2 );
tomlankhorst 5:519e9002b10e 102
tomlankhorst 5:519e9002b10e 103 /**
tomlankhorst 5:519e9002b10e 104 * Initialize a PIDF biquad.
tomlankhorst 5:519e9002b10e 105 * Based on Tustin-approx (trapezoidal). of the continous time version
tomlankhorst 5:519e9002b10e 106 * @param Kp
tomlankhorst 5:519e9002b10e 107 * @param Ki
tomlankhorst 5:519e9002b10e 108 * @param Kd
tomlankhorst 5:519e9002b10e 109 * @param N
tomlankhorst 5:519e9002b10e 110 * @param Ts
tomlankhorst 5:519e9002b10e 111 */
tomlankhorst 5:519e9002b10e 112 void PIDF( double Kp, double Ki, double Kd, double N, double Ts );
tomlankhorst 5:519e9002b10e 113
tomlankhorst 5:519e9002b10e 114 /**
tomlankhorst 5:519e9002b10e 115 * Execute one digital timestep and return the result...
tomlankhorst 5:519e9002b10e 116 * @param x input of the filer
tomlankhorst 5:519e9002b10e 117 * @return output of the filter
tomlankhorst 5:519e9002b10e 118 */
tomlankhorst 5:519e9002b10e 119 double step( double x );
tomlankhorst 5:519e9002b10e 120
tomlankhorst 5:519e9002b10e 121 /**
tomlankhorst 5:519e9002b10e 122 * Return poles of the BiQuad filter
tomlankhorst 5:519e9002b10e 123 * @return vector of std::complex poles
tomlankhorst 5:519e9002b10e 124 */
tomlankhorst 5:519e9002b10e 125 std::vector< std::complex<double> > poles( );
tomlankhorst 5:519e9002b10e 126
tomlankhorst 5:519e9002b10e 127 /**
tomlankhorst 5:519e9002b10e 128 * Return zeros of the BiQuad filter
tomlankhorst 5:519e9002b10e 129 * @return vector of std::complex zeros
tomlankhorst 5:519e9002b10e 130 */
tomlankhorst 5:519e9002b10e 131 std::vector< std::complex<double> > zeros( );
tomlankhorst 5:519e9002b10e 132
tomlankhorst 5:519e9002b10e 133 /**
tomlankhorst 5:519e9002b10e 134 * Is this biquad stable?
tomlankhorst 5:519e9002b10e 135 * Checks if all poles lie within the unit-circle
tomlankhorst 5:519e9002b10e 136 * @return boolean whether the filter is stable or not
tomlankhorst 5:519e9002b10e 137 */
tomlankhorst 5:519e9002b10e 138 bool stable ();
tomlankhorst 5:519e9002b10e 139
tomlankhorst 5:519e9002b10e 140 /**
tomlankhorst 5:519e9002b10e 141 * Determines if the state variables are reset to zero on gain change.
tomlankhorst 5:519e9002b10e 142 * Can be used for changing gain parameters on the fly.
tomlankhorst 5:519e9002b10e 143 * @param v Value of the reset boolean
tomlankhorst 5:519e9002b10e 144 */
tomlankhorst 5:519e9002b10e 145 void setResetStateOnGainChange( bool v );
tomlankhorst 5:519e9002b10e 146
tomlankhorst 0:dca6a1d16911 147 };
tomlankhorst 0:dca6a1d16911 148
tomlankhorst 5:519e9002b10e 149 /**
tomlankhorst 5:519e9002b10e 150 * The BiQuadChain class implements a chain of BiQuad filters
tomlankhorst 5:519e9002b10e 151 */
tomlankhorst 5:519e9002b10e 152 class BiQuadChain {
tomlankhorst 5:519e9002b10e 153
tomlankhorst 5:519e9002b10e 154 private:
tomlankhorst 5:519e9002b10e 155 std::vector< BiQuad* > biquads;
tomlankhorst 5:519e9002b10e 156 std::vector< std::complex<double> > poles_zeros( bool zeros = false );
tomlankhorst 5:519e9002b10e 157
tomlankhorst 5:519e9002b10e 158 public:
tomlankhorst 5:519e9002b10e 159
tomlankhorst 5:519e9002b10e 160 /**
tomlankhorst 5:519e9002b10e 161 * Add a BiQuad pointer to the list: bqc.add(&bq);
tomlankhorst 5:519e9002b10e 162 * @param bq Pointer to BiQuad instance
tomlankhorst 5:519e9002b10e 163 * @return Pointer to BiQuadChain
tomlankhorst 5:519e9002b10e 164 */
tomlankhorst 5:519e9002b10e 165 BiQuadChain &add( BiQuad *bq );
tomlankhorst 5:519e9002b10e 166
tomlankhorst 5:519e9002b10e 167 /**
tomlankhorst 5:519e9002b10e 168 * Execute a digital time step cascaded through all bq's
tomlankhorst 5:519e9002b10e 169 * @param x Input of the filter chain
tomlankhorst 5:519e9002b10e 170 * @return Output of the chain
tomlankhorst 5:519e9002b10e 171 */
tomlankhorst 5:519e9002b10e 172 double step(double x);
tomlankhorst 5:519e9002b10e 173
tomlankhorst 5:519e9002b10e 174 /**
tomlankhorst 5:519e9002b10e 175 * Return poles of the BiQuad filter
tomlankhorst 5:519e9002b10e 176 * @return vector of std::complex poles
tomlankhorst 5:519e9002b10e 177 */
tomlankhorst 5:519e9002b10e 178 std::vector< std::complex<double> > poles( );
tomlankhorst 5:519e9002b10e 179
tomlankhorst 5:519e9002b10e 180 /**
tomlankhorst 5:519e9002b10e 181 * Return zeros of the BiQuad filter
tomlankhorst 5:519e9002b10e 182 * @return vector of std::complex zeros
tomlankhorst 5:519e9002b10e 183 */
tomlankhorst 5:519e9002b10e 184 std::vector< std::complex<double> > zeros( );
tomlankhorst 5:519e9002b10e 185
tomlankhorst 5:519e9002b10e 186 /**
tomlankhorst 5:519e9002b10e 187 * Is this biquad-chain stable?
tomlankhorst 5:519e9002b10e 188 * Checks if all poles lie within the unit-circle
tomlankhorst 5:519e9002b10e 189 * @return boolean whether the chain is stable or not
tomlankhorst 5:519e9002b10e 190 */
tomlankhorst 5:519e9002b10e 191 bool stable ();
tomlankhorst 5:519e9002b10e 192 };
tomlankhorst 5:519e9002b10e 193
tomlankhorst 5:519e9002b10e 194
tomlankhorst 5:519e9002b10e 195 #endif //BIQUAD_BIQUAD_H