Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

BiQuad.h@9:e764cb50d343, 2016-11-07 (annotated)

Committer:
s1588141
Date:
Mon Nov 07 13:30:49 2016 +0000
Revision:
9:e764cb50d343
Parent:
5:4b2ff2a4664a 
Final program;

Who changed what in which revision?

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