Implement ButterworthLowPassFilter

[GiulioRomualdi]

This PR implements the ButterworthLowPassFilter. The filter is described by the following transfer function

H(s) = \frac{1}{\sqrt{1 + \left(\frac{s}{\omega_c}\right)^{2N}}}

where $\omega_c$ is the cutoff frequency and $N$ is the order of the filter and $s$ is the Laplace variable.

To compute the coefficient of the filter we split the problem in three steps:

1. Compute the transfer function of the continuous system.
2. Compute the transfer function of the discrete system.
3. Compute the coefficients of the discrete system.

Compute the transfer function of the continuous system

What follows is taken from Passive and Active Network Analysis and Synthesis, Aram Budak, Houghton Mifflin, 1974 and from https://dsp.stackexchange.com/questions/79498/butterworth-filter-poles The poles of the Butterworth filter are evenly spaced on a circle of radius $\omega_c$ in the s-plane. The poles are given by

p_k = \omega_c e^{j \frac{\pi}{2} \left(1 + \frac{2k - 1}{2N}\right)}

where $k = 0, 1, \ldots, N-1$ and $j$ is the imaginary unit. By construction, the Butterworth filter does not have zeros. The gain of the filter is given by

K = \prod_{k=0}^{N-1} s_k = \omega_c^N

Compute the transfer function of the discrete system

As mentioned before, the transfer function of the discrete system is obtained by the bilinear transform

s = \frac{2}{\delta t} \frac{1 - z^{-1}}{1 + z^{-1}}

The poles of the discrete system are obtained by substituting the poles of the continuous system in the bilinear transformation as explained in https://it.mathworks.com/help/signal/ref/bilinear.html The poles of the discrete system are given by

p^d_k = \frac{1 + p_k \delta t/2}{1 - p_k \delta t/2}

where $p_k$ are the poles of the continuous system, $\delta t$ is the sampling time and $k = 0, 1, \ldots, N-1$. All the zeros of the continuous system are mapped to -1. Finally, the gain of the discrete system is given by

K^d = \text{real} \frac{K}{ \prod (\frac{2}{\delta T} - p_k) }

Compute the coefficients of the discrete system

Once we have the poles and the gain of the discrete system we can compute the coefficients of the filter by applying the Vieta's formulas. The transfer function of the discrete system is given by

H(z) = \frac{a _ n + a _ {n-1} z ^ {-1} + \ldots + a _ 1 z ^ {-n+1} + a _ 0 z ^ {-n}}{1 + b _ {n-1} z ^ {-1} + \ldots + b_1 z ^ {-n+1} + b_0 z^{-n}}

Once the numerator and the denominator are computed we can easily antitransform the transfer function to obtain the coefficients of the filter as

y[k] = \frac{1}{b_0} \left( a_0 x[k] + a_1 x[k-1] + \ldots + a_n x[k-n] - b_1 y[k-1] - \ldots - b_n y[k-n] \right)

where $x[k]$ is the input of the filter and $y[k]$ is the output of the filter.

[GiulioRomualdi]

The main idea is to use this filter to filter the signals coming from the robot. Authors of Quadratic Programming for Multirobot and Task-Space Force Control showed good performances when the filter was applied to the external force.

Nevertheless, this can be a valid alternative to first order low pass filter already implemented in the framework

[GiulioRomualdi]

@isorrentino @fils99 you may use it to filter the data online