Steven Spoolder / Mbed 2 deprecated EMGfilter24_2

EMGfilter24_2 ERROR

Dependencies:   HIDScope MODSERIAL mbed

BiQuad.cpp@0:41226c0fd285, 2016-10-24 (annotated)

Committer:
Iknowright
Date:
Mon Oct 24 14:46:20 2016 +0000
Revision:
0:41226c0fd285
EMGfilter24_2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Iknowright 0:41226c0fd285 1 #include "BiQuad.h"
Iknowright 0:41226c0fd285 2 #include <stdlib.h>
Iknowright 0:41226c0fd285 3 #include <stddef.h>
Iknowright 0:41226c0fd285 4
Iknowright 0:41226c0fd285 5 BiQuad::BiQuad() {
Iknowright 0:41226c0fd285 6 resetStateOnGainChange = true;
Iknowright 0:41226c0fd285 7 set( 1.0, 0.0, 0.0, 0.0, 0.0 );
Iknowright 0:41226c0fd285 8 }
Iknowright 0:41226c0fd285 9
Iknowright 0:41226c0fd285 10 BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) {
Iknowright 0:41226c0fd285 11 resetStateOnGainChange = true;
Iknowright 0:41226c0fd285 12 set( b0, b1, b2, a1, a2 );
Iknowright 0:41226c0fd285 13 }
Iknowright 0:41226c0fd285 14
Iknowright 0:41226c0fd285 15 BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) {
Iknowright 0:41226c0fd285 16 resetStateOnGainChange = true;
Iknowright 0:41226c0fd285 17 set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 );
Iknowright 0:41226c0fd285 18 }
Iknowright 0:41226c0fd285 19
Iknowright 0:41226c0fd285 20 void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts ) {
Iknowright 0:41226c0fd285 21
Iknowright 0:41226c0fd285 22 double b0, b1, b2, bd, a1, a2;
Iknowright 0:41226c0fd285 23
Iknowright 0:41226c0fd285 24 a1 = -4.0/(N*Ts+2.0);
Iknowright 0:41226c0fd285 25 a2 = -(N*Ts-2.0)/(N*Ts+2.0);
Iknowright 0:41226c0fd285 26
Iknowright 0:41226c0fd285 27 bd = ( N*Ts+2.0 );
Iknowright 0:41226c0fd285 28
Iknowright 0:41226c0fd285 29 b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
Iknowright 0:41226c0fd285 30 b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd;
Iknowright 0:41226c0fd285 31 b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
Iknowright 0:41226c0fd285 32
Iknowright 0:41226c0fd285 33 set( b0, b1, b2, a1, a2 );
Iknowright 0:41226c0fd285 34
Iknowright 0:41226c0fd285 35 };
Iknowright 0:41226c0fd285 36
Iknowright 0:41226c0fd285 37 void BiQuad::set(double b0, double b1, double b2, double a1, double a2) {
Iknowright 0:41226c0fd285 38
Iknowright 0:41226c0fd285 39 B[0] = b0; B[1] = b1; B[2] = b2;
Iknowright 0:41226c0fd285 40 A[0] = a1; A[1] = a2;
Iknowright 0:41226c0fd285 41
Iknowright 0:41226c0fd285 42 if( resetStateOnGainChange )
Iknowright 0:41226c0fd285 43 wz[0] = 0; wz[1] = 0;
Iknowright 0:41226c0fd285 44
Iknowright 0:41226c0fd285 45 }
Iknowright 0:41226c0fd285 46
Iknowright 0:41226c0fd285 47 double BiQuad::step(double x) {
Iknowright 0:41226c0fd285 48
Iknowright 0:41226c0fd285 49 double y,w;
Iknowright 0:41226c0fd285 50
Iknowright 0:41226c0fd285 51 /* Direct form II */
Iknowright 0:41226c0fd285 52 w = x - A[0]*wz[0] - A[1]*wz[1];
Iknowright 0:41226c0fd285 53 y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
Iknowright 0:41226c0fd285 54
Iknowright 0:41226c0fd285 55 /* Shift */
Iknowright 0:41226c0fd285 56 wz[1] = wz[0];
Iknowright 0:41226c0fd285 57 wz[0] = w;
Iknowright 0:41226c0fd285 58
Iknowright 0:41226c0fd285 59 return y;
Iknowright 0:41226c0fd285 60
Iknowright 0:41226c0fd285 61 }
Iknowright 0:41226c0fd285 62
Iknowright 0:41226c0fd285 63 std::vector< std::complex<double> > BiQuad::poles() {
Iknowright 0:41226c0fd285 64
Iknowright 0:41226c0fd285 65 std::vector< std::complex<double> > poles;
Iknowright 0:41226c0fd285 66
Iknowright 0:41226c0fd285 67 std::complex<double> b2(A[0]*A[0],0);
Iknowright 0:41226c0fd285 68 std::complex<double> ds = std::sqrt( b2-4*A[1] );
Iknowright 0:41226c0fd285 69
Iknowright 0:41226c0fd285 70 poles.push_back( 0.5*(-A[0]+ds) );
Iknowright 0:41226c0fd285 71 poles.push_back( 0.5*(-A[0]-ds) );
Iknowright 0:41226c0fd285 72
Iknowright 0:41226c0fd285 73 return poles;
Iknowright 0:41226c0fd285 74
Iknowright 0:41226c0fd285 75 }
Iknowright 0:41226c0fd285 76
Iknowright 0:41226c0fd285 77 std::vector< std::complex<double> > BiQuad::zeros() {
Iknowright 0:41226c0fd285 78
Iknowright 0:41226c0fd285 79 std::vector< std::complex<double> > zeros;
Iknowright 0:41226c0fd285 80
Iknowright 0:41226c0fd285 81 std::complex<double> b2(B[1]*B[1],0);
Iknowright 0:41226c0fd285 82 std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] );
Iknowright 0:41226c0fd285 83
Iknowright 0:41226c0fd285 84 zeros.push_back( 0.5*(-B[1]+ds)/B[0] );
Iknowright 0:41226c0fd285 85 zeros.push_back( 0.5*(-B[1]-ds)/B[0] );
Iknowright 0:41226c0fd285 86
Iknowright 0:41226c0fd285 87 return zeros;
Iknowright 0:41226c0fd285 88
Iknowright 0:41226c0fd285 89 }
Iknowright 0:41226c0fd285 90
Iknowright 0:41226c0fd285 91 bool BiQuad::stable() {
Iknowright 0:41226c0fd285 92 bool stable = true;
Iknowright 0:41226c0fd285 93 std::vector< std::complex<double> > ps = poles();
Iknowright 0:41226c0fd285 94 for( size_t i = 0; i < ps.size(); i++ )
Iknowright 0:41226c0fd285 95 stable = stable & ( std::abs( ps[i] ) < 1 );
Iknowright 0:41226c0fd285 96 return stable;
Iknowright 0:41226c0fd285 97 }
Iknowright 0:41226c0fd285 98
Iknowright 0:41226c0fd285 99 void BiQuad::setResetStateOnGainChange( bool v ){
Iknowright 0:41226c0fd285 100 resetStateOnGainChange = v;
Iknowright 0:41226c0fd285 101 }
Iknowright 0:41226c0fd285 102
Iknowright 0:41226c0fd285 103 BiQuadChain &BiQuadChain::add(BiQuad *bq) {
Iknowright 0:41226c0fd285 104 biquads.push_back( bq );
Iknowright 0:41226c0fd285 105 return *this;
Iknowright 0:41226c0fd285 106 }
Iknowright 0:41226c0fd285 107
Iknowright 0:41226c0fd285 108 double BiQuadChain::step(double x) {
Iknowright 0:41226c0fd285 109
Iknowright 0:41226c0fd285 110 int i;
Iknowright 0:41226c0fd285 111 size_t bqs;
Iknowright 0:41226c0fd285 112
Iknowright 0:41226c0fd285 113 bqs = biquads.size();
Iknowright 0:41226c0fd285 114
Iknowright 0:41226c0fd285 115 for( i = 0; i < bqs; i++ )
Iknowright 0:41226c0fd285 116 x = biquads[i]->step( x );
Iknowright 0:41226c0fd285 117
Iknowright 0:41226c0fd285 118 return x;
Iknowright 0:41226c0fd285 119 }
Iknowright 0:41226c0fd285 120
Iknowright 0:41226c0fd285 121 std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) {
Iknowright 0:41226c0fd285 122
Iknowright 0:41226c0fd285 123 std::vector< std::complex<double> > chain, bq;
Iknowright 0:41226c0fd285 124 int i;
Iknowright 0:41226c0fd285 125 size_t bqs;
Iknowright 0:41226c0fd285 126
Iknowright 0:41226c0fd285 127 bqs = biquads.size();
Iknowright 0:41226c0fd285 128
Iknowright 0:41226c0fd285 129 for( i = 0; i < bqs; i++ ){
Iknowright 0:41226c0fd285 130 bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles();
Iknowright 0:41226c0fd285 131 chain.insert( chain.end(), bq.begin(), bq.end() );
Iknowright 0:41226c0fd285 132 }
Iknowright 0:41226c0fd285 133
Iknowright 0:41226c0fd285 134 return chain;
Iknowright 0:41226c0fd285 135
Iknowright 0:41226c0fd285 136 }
Iknowright 0:41226c0fd285 137
Iknowright 0:41226c0fd285 138 std::vector< std::complex<double> > BiQuadChain::poles() {
Iknowright 0:41226c0fd285 139 return poles_zeros( false );
Iknowright 0:41226c0fd285 140 }
Iknowright 0:41226c0fd285 141
Iknowright 0:41226c0fd285 142 std::vector< std::complex<double> > BiQuadChain::zeros() {
Iknowright 0:41226c0fd285 143 return poles_zeros( true );
Iknowright 0:41226c0fd285 144 }
Iknowright 0:41226c0fd285 145
Iknowright 0:41226c0fd285 146 bool BiQuadChain::stable() {
Iknowright 0:41226c0fd285 147 bool stable = true;
Iknowright 0:41226c0fd285 148 for( size_t i = 0; i < biquads.size(); i++ )
Iknowright 0:41226c0fd285 149 stable = stable & biquads[i]->stable();
Iknowright 0:41226c0fd285 150 return stable;
Iknowright 0:41226c0fd285 151 }