Tom Tom / biquadFilter

Simple biquad filter

Dependents:   EMG_Filter frdm_Motor_V2_2 frdm_Motor_V2_2 frdm_Motor_V2_3 ... more

BiQuad.cpp@5:519e9002b10e, 2016-10-02 (annotated)

Committer:
tomlankhorst
Date:
Sun Oct 02 20:45:07 2016 +0000
Revision:
5:519e9002b10e
Update

Who changed what in which revision?

UserRevisionLine numberNew 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 }