Roy van Zijl / biquad-master

Poging tot BiQuad werkend

BiQuad.h@0:1bd88b638bfa, 2017-10-13 (annotated)

Committer:
RoyvZ
Date:
Fri Oct 13 08:59:10 2017 +0000
Revision:
0:1bd88b638bfa
JO, Hopenlijk werkt het wel op mijn computer;

Who changed what in which revision?

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