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