Diff: BiQuad.h

Revision:

6:ee243782bf51

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.h	Thu Oct 20 12:07:30 2016 +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