Roy van Zijl
Poging tot BiQuad werkend
Revision 0:1bd88b638bfa, committed 2017-10-13
- Comitter:
- RoyvZ
- Date:
- Fri Oct 13 08:59:10 2017 +0000
- Commit message:
- JO, Hopenlijk werkt het wel op mijn computer;
Changed in this revision
diff -r 000000000000 -r 1bd88b638bfa BiQuad.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.cpp Fri Oct 13 08:59:10 2017 +0000
@@ -0,0 +1,159 @@
+#include "BiQuad.h"
+
+BiQuad::BiQuad() {
+ resetStateOnGainChange = true;
+ set( 1.0, 0.0, 0.0, 0.0, 0.0 );
+}
+
+BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) {
+ resetStateOnGainChange = true;
+ set( b0, b1, b2, a1, a2 );
+}
+
+BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) {
+ resetStateOnGainChange = true;
+ set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 );
+}
+
+void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts ) {
+
+ double b0, b1, b2, bd, a1, a2;
+
+ a1 = -4.0/(N*Ts+2.0);
+ a2 = -(N*Ts-2.0)/(N*Ts+2.0);
+
+ bd = ( N*Ts+2.0 );
+
+ b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
+ b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd;
+ b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
+
+ set( b0, b1, b2, a1, a2 );
+
+};
+
+void BiQuad::set(double b0, double b1, double b2, double a1, double a2) {
+
+ B[0] = b0; B[1] = b1; B[2] = b2;
+ A[0] = a1; A[1] = a2;
+
+ if( resetStateOnGainChange ) {
+ wz[0] = 0;
+ wz[1] = 0;
+ }
+
+}
+
+double BiQuad::step(double x) {
+
+ double y;
+
+ /* Direct form II transposed */
+ y = B[0] * x + wz[0];
+ wz[0] = B[1] * x - A[0] * y + wz[1];
+ wz[1] = B[2] * x - A[1] * y;
+
+ return y;
+
+}
+
+std::vector< std::complex<double> > BiQuad::poles() {
+
+ std::vector< std::complex<double> > poles;
+
+ std::complex<double> b2(A[0]*A[0],0);
+ std::complex<double> ds = std::sqrt( b2-4*A[1] );
+
+ poles.push_back( 0.5*(-A[0]+ds) );
+ poles.push_back( 0.5*(-A[0]-ds) );
+
+ return poles;
+
+}
+
+std::vector< std::complex<double> > BiQuad::zeros() {
+
+ std::vector< std::complex<double> > zeros;
+
+ std::complex<double> b2(B[1]*B[1],0);
+ std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] );
+
+ zeros.push_back( 0.5*(-B[1]+ds)/B[0] );
+ zeros.push_back( 0.5*(-B[1]-ds)/B[0] );
+
+ return zeros;
+
+}
+
+bool BiQuad::stable() {
+ bool stable = true;
+ std::vector< std::complex<double> > ps = poles();
+ for( size_t i = 0; i < ps.size(); i++ )
+ stable = stable & ( std::abs( ps[i] ) < 1 );
+ return stable;
+}
+
+void BiQuad::setResetStateOnGainChange( bool v ){
+ resetStateOnGainChange = v;
+}
+
+BiQuadChain &BiQuadChain::add(BiQuad *bq) {
+ biquads.push_back( bq );
+ return *this;
+}
+
+BiQuadChain operator*( BiQuad &bq1, BiQuad &bq2 ) {
+ BiQuadChain bqc;
+ bqc.add( &bq1 ).add( &bq2 );
+ return bqc;
+}
+
+double BiQuadChain::step(double x) {
+
+ size_t i;
+ size_t bqs;
+
+ bqs = biquads.size();
+
+ for( i = 0; i < bqs; i++ )
+ x = biquads[i]->step( x );
+
+ return x;
+}
+
+std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) {
+
+ std::vector< std::complex<double> > chain, bq;
+ size_t i;
+ size_t bqs;
+
+ bqs = biquads.size();
+
+ for( i = 0; i < bqs; i++ ){
+ bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles();
+ chain.insert( chain.end(), bq.begin(), bq.end() );
+ }
+
+ return chain;
+
+}
+
+std::vector< std::complex<double> > BiQuadChain::poles() {
+ return poles_zeros( false );
+}
+
+std::vector< std::complex<double> > BiQuadChain::zeros() {
+ return poles_zeros( true );
+}
+
+bool BiQuadChain::stable() {
+ bool stable = true;
+ for( size_t i = 0; i < biquads.size(); i++ )
+ stable = stable & biquads[i]->stable();
+ return stable;
+}
+
+BiQuadChain& BiQuadChain::operator*( BiQuad& bq ) {
+ add( &bq );
+ return *this;
+}
diff -r 000000000000 -r 1bd88b638bfa BiQuad.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.h Fri Oct 13 08:59:10 2017 +0000
@@ -0,0 +1,219 @@
+#ifndef BIQUAD_BIQUAD_H
+#define BIQUAD_BIQUAD_H
+
+#include <vector>
+#include <complex>
+
+class BiQuadChain;
+
+/** BiQuad class implements a single filter
+ *
+ * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl>
+ *
+ * Filters that - in the z domain - are the ratio of two quadratic functions. The general form is:
+ *
+ * b0 + b1 z^-1 + b2 z^-2
+ * H(z) = ----------------------
+ * a0 + a1 z^-1 + a2 z^-2
+ *
+ * Which is often normalized by dividing all coefficients by a0.
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include <complex>
+ *
+ * // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
+ * BiQuad bq1( 4.16599e-04, 8.33198e-04, 4.16599e-04, -1.47967e+00, 5.55822e-01 );
+ * BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.70096e+00, 7.88500e-01 );
+ *
+ * BiQuadChain bqc;
+ *
+ * int main() {
+ *
+ * // Add the biquads to the chain
+ * bqc.add( &bq1 ).add( &bq2 );
+ *
+ * // Find the poles of the filter
+ * std::cout << "Filter poles" << std::endl;
+ * std::vector< std::complex<double> > poles = bqc.poles();
+ * for( size_t i = 0; i < poles.size(); i++ )
+ * std::cout << "\t" << poles[i] << std::endl;
+ *
+ * // Find the zeros of the filter
+ * std::cout << "Filter zeros" << std::endl;
+ * std::vector< std::complex<double> > zeros = bqc.zeros();
+ * for( size_t i = 0; i < poles.size(); i++ )
+ * std::cout << "\t" << zeros[i] << std::endl;
+ *
+ * // Is the filter stable?
+ * std::cout << "This filter is " << (bqc.stable() ? "stable" : "instable") << std::endl;
+ *
+ * // Output the step-response of 20 samples
+ * std::cout << "Step response 20 samples" << std::endl;
+ * for( int i = 0; i < 20; i++ )
+ * std::cout << "\t" << bqc.step( 1.0 ) << std::endl;
+ * }
+ * @endcode
+ *
+ * https://github.com/tomlankhorst/biquad
+ *
+ */
+class BiQuad {
+
+private:
+
+ double B[3];
+ double A[2];
+ double wz[2];
+
+ bool resetStateOnGainChange;
+
+ /**
+ * Sets the gain parameters
+ */
+ void set( double b0, double b1, double b2, double a1, double a2 );
+
+public:
+
+ /**
+ * Initialize a unity TF biquad
+ * @return BiQuad instance
+ */
+ BiQuad( );
+
+ /**
+ * Initialize a normalized biquad filter
+ * @param b0
+ * @param b1
+ * @param b2
+ * @param a1
+ * @param a2
+ * @return BiQuad instance
+ */
+ BiQuad( double b0, double b1, double b2, double a1, double a2 );
+
+ /**
+ * Initialize a biquad filter with all six coefficients
+ * @param b0
+ * @param b1
+ * @param b2
+ * @param a0
+ * @param a1
+ * @param a2
+ * @return BiQuad instance
+ */
+ BiQuad( double b0, double b1, double b2, double a0, double a1, double a2 );
+
+ /**
+ * Initialize a PIDF biquad.
+ * Based on Tustin-approx (trapezoidal) of the continous time version.
+ * Behaviour equivalent to the PID controller created with the following MATLAB expression:
+ *
+ * C = pid( Kp, Ki, Kd, 1/N, Ts, 'IFormula', 'Trapezoidal', 'DFormula', 'Trapezoidal' );
+ *
+ * @param Kp Proportional gain
+ * @param Ki Integral gain
+ * @param Kd Derivative gain
+ * @param N Filter coefficient ( N = 1/Tf )
+ * @param Ts Timestep
+ */
+ void PIDF( double Kp, double Ki, double Kd, double N, double Ts );
+
+ /**
+ * Execute one digital timestep and return the result...
+ * @param x input of the filer
+ * @return output of the filter
+ */
+ double step( double x );
+
+ /**
+ * Return poles of the BiQuad filter
+ * @return vector of std::complex poles
+ */
+ std::vector< std::complex<double> > poles( );
+
+ /**
+ * Return zeros of the BiQuad filter
+ * @return vector of std::complex zeros
+ */
+ std::vector< std::complex<double> > zeros( );
+
+ /**
+ * Is this biquad stable?
+ * Checks if all poles lie within the unit-circle
+ * @return boolean whether the filter is stable or not
+ */
+ bool stable ();
+
+ /**
+ * Determines if the state variables are reset to zero on gain change.
+ * Can be used for changing gain parameters on the fly.
+ * @param v Value of the reset boolean
+ */
+ void setResetStateOnGainChange( bool v );
+
+};
+
+/**
+ * The BiQuadChain class implements a chain of BiQuad filters
+ */
+class BiQuadChain {
+
+private:
+ std::vector< BiQuad* > biquads;
+ std::vector< std::complex<double> > poles_zeros( bool zeros = false );
+
+public:
+
+ /**
+ * Add a BiQuad pointer to the list: bqc.add(&bq);
+ * @param bq Pointer to BiQuad instance
+ * @return Pointer to BiQuadChain
+ */
+ BiQuadChain &add( BiQuad *bq );
+
+ /**
+ * Execute a digital time step cascaded through all bq's
+ * @param x Input of the filter chain
+ * @return Output of the chain
+ */
+ double step(double x);
+
+ /**
+ * Return poles of the BiQuad filter
+ * @return vector of std::complex poles
+ */
+ std::vector< std::complex<double> > poles( );
+
+ /**
+ * Return zeros of the BiQuad filter
+ * @return vector of std::complex zeros
+ */
+ std::vector< std::complex<double> > zeros( );
+
+ /**
+ * Is this biquad-chain stable?
+ * Checks if all poles lie within the unit-circle
+ * @return boolean whether the chain is stable or not
+ */
+ bool stable ();
+
+ /**
+ * Appends a BiQuad to the chain
+ * Shorthand for .add(&bq)
+ * @param bq BiQuad
+ * @return Pointer to BiQuadChain
+ */
+ BiQuadChain &operator*( BiQuad& bq );
+
+};
+
+/**
+ * Multiply two BiQuads
+ * ... which in fact means appending them into a BiQuadChain
+ * @return BiQuadChain of the two BiQuads
+ */
+BiQuadChain operator*( BiQuad&, BiQuad& );
+
+#endif //BIQUAD_BIQUAD_H
\ No newline at end of file
diff -r 000000000000 -r 1bd88b638bfa CMakeLists.txt
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CMakeLists.txt Fri Oct 13 08:59:10 2017 +0000
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.3)
+project(BiQuad)
+
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+set(SOURCE_FILES main.cpp BiQuad.cpp BiQuad.h)
+add_executable(BiQuad ${SOURCE_FILES})
\ No newline at end of file
diff -r 000000000000 -r 1bd88b638bfa README.md
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/README.md Fri Oct 13 08:59:10 2017 +0000
@@ -0,0 +1,62 @@
+Biquad Filter Class
+===================
+
+This is a simple biquad filter class that enables live digital filtering on real-time devices and microcontrollers or signal processing on all other computer devices.
+
+The file main.cpp contains an application example.
+
+
+
+Please refer to [my blog post](https://tomlankhorst.nl/filter-controller-cpp-implementation/) for more details on the subject and application.
+
+Generating C++ code from a MATLAB transfer-function
+--------
+
+The following MATLAB function converts a SOS matrix to C++ code:
+```MATLAB
+function [ ] = tf2cppbq( sos )
+%TF2CPPBQ( sos ) Transfer-function to C++ code that initializes BiQuads and BiQuad chain
+% Input: matrix of second-order-sections (use tf2sos(H) for example).
+
+fprintf('\n');
+
+i = 0;
+for s = sos.'
+ i = i + 1;
+ fprintf('BiQuad bq%d( %.5e, %.5e, %.5e, %.5e, %.5e );\n', i, s(1), s(2), s(3), s(5), s(6));
+end
+
+fprintf('\nBiQuadChain bqc;\n');
+fprintf('bqc');
+i = 0;
+for s = sos.'
+ i = i + 1;
+ fprintf('.add( &bq%d )', i);
+end
+
+fprintf(';\n');
+
+end
+```
+
+Use it as follows:
+
+```MATLAB
+[b,a] = butter(4,0.2,'low');
+sos = tf2sos(b,a);
+tf2cppbq( sos );
+```
+
+Output:
+```C++
+BiQuad bq1( 4.82434e-03, 9.64869e-03, 4.82434e-03, -1.04860e+00, 2.96140e-01 );
+BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.32091e+00, 6.32739e-01 );
+
+BiQuadChain bqc;
+bqc.add( &bq1 ).add( &bq2 );
+```
+
+
+License
+-------
+MIT
\ No newline at end of file