Discrete time filters in feedback control

In this article, are discussed the complications in feedback control associated with noise. These are demonstrated on a PID controller implemented in C++. Similarly, the undesired effects discussed in the article will appear in advanced control methods such as LQR and LQG.

The velocity form of a controller

In most practical applications the actuator is a servomotor. In other words, the command variable can not be directly changed but only altered through its speed. If this is the case, one needs to use the control algorithm in its velocity form which will output the speed signal. In "s"-domain the velocity form for a PID controller is given by the following expression:

$$C(s) = s\left(K_p + \frac{K_i}{s} + s K_d\right)$$

$$C(s) = s K_p + K_i + s^2 K_d$$

Thus, the feedback signal is differentiated once for the proportional action and twice for the derivative action of the controller. Keep in mind that the $s$ operator gain grows as fast as the angular frequency $\omega$ of the signal, and the $s^2$ operator gain - grows as fast as $\omega^2$. Therefore, high-frequency noise, even though initially negligible, is amplified with high gain and the PID output becomes unacceptable because of the noise. This effect is often observed as chattering of the motor or high frequency spikes on the waveform.

Sensor noise

Suppose, a low-frequency signal is being collected from a sensor. For simplicity assume a sinewave $a\sin(\omega t)$ with an amplitude of $a = \sqrt{2}$ and the angular frequency of $\omega = 1$ rad/s. Let us now add some white noise $\xi(t)$ (Fig.1 b) with a relatively small variance of $\sigma = 1\cdot10^{-3}$ to the original signal. Note that the noise which is added to the signal is so small that it is not even seen on the graph (fig.1a).

$$u(t) = a\sin(\omega t) + \xi(t)$$

Using the definition of signal-to-noise ratio

$${SNR}_{dB} = 10\log_{10}{\left(\frac{\sigma_{signal}^2}{\sigma_{noise}^2}\right)}$$

we obtain ${SNR}_{u} = 60$ dB for the initial signal (Fig.1 a).

Suppose the controller has a sampling rate of $f_N = 1$ kHz meaning that the sampling period is $T = 1\cdot10^{-3}$ seconds. Then, it can be shown that 1st derivative of the signal $\dot{u}(t)$ will have signal-to-noise ratio of ${SNR}_{\dot{u}} = -2.9$ dB (Fig.1 c). And 2nd derivative $\ddot{u}(t)$ will have ${SNR}_{\ddot{u}} = -67.8$ dB (Fig.1 d).

Sensor signal with tiny noise

Noise which is added to the sensor signal

a) Measured signal $u(t) = a\sin(\omega t) + \xi(t)$
${SNR}_{u} = 60$ dB

b) Noise $\xi(t)$ with a small variance
$\sigma = 1\cdot10^{-3}$

1st discrete derivative of the sensor signal

1nd discrete derivative of the sensor signal

c) 1st discrete derivative of the signal $\dot{u}(t)$,
${SNR}_{\dot{u}} = -2.9$ dB

d) 2nd discrete derivative of the signal $\ddot{u}(t)$,
${SNR}_{\ddot{u}} = -67.8$ dB

Fig.1 Sensor signal with small noise, 1st discrete derivative and 2nd discrete derivative

Discrete filter

To make the comand signal clear and smooth one needs to filter the high-frequency noise from the sensor before differentiation. For this purpose a digital filter can be used:

$$H(z) = \frac{b_0 + b_1 z^{-1} + b_2 z^{-2} + \cdots}{1 + a_1 z^{-1} + a_2 z^{-2} + \cdots}$$

In fig.2 are shown the typical frequency responses of some common digital filters. For more information about digital filters read Wikipedia - Digital filter.

Typical frequency response plots of some common digital filters

Fig.2 Typical frequency responce plots of some common digital filters

How to interpret the transfer function H(z)

This section is a friendly reminder about how to interpret discrete transfer functions such as H(z).

A discrete transfer function defines the relation between two variables: the input $u(t)$ and the output $y(t)$. Note that both the input and the output are discretized: $u_k$ and $y_k$, where $k$ being the lag index. For example, $y_0$ would mean output value at the current moment of time, similarly, for instance, $u_1$ would mean the input value at the previous iteration, i.e. one sampling period ago in the past, and so on. The operator $z^{-k}$ is the lag operator. When $z^{-k}$ acts on a variable it yields the value of the variable lagging by $k$ discretization periods, e.g. the expression $z^{-2} u$ is nothing else but the delayed value of $u$, namely $z^{-2} u = u_2$. Thus, we can write down the equation represented by the transfer function $H(z)$:

$$\left(1 + a_1 z^{-1} + a_2 z^{-2} + \cdots \right) y = \left(b_0 + b_1 z^{-1} + b_2 z^{-2} + \cdots \right) u$$

$$y_0 + a_1 y_1 + a_2 y_2 + \cdots = b_0 u_0 + b_1 u_1 + b_2 u_2 + \cdots$$

Thus, we can express the output at the current moment of time as:

$$y_0 = -\left( a_1 y_1 + a_2 y_2 + \cdots \right) + b_0 u_0 + b_1 u_1 + b_2 u_2 + \cdots$$

For instance, an elliptic 2nd order low-pass filter can be designed in Matlab with a simple command [b,a] = ellip(n,Rp,Rs,Wp); and implemented in C++ (for Arduino) as follows (see Listing 1):

Listing 1. Discrete filter implementation on C++

// Elliptic Low Pass Filter 
/*   n = 2;          % filter order 
 *   Rp = 0.1;       % passband peak to peak ripple, dB 
 *   Rs = 1000;      % stopband attenuation, dB 
 *   fn = 1000;      % Nyquist frequency, Hz 
 *   fc = 1;         % cutoff frequency, Hz  
 *   Wp = fc/fn;     % normalized cutoff frequency */  

// PID coefficients
 double Kd = 0.1;
 double Kp = 1;
 double Ki = 10;

// Filter coefficients
 double a[3] = {1, -1.99254216118629, 0.992574747774901};
 double b[3] = {8.05339325492860e-06, 1.61067865098572e-05,  8.05339325492859e-06};  

// Filter variables
 double u[3] = {0};  // input of the filter
 double y[3] = {0};  // output of the filter

 void loop(){ // or timer interrupt service routine
   u[0] = (analogRead(A0) - analogRead(A1));   // measurement
   y[0] = -a[1]*y[1] - a[2]*y[2] + b[0]*u[0] + b[1]*u[1] + b[2]*u[2];  // filtering

   // Derivative 1 and 2
   dy = (y[0] - y[1])/T;
   d2y = (y[0] - 2*y[1] + y[2])/T/T;

   u[2] = u[1]; u[1] = u[0];  // shift register for the input
   y[2] = y[1]; y[1] = y[0];  // shift register for the output

   w = Ki*y[0] + Kp*dy + Kd*d2y;   // PID output signal: motor speed


Filtered signals

It is seen from Fig.3 that choosing the cutoff frequency $f_c$ of the filter is a trade off between the quality of filtering and the phase delay. Depending on the application, it may be reasonable to use either the filter depicted on Fig.3 b) or the one depicted on Fig.3 c), or a different filter with similar parameters. At the same time we can see that the filter on Fig.3 a) produces too much attenuation and has significant phase delay whereas the one on Fig.3 d) does not demonstrate acceptable level of filtering capability although it is faster than the others.

Action of the elliptic 2nd order filter with cutoff frequency of 0.1 Hz

Action of the elliptic 2nd order filter with cutoff frequency of 1 Hz

a) $f_c = 0.1$ Hz

b) $f_c = 1$ Hz

Action of the elliptic 2nd order filter with cutoff frequency of 10 Hz

Action of the elliptic 2nd order filter with cutoff frequency of 100 Hz

c) $f_c = 10$ Hz

d) $f_c = 100$ Hz

Fig.3 Discrete derivative of the signal filtered by digital filters having different cutoff frequencies $f_c$.
The blue line represents the analytical derivative and the red line is the computed one

Phase delay

Note that any filter results in an additional phase delay in the system, and thus the phase stability margin will change, which means that the system may often become unstable at high feedback gains. The higher the order of the filter - the sharper the cutoff region on the magnitude graph. At the same time, with higher orders the phase delay increase by 90 degrees with each extra order.

Let us consider a plant with the transfer function

$$G(s) = \frac{10}{s^2 + s + 10}$$

In Fig.5 is shown the control diagram of possible experimental setup arrangement

Fig.5 Control diagram


In Fig.6 are shown the Bode plot of the plant itself and of the plant combined with the discrete filter.

a) Plant $G(s)$ isolated

b) Plant $G(s)$ combined with the filter

Fig.6 Bode plot of the isolated and the filtered plant