filter-controller-cpp-implementation/

[utterances-bot]

Implementing a MATLAB digital filter or controllers in C++ | Tom Lankhorst

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 digital filters with different cut-off f

https://tomlankhorst.nl/filter-controller-cpp-implementation/

[tomlankhorst]

Original author: Jonti Olds @jontiolds
Original date: 2017-03-31T07:40:41Z

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 file\n------\n\n');
fprintf('BiQuadChain bqc;\n');
i = 0;
for s = sos.'
i = i + 1;
fprintf('BiQuad bq%d;\n', i);
end
fprintf('\n\n.cpp file\n------\n\n');
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

[tomlankhorst]

Original author: Jonti Olds @jontiolds
Original date: 2017-03-31T08:01:58Z

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.

[tomlankhorst]

Original author: Jonti Olds @jontiolds
Original date: 2017-03-31T08:06:36Z

I found a bug

if( resetStateOnGainChange )

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

should be

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

[tomlankhorst]

Original date: 2017-04-05T17:22:07Z

You're right! I changed it :)

[tomlankhorst]

Original date: 2017-04-05T17:45:13Z

Thanks, this is indeed an improvement.

[FahadRazaKhan]

Hello Tom,

I am wondering if I can use this Biquad library in ROS? Let's say if I have a vector V and I need it to pass through the BiQuad every step, V_filtered = bq.step(V); Would this work?

Thank you for all the hard work.

[tomlankhorst]

@FahadRazaKhan

Try std::transform

// output vector with same `value_type` and `size` as `V` 
Y = std::vector<decltype(V)::value_type>(V.size());

std::transform(V.cbegin(), V.cend(), Y.begin(), [&bq](const auto& v){
  return bq.step(v);
});

[FahadRazaKhan]

Thank you, Tom. I could not make it work since my vector is Eigen which does not have begin, end iterators. However, I am using it index-wise.

[tomlankhorst]

That'll work too!

Actually, Eigen does expose .begin() and .end() on 1D data and you can reshape 2D data to 1D data alternatively.

[FahadRazaKhan]

Thanks, I will try it again.

[FahadRazaKhan]

Hi Tom,

I am facing another issue with the BiQuad library. I successfully implemented a second order filter, however, when I try a first order LPF it gives me error during compiling "error: could not convert ‘{0.0, 2.7389999999999998e-1, -7.2609999999999997e-1}’ from ‘’ to ‘BiQuad’ ".

My Biquad coefficients for the first order filter are b0 = 0, b1= 0.2739, a1 = -0.7261.

I am wondering if you may help me out here.

Thank you.

[tomlankhorst]

The Biquad needs 5 parameters, set those unused to zero. BiQuad {.0, 0.2739, .0, -0.7261, .0}

[FahadRazaKhan]

Thanks Tom, it worked perfectly.