Nick van den Berg / Mbed 2 deprecated EMGfilter24

Filter for EMG signals The signal will be filtered using a notch, highpass and lowpass filter. The filtered signal will be compared to a preset threshold and according to the strength of the signal the program will perform an action. In this case it will assign a colour to a led.

Dependencies:   HIDScope MODSERIAL mbed

Fork of EMGfilter24 by Steven Spoolder

BiQuad.h@6:d2e6404e1cb8, 2016-11-01 (annotated)

Committer:
Nickname
Date:
Tue Nov 01 09:18:44 2016 +0000
Revision:
6:d2e6404e1cb8
Parent:
0:41226c0fd285 
Filter with extra notch filter 100 Hz

Who changed what in which revision?

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