In previous lessons, we treated filters (e.g., low-pass, high-pass, notch) mainly as transfer functions shaping frequency response. In a feedback control loop, the same filters:

• Increase attenuation of noise and unwanted dynamics at certain frequencies.
• Introduce additional phase lag and hence group delay.
• Are implemented with finite order, finite precision, and finite sampling rate.

Consider a generic proper rational filter \( F(s) \) of order \( n \):

\[ F(s) = K \frac{\displaystyle\prod_{i=1}^{m} (s - z_i)} {\displaystyle\prod_{j=1}^{n} (s - p_j)}, \quad n \ge m, \quad \Re(p_j) < 0. \]

When inserted in the loop gain \( L(s) = C(s)G(s)F(s) \), the filter changes both the magnitude \( |L(j\omega)| \) and the phase \( \arg L(j\omega) \), which directly influence stability margins.

flowchart TD
  R["Reference r"] --> C["Controller C(s)"]
  C --> Fm["Measurement filter Fm(s)"]
  Fm --> SUM["+ error computation"]
  SUM --> PLANT["Plant G(s)"]
  PLANT --> Y["Output y"]
  Y -->|feedback| Fm
  PLANT --> D["Disturbances d"]
  Fm --> N["Noise attenuation vs phase lag"]
        

The central design tension is: higher-order filters give steeper attenuation but also more phase lag and implementation sensitivity.

For a proper low-pass filter with no zeros at infinity (i.e. \( m \le n \)) and real, stable poles, the high-frequency asymptotic behaviour is dominated by the denominator. Assume for simplicity \( m = 0 \):

\[ F(s) = K \prod_{j=1}^{n} \frac{1}{s - p_j}. \]

At high frequency \( \omega \), with \( s = j\omega \) and \( \omega \gg |p_j| \), we approximate

\[ |F(j\omega)| = |K| \prod_{j=1}^{n} \frac{1}{|j\omega - p_j|} \approx |K| \prod_{j=1}^{n} \frac{1}{\omega} = |K| \,\omega^{-n}. \]

The Bode magnitude in decibels is

\[ M(\omega) = 20 \log_{10} |F(j\omega)| \approx 20 \log_{10} |K| - 20 n \log_{10} \omega. \]

Differentiating with respect to \( \log_{10} \omega \) gives the high-frequency slope

\[ \frac{\mathrm{d}M(\omega)}{\mathrm{d}\log_{10}\omega} = -20n \quad \text{dB per decade}. \]

Thus, each additional pole (for a low-pass) increases high-frequency attenuation by 20 dB/decade, but also contributes phase lag. This is why higher-order filters are attractive for noise suppression but dangerous for stability.

For a general proper filter with \( m \) zeros and \( n \) poles, the asymptotic slope becomes

\[ \frac{\mathrm{d}M(\omega)}{\mathrm{d}\log_{10}\omega} \approx 20(m - n)\;\text{dB per decade}. \]

In control-oriented filter design we typically factor high-order filters into first- and second-order sections, each contributing a known slope and phase shape, then assemble them to meet magnitude and phase specifications.

The phase of a stable, minimum-phase filter \( F(j\omega) \) is defined as \( \varphi_f(\omega) = \arg F(j\omega) \). The group delay is

\[ \tau_g(\omega) = -\frac{\mathrm{d}\varphi_f(\omega)}{\mathrm{d}\omega}. \]

For a first-order low-pass filter:

\[ F_1(s) = \frac{\omega_c}{s + \omega_c}, \]

we have

\[ F_1(j\omega) = \frac{\omega_c}{\omega_c + j\omega} = \frac{\omega_c}{\sqrt{\omega^2 + \omega_c^2}}\, e^{-j\arctan\left(\frac{\omega}{\omega_c}\right)}. \]

Hence the phase lag is

\[ \varphi_1(\omega) = -\arctan\left(\frac{\omega}{\omega_c}\right), \]

and the group delay becomes

\[ \tau_{g,1}(\omega) = -\frac{\mathrm{d}\varphi_1}{\mathrm{d}\omega} = -\frac{\mathrm{d}}{\mathrm{d}\omega} \left[-\arctan\left(\frac{\omega}{\omega_c}\right)\right] = \frac{1}{\omega_c}\frac{1}{1 + (\omega / \omega_c)^2}. \]

At low frequencies, \( \omega \ll \omega_c \), this is approximately constant:

\[ \tau_{g,1}(\omega) \approx \frac{1}{\omega_c}. \]

Thus, a first-order low-pass behaves approximately as a pure time delay of magnitude \( 1/\omega_c \) at low frequencies.

For a standard second-order low-pass:

\[ F_2(s) = \frac{\omega_n^2}{s^2 + 2\zeta \omega_n s + \omega_n^2}, \]

the phase is

\[ \varphi_2(\omega) = -\arctan\left( \frac{2\zeta (\omega / \omega_n)} {1 - (\omega / \omega_n)^2} \right), \]

which yields a steeper phase transition around \( \omega \approx \omega_n \) than the first-order filter. This additional lag is what often limits the admissible filter order in loops with tight phase-margin requirements.

Let \( L_0(s) \) be the original loop transfer function with phase margin \( \mathrm{PM}_0 \) at crossover frequency \( \omega_c \). Suppose we insert a first-order low-pass filter in the measurement path:

\[ F_m(s) = \frac{1}{1 + s \tau_f}. \]

The phase lag contributed by \( F_m \) at \( \omega_c \) is

\[ \Delta\varphi_f(\omega_c) = -\arctan\left(\omega_c \tau_f\right). \]

Neglecting the small change in crossover frequency, the new phase margin is

\[ \mathrm{PM} \approx \mathrm{PM}_0 - \arctan\left(\omega_c \tau_f\right). \]

If we require a minimum phase margin \( \mathrm{PM}_\text{min} \), the design constraint becomes

\[ \mathrm{PM}_0 - \arctan\left(\omega_c \tau_f\right) \ge \mathrm{PM}_\text{min}. \]

Equivalently,

\[ \arctan\left(\omega_c \tau_f\right) \le \mathrm{PM}_0 - \mathrm{PM}_\text{min}, \]

and since \( \arctan \) is strictly increasing,

\[ \omega_c \tau_f \le \tan\left(\mathrm{PM}_0 - \mathrm{PM}_\text{min}\right). \]

Therefore a simple but powerful engineering rule is:

\[ \tau_f \le \frac{1}{\omega_c}\, \tan\bigl(\mathrm{PM}_0 - \mathrm{PM}_\text{min}\bigr). \]

This inequality tells us how fast the measurement filter must be, relative to the loop crossover, to preserve phase margin. Higher-order filters can be seen as multiple first-order sections and typically require even smaller \( \tau \) per section to keep the total lag acceptable.

In control applications (including robotic joints, UAV attitude control, and mobile robots), typical rules for filter order selection are:

• Use first-order filters when only mild attenuation is needed and phase margin is tight.
• Use second-order filters (often with moderate damping) when a sharper roll-off is needed, e.g., to suppress a flexible mode.
• Avoid high-order monolithic filters; instead, cascade first- and second-order sections and verify phase margins after each change.
• Align measurement-filter bandwidth with or slightly above the closed-loop bandwidth to avoid sluggish feedback.

flowchart TD
  S["Start: noise and unmodeled dynamics identified"] --> A["Set required attenuation at high frequency"]
  A --> B["Compute max phase lag allowed from margins"]
  B --> C["Decide filter type: 'low-pass', 'notch', 'band-pass'"]
  C --> D["Pick candidate order (1st or 2nd)"]
  D --> E["Tune cutoff and damping; simulate Bode of L(s)"]
  E --> F{"Margins ok and \nnoise attenuated?"}
  F -->|yes| G["Implement in continuous/discrete time"]
  F -->|no| H["Adjust order or bandwidth; iterate"]
        

This loop-shaping viewpoint treats filters as part of the overall controller design rather than afterthought add-ons.

Real controllers in robotics and mechatronics are almost always implemented digitally with sampling period \( T_s \). A continuous-time filter \( F(s) \) must therefore be converted to a discrete-time transfer function \( F(z) \) or an equivalent difference equation.

For the first-order low-pass

\[ F(s) = \frac{1}{1 + s \tau_f}, \]

one simple discretization method is the backward Euler approximation \( s \approx \frac{z - 1}{T_s z} \). Substituting and solving for \( F(z) \):

\[ F(z) \approx \frac{1}{1 + \tau_f \frac{z - 1}{T_s z}} = \frac{T_s z}{T_s z + \tau_f (z - 1)}. \]

Multiply numerator and denominator to obtain a standard rational form in \( z^{-1} \):

\[ F(z) = \frac{b_0 + b_1 z^{-1}}{1 + a_1 z^{-1}}, \]

with coefficients

\[ b_0 = \frac{T_s}{T_s + \tau_f}, \quad b_1 = 0, \quad a_1 = -\frac{T_s}{T_s + \tau_f}. \]

The corresponding difference equation is

\[ y[k] = -a_1 y[k-1] + b_0 u[k], \]

where \( u[k] \) is the sampled input and \( y[k] \) is the filtered output. Note that backward Euler produces a stable discrete filter whenever the continuous-time filter is stable.

More accurate mappings (e.g., bilinear transform) better preserve frequency response around the desired bandwidth but require more algebra. These techniques are standard in digital control courses; for this lesson we focus on the main structural effects: sampling introduces additional delay of roughly \( T_s/2 \) and requires that the discrete filter update completes well within one sampling period.

When deploying filters in an embedded controller (e.g., motor driver board, microcontroller on a robot), numerical aspects become crucial:

• Filter structure: Direct Form I vs Direct Form II vs cascade of biquadratic sections. Cascades generally yield better numerical properties for higher orders.
• Finite precision: Fixed-point arithmetic requires careful scaling of coefficients and internal states to avoid overflow.
• Execution time: The filter must execute in much less than \( T_s \) to avoid overruns and effective extra delay.
• Saturation and overflow: Implementation should include saturation and optional anti-windup when filters contain integrator-like behaviour.

A general discrete-time linear filter of order \( n \) and \( m \) zeros can be written as:

\[ y[k] = -\sum_{i=1}^{n} a_i y[k-i] + \sum_{j=0}^{m} b_j u[k-j], \]

where the coefficients \( a_i, b_j \) come from the chosen discretization method or from direct digital filter design. Efficient implementations maintain the last \( \max\{n,m\} \) samples in state variables and update them each sampling instant.

We now illustrate simple implementations of a first-order low-pass filter used to smooth a noisy joint-velocity measurement in a robotic manipulator. The same structure extends to higher-order filters and different signals.

8.1 Python (SciPy / control, ROS ecosystem)

In Python, robotic applications often rely on scipy.signal, the python-control library, and ROS (rclpy) nodes. The snippet below designs a continuous first-order low-pass filter and discretizes it for a sampled control loop:

import numpy as np
from scipy import signal

# Design parameters
Ts = 0.002   # 2 ms sampling for a fast servo loop
tau_f = 0.01 # filter time constant (10 ms)

# Continuous-time first-order low-pass: F(s) = 1 / (1 + tau_f s)
num_c = [1.0]
den_c = [tau_f, 1.0]

# Discretize using backward Euler (or 'gbt'/'bilinear' for higher fidelity)
system_d = signal.cont2discrete((num_c, den_c), Ts, method="gbt", alpha=1.0)
b_d, a_d = system_d[0].flatten(), system_d[1].flatten()

print("Discrete coefficients b:", b_d, "a:", a_d)

class FirstOrderLPF:
    def __init__(self, b, a):
        self.b0 = b[0]
        self.a1 = a[1]
        self.y1 = 0.0

    def filter(self, u):
        # y[k] = -a1*y[k-1] + b0*u[k]
        y = -self.a1 * self.y1 + self.b0 * u
        self.y1 = y
        return y

lpf = FirstOrderLPF(b_d, a_d)

# Example: filtering a noisy velocity measurement
def filter_velocity_stream(samples):
    return [lpf.filter(v) for v in samples]
      

In a ROS-based robot, this code would typically run inside a velocity-estimation node, smoothing encoder-derived velocities before sending them to a JointTrajectoryController.

8.2 C++ (Embedded / ROS control)

In C++ robotic controllers (e.g., ROS ros_control hardware interfaces), we often implement the difference equation directly for speed and deterministic timing:

#include <cmath>

class FirstOrderLowPass {
public:
    FirstOrderLowPass(double Ts, double tau_f)
    {
        // Backward Euler discretization of F(s) = 1 / (1 + tau_f s)
        double denom = Ts + tau_f;
        b0_ = Ts / denom;
        a1_ = -Ts / denom;
        y1_ = 0.0;
    }

    double filter(double u)
    {
        // y[k] = -a1*y[k-1] + b0*u[k]
        double y = -a1_ * y1_ + b0_ * u;
        y1_ = y;
        return y;
    }

private:
    double b0_;
    double a1_;
    double y1_;
};

// Example usage in a joint controller update function:
// FirstOrderLowPass vel_filter(Ts, 0.01);
// double vel_filtered = vel_filter.filter(vel_measured);
      

In the ROS ecosystem, a filter like this is often wrapped inside a node or combined with the filters package, then integrated with ros_control or other robotic middleware.

8.3 Java (FRC / WPILib-style robotics)

In Java-based robot frameworks such as FRC WPILib, linear filters are commonly used to clean sensor data:

// Simple first-order low-pass filter implemented manually
public class FirstOrderLPF {
    private final double b0;
    private final double a1;
    private double y1 = 0.0;

    public FirstOrderLPF(double Ts, double tau_f) {
        double denom = Ts + tau_f;
        this.b0 = Ts / denom;
        this.a1 = -Ts / denom;
    }

    public double filter(double u) {
        double y = -a1 * y1 + b0 * u;
        y1 = y;
        return y;
    }
}

// In a robot control loop:
// FirstOrderLPF gyroFilter = new FirstOrderLPF(0.005, 0.02);
// double filteredRate = gyroFilter.filter(rawGyroRate);
      

WPILib also provides built-in linear filters (e.g., moving-average or single-pole IIR) that internally implement similar difference equations with attention to numerical robustness.

8.4 MATLAB/Simulink (Control System Toolbox, Robotics System Toolbox)

MATLAB offers high-level functions to define and discretize filters, and Simulink provides corresponding blocks such as Transfer Fcn and Discrete Transfer Fcn:

Ts   = 0.002;
tauf = 0.01;

s = tf('s');
F   = 1 / (1 + tauf*s);      % continuous-time filter
Fd  = c2d(F, Ts, 'tustin');  % discrete-time version

% Bode and step responses to check magnitude and phase effects
figure;
bode(F, Fd);
legend('F(s)', 'F(z)');

% In Simulink:
% - Insert a "Transfer Fcn" block with numerator [1], denominator [tauf 1]
%   in the measurement path for continuous simulation.
% - For code generation and hardware deployment, use "Discrete Transfer Fcn"
%   and set its numerator and denominator to the coefficients of Fd.
      

In robotics applications, the filtered signals can be fed into controllers provided by the Robotics System Toolbox or custom Simulink models for manipulators and mobile platforms.

8.5 Wolfram Mathematica (Continuous and Discrete Models)

Mathematica can symbolically manipulate transfer functions and convert them to discrete-time for simulation:

(* Continuous-time first-order low-pass *)
tauf = 0.01;
F = TransferFunctionModel[1/(1 + tauf*s), s];

(* Discrete-time version with sampling time Ts *)
Ts = 0.002;
Fd = ToDiscreteTimeModel[F, Ts, Method -> "BackwardDifference"];

(* Frequency response to inspect magnitude and phase *)
BodePlot[{F, Fd}, {10, 1*^4},
  PlotLegends -> {"F(s)", "F(z)"},
  GridLines -> Automatic];

(* Extract difference equation coefficients *)
{numz, denz} = TransferFunctionExpand[Fd]["NumeratorDenominator"];
      

Mathematica is particularly useful for deriving symbolic relationships between filter parameters, phase lag, and group delay before coding the implementation in a real-time language.

Problem 1 (High-Frequency Slope of an n-th Order Low-Pass): Consider a proper, strictly stable low-pass filter with transfer function \( F(s) = K / \prod_{j=1}^{n} (s - p_j) \), where \(\Re(p_j) < 0\). Show that its high-frequency magnitude asymptote in the Bode plot has slope \(-20n\) dB/decade.

Solution:

For \(\omega \gg |p_j|\) we approximate \(|j\omega - p_j| \approx \omega\), so

\[ |F(j\omega)| = |K| \prod_{j=1}^{n} \frac{1}{|j\omega - p_j|} \approx |K| \prod_{j=1}^{n} \frac{1}{\omega} = |K|\,\omega^{-n}. \]

The magnitude in decibels is

\[ M(\omega) = 20 \log_{10} |F(j\omega)| \approx 20 \log_{10} |K| - 20 n \log_{10}\omega, \]

hence the slope with respect to \(\log_{10}\omega\) is \(-20n\) dB per decade.

Problem 2 (Phase-Margin Constraint for a Sensor Filter): An existing loop has crossover frequency \(\omega_c\) and phase margin \(\mathrm{PM}_0\). You add a first-order low-pass measurement filter \(F_m(s) = 1/(1 + s \tau_f)\). Derive a bound on \tau_f that guarantees the new phase margin remains above \(\mathrm{PM}_\text{min}\), assuming the crossover frequency does not change significantly.

Solution:

The additional phase lag at \(\omega_c\) is

\[ \Delta\varphi_f(\omega_c) = -\arctan(\omega_c \tau_f). \]

Therefore, the new phase margin is

\[ \mathrm{PM} \approx \mathrm{PM}_0 - \arctan(\omega_c \tau_f). \]

Requiring \(\mathrm{PM} \ge \mathrm{PM}_\text{min}\) yields

\[ \mathrm{PM}_0 - \arctan(\omega_c \tau_f) \ge \mathrm{PM}_\text{min} \;\Rightarrow\; \arctan(\omega_c \tau_f) \le \mathrm{PM}_0 - \mathrm{PM}_\text{min}. \]

Because \(\arctan\) is strictly increasing, \(\omega_c \tau_f \le \tan(\mathrm{PM}_0 - \mathrm{PM}_\text{min})\), so the required bound is

\[ \tau_f \le \frac{1}{\omega_c} \tan\bigl(\mathrm{PM}_0 - \mathrm{PM}_\text{min}\bigr). \]

Problem 3 (Group Delay of First-Order Low-Pass): For the first-order low-pass \(F_1(s) = \omega_c/(s + \omega_c)\), derive the group delay \(\tau_{g,1}(\omega)\) and show that \(\tau_{g,1}(\omega)\) tends to \(1/\omega_c\) as \(\omega \to 0\).

Solution:

We already know that the phase of \(F_1\) is \(\varphi_1(\omega) = -\arctan(\omega/\omega_c)\). Differentiating:

\[ \tau_{g,1}(\omega) = -\frac{\mathrm{d}\varphi_1(\omega)}{\mathrm{d}\omega} = -\frac{\mathrm{d}}{\mathrm{d}\omega} \left[-\arctan\left(\frac{\omega}{\omega_c}\right)\right] = \frac{1}{\omega_c} \frac{1}{1 + (\omega/\omega_c)^2}. \]

Taking the limit as \(\omega \to 0\),

\[ \lim_{\omega \to 0} \tau_{g,1}(\omega) = \frac{1}{\omega_c}, \]

confirming that the filter behaves like a pure delay of magnitude \(1/\omega_c\) at low frequencies.

Problem 4 (Backward Euler Discretization): Starting from the continuous-time low-pass \(F(s) = 1/(1 + s \tau_f)\) and the backward Euler approximation \(s \approx (z - 1)/(T_s z)\), derive the discrete-time filter \(F(z)\) and the corresponding first-order difference equation.

Solution:

Substitute \(s \approx (z - 1)/(T_s z)\) into the denominator:

\[ F(z) \approx \frac{1}{1 + \tau_f \frac{z - 1}{T_s z}} = \frac{T_s z}{T_s z + \tau_f (z - 1)}. \]

Express numerator and denominator in terms of \(z^{-1}\). Multiplying numerator and denominator by \(z^{-1}\) gives

\[ F(z) = \frac{T_s}{T_s + \tau_f - \tau_f z^{-1}}. \]

Divide numerator and denominator by \(T_s + \tau_f\):

\[ F(z) = \frac{T_s/(T_s + \tau_f)} {1 - \frac{\tau_f}{T_s + \tau_f} z^{-1}} = \frac{b_0}{1 + a_1 z^{-1}}, \]

where \(b_0 = T_s/(T_s + \tau_f)\) and \(a_1 = -\tau_f/(T_s + \tau_f)\). Therefore, the difference equation is

\[ y[k] = -a_1 y[k-1] + b_0 u[k]. \]

Problem 5 (Choosing Filter Cutoff for a Robot Joint): A robot joint velocity controller runs at sampling frequency \(f_s = 500\) Hz (\(T_s = 0.002\) s). The loop crossover frequency is approximately \(\omega_c = 2\pi \cdot 20\) rad/s and the existing phase margin is \(\mathrm{PM}_0 = 60^\circ\). You wish to add a first-order low-pass filter in the velocity measurement path but insist on a minimum phase margin of \(\mathrm{PM}_\text{min} = 40^\circ\). Compute an upper bound on \tau_f.

Solution:

Use the inequality from Problem 2:

\[ \tau_f \le \frac{1}{\omega_c} \tan\bigl(\mathrm{PM}_0 - \mathrm{PM}_\text{min}\bigr) = \frac{1}{2\pi \cdot 20} \tan(20^\circ). \]

Numerically, \(2\pi \cdot 20 \approx 125.66\) rad/s and \(\tan(20^\circ) \approx 0.364\). Thus

\[ \tau_f \lesssim \frac{0.364}{125.66} \approx 2.9 \times 10^{-3} \text{ s}. \]

Therefore, the filter time constant should not exceed approximately \(3\) ms to preserve at least \(40^\circ\) of phase margin. A practical design might choose \tau_f around \(1\) to \(2\) ms and verify the resulting margins using Bode or Nyquist plots.

This lesson examined practical issues in using linear filters within control systems, particularly in robotic applications. We related filter order to high-frequency magnitude slope, derived how first- and second-order filters contribute phase lag and group delay, and showed how such lag erodes phase margin. Design inequalities were obtained to limit measurement-filter time constants based on existing stability margins.

We then discussed continuous-time vs discrete-time realization, highlighting backward Euler discretization and the resulting difference equations, as well as numerical implementation concerns such as filter structure, finite precision, and execution time. Finally, we provided multi-language examples (Python, C++, Java, MATLAB/Simulink, and Mathematica) illustrating how to implement simple filters in practical robotic control loops.