Implementing a MATLAB digital filter or controllers in C++

Controllers and filters are often a good way to stabilise a system, condition a signal or make a system follow a reference. The usual approach is to model the system or signal in an application like MATLAB and then design a filter or controller. Either by applying design rules, some sort of optimisation algorithm or tuning parameters experimentally. When the design is finished the controller or filter is not ready for use yet. It is still necessary to realise it in practice. This is often done digitally on a micro controller or real-time computer. This article will describe a effective, open and fast approach to realising a filter or controller in C++.

Step responses of Butterworth Low-pass Filters
Step responses of Butterworth Low-pass digital filters with different cut-off frequencies

We assume a Linear Time Invariant (LTI) continuous time transfer function of any order. If the transfer function is already discrete, you can skip the discretisation section.

Transfer function discretisation

Discretisation only applies when the controller of filter has been designed in continuous time. As the micro-controller can only execute computations as a sequence of discrete steps it is necessary to convert the continuous time transfer-function to a discrete time approximation. Different methods exist for converting a continuous time transfer function to a digital approximation. We will use the Tustin-approximation as it yields the best match in the frequency domain between the continuous and discrete time transfers.

The sample time (Ts) or frequency (fs = 1/Ts) has to be determined. Faster sampling means a better approximation of the continuous time transfer-function. It will however require more computational power and performance of the Analog Digital Converter (ADC) input and Digital Analog Converter (DAC) / Pulse Width Modulation (PWM) outputs in terms of sampling frequency and noise. As a rule of thumb, select the sample frequency one or two orders of magnitude (10x – 100x) higher than the highest bandwidth that has to be controlled or filtered. For example, for a controlled pendulum with target bandwidth of 100 rad/s (16 Hz) select sample frequency 160 Hz or higher.

MATLAB provides tools for applying the Tustin-transformation to a continuous time transfer function. This is the continuous-to-digital c2d function. Assuming a transfer-function H, converting goes as follows:

Example: discretising a 5th order analog Butterworth filter

A 5th order analog Butterworth Low-pass filter is designed and discretised. The resulting and original transfer functions are compared in a bode diagram and step-response.

The resulting bode figure looks as follows:

Bode plot of continuous and discretised filter
Bode plot of continuous and discretised filter

It is clear that the filter stops working at the Nyquist frequency (Fs/2). Up to 2e3 rad/s the discretised filter resembles great similarity with the original filter. It attenuates more than the original filter as the frequency is increased.

Step response comparison of original and discretised filter
Step response comparison of original and discretised filter

The step-response shows very good similarity between the original and the discretised filter. MATLAB shows how the discrete samples as a stair-signal.

The biquad filter as building brick

It is risky to implement complex controllers on digital hardware as round off errors might render the filter unstable. The biquad however is a very simple digital filter which can be proven being stable even if it is cascaded. Converting a filter to a series of biquads is the key to successful digital filter implementation.

Biquad stands short for bi-quadratic: its transfer function is the ratio of two quadratic functions in the z-domain (discrete time).

H(z)=\frac{b_0+b_1z^{-1}+b_2z^{-2}}{a_0+a_1z^{-1}+a_2z^{-2}}

By dividing both numerator and denominator by a_0 the effective number of parameters is reduced to 5. If you are not interested in the filter details, you could go straight to the next section about converting filters to a series of biquads.

Direct form I

Finding the output of the filter for a sequence of input values is done by rearranging the equation y/u=H(z). This is visualized in the following figure:

Direct Form I, four storage elements are required
Direct Form I, four storage elements are required | CC BY-SA 3.0 Akilaa

Working from output back the following equation can be deduced:

y=b_0x+b_1xz^{-1}+b_2xz^{-2}-a_1yz^{-1}-a_2yz^{-2}

This is called direct form I and is the most straightforward structure. It requires four memory elements. Two for the input (u) and two for the output (y) values. By rearranging this can be improved a bit.

Direct form II

Rearranging the equation reduces the memory usage by two elements without losing precision or performance. This is direct form II and it will be used in the following C++ implementation.

Direct Form I, four storage elements are required
Direct Form I, four storage elements are required | CC BY-SA 3.0 Akilaa

When the equation is worked out it looks as follows:

y=b_0w+b_1wz^{-1}+b_2wz^{-2}

with

w=x-a_1wz^{-1}-a_2wz^{-2}

Working this out shows that it is equivalent to the first form.

Converting the filter to a series of biquads

Biquads are also called Second Order Sections (SOS). MATLAB contains functions for converting arbitrary filter types to second order sections. This is done by rearranging all poles and zeros and grouping them into second-order sections (biquads). Depending on the filter type suitable functions are tf2sos, ss2sos, etc.

Example: converting a digital filter to biquads

Continuing to work on the previously created Butterworth low-pass filter. The function to convert transfer-functions to biquads has numerator (b) and denominator (a) coefficients as input. These can be obtained from a transfer-function object in MATLAB as follows.

The resulting matrix SOS looks as follows:

The rows correspond to biquad sections, the columns correspond to respectively b0, b1, b2, a0, a1 and a2. Note that the coefficients have already been normalised.

C++ BiQuad implementation

The BiQuad class will store the five coefficients and two memory values. The storage footprint will thus be seven times the base type of storage. The BiQuadChain class will store pointers to BiQuad instances in a C++ std::vector.

The code can be found on GitHub. The following example application explains how to use the classes.

Generating the code for the filter directly from MATLAB is easy. Try the following snippet that takes a set of biquads sos.

Sponsored content 




  • Jonti Olds

    Hi Tom,

    Thanks for the info. I’ve been having a hard time figuring out how Matlab and C++ IIR filters go together and how to use Matlab filter design for C++ projects. Your C++ code is nice an easy to understand and get working.

    The thing that I found confusing was that fdatool gives an input gain and all the C/C++ implementations I found for biquads had no way of setting this value. So I added double g; to your class BiQuad and in double BiQuad::step(double x) I put x*=g; . Now I can control the input gain. I also changed your C++ generation code in Matlab to more like

    Fs = 192000; % Sampling Frequency
    N = 6; % Order
    Fpass = 10000; % Passband Frequency
    Apass = 1; % Passband Ripple (dB)
    Astop = 80; % Stopband Attenuation (dB)

    % Construct an FDESIGN object and call its ELLIP method.
    h = fdesign.lowpass(‘N,Fp,Ap,Ast’, N, Fpass, Apass, Astop, Fs);
    Hd = design(h, ‘ellip’);

    sos=Hd.sosmatrix;
    scalevalues=Hd.scalevalues;

    fprintf(‘n.h filen——nn’);
    fprintf(‘BiQuadChain bqc;n’);
    i = 0;
    for s = sos.’
    i = i + 1;
    fprintf(‘BiQuad bq%d;n’, i);
    end
    fprintf(‘nn.cpp filen——nn’);
    i = 0;
    for s = sos.’
    i = i + 1;
    fprintf(‘bq%d.set( %.9e, %.9e, %.9e, %.9e, %.9e );n’, i, s(1), s(2), s(3), s(5), s(6));
    end
    i=0;
    for g = scalevalues(1:end-1).’
    i = i + 1;
    fprintf(‘bq%d.gain=%.9e;n’,i,g);
    end
    fprintf(‘bqc’);
    i = 0;
    for s = sos.’
    i = i + 1;
    fprintf(‘.add( &bq%d )’, i);
    end
    fprintf(‘;n’);

    That give me an output of…

    .h file
    ——

    BiQuadChain bqc;
    BiQuad bq1;
    BiQuad bq2;
    BiQuad bq3;

    .cpp file
    ——

    .h file
    ——

    BiQuadChain bqc;
    BiQuad bq1;
    BiQuad bq2;
    BiQuad bq3;

    .cpp file
    ——

    bq1.set( 1.000000000e+00, -1.448553231e+00, 1.000000000e+00, -1.838841453e+00, 9.009598487e-01 );
    bq2.set( 1.000000000e+00, 9.621110853e-02, 1.000000000e+00, -1.832896546e+00, 8.478630198e-01 );
    bq3.set( 1.000000000e+00, -1.661029804e+00, 1.000000000e+00, -1.863639026e+00, 9.673926165e-01 );
    bq1.gain=2.827097186e-01;
    bq2.gain=2.008277409e-01;
    bq3.gain=3.864366325e-03;
    bqc.add( &bq1 ).add( &bq2 ).add( &bq3 );

    Now the filter works as I had hoped. The output gain seems to be 1 so I’ve ignored that.

    Thanks again.

    Jonti

  • Jonti Olds

    I see the transposed direct form II is more efficient. You can replace the step function with…

    y = B[0] * x + wz[0];

    wz[0] = B[1] * x – A[0] * y + wz[1];

    wz[1] = B[2] * x – A[1] * y;

    It still uses 9 binary operators but you get rid of the 2 shift operators.

    • Tom

      Thanks, this is indeed an improvement.

  • Jonti Olds

    I found a bug

    if( resetStateOnGainChange )

    wz[0] = 0; wz[1] = 0;

    should be

    if( resetStateOnGainChange )
    {wz[0] = 0; wz[1] = 0;}

    • Tom

      You’re right! I changed it 🙂