By now you have designed a controller for an \(n\)-DOF robot using tools from the whole course: model-based nonlinear control, task-space control, observers, MPC, robust and adaptive augmentation, and safety layers. The theoretical design typically assumes continuous time:

\[ \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t)) + \mathbf{g}(\mathbf{x}(t))\mathbf{u}(t) + \mathbf{d}(t), \quad \mathbf{u}(t) = \pi\big(\mathbf{x}(t), \mathbf{r}(t)\big) \]

Here \( \mathbf{x} \in \mathbb{R}^{2n} \) typically stacks joints and velocities, \( \mathbf{u} \in \mathbb{R}^{n} \) are actuator torques or forces, and \( \mathbf{r}(t) \) is the reference (joint or task space). In software, however, your controller runs in discrete time with sampling period \( T_s \):

\[ \mathbf{x}_{k+1} = F_d(\mathbf{x}_k, \mathbf{u}_k, \mathbf{w}_k), \quad \mathbf{u}_k = \pi_d\big(\hat{\mathbf{x}}_k, \mathbf{r}_k\big), \quad k = 0,1,2,\dots \]

where \( \hat{\mathbf{x}}_k \) is the estimated state from your observer (Chapter 11), and \( \mathbf{w}_k \) lumps discretization error, model mismatch, and disturbances. Implementation is about realizing \( \pi_d \) and the loop around it faithfully; debugging is about detecting when the real loop does not match the designed dynamics and systematically locating the cause.

A key idea throughout this lesson is to view every observed bug as a deviation of the implemented closed-loop map \( \mathbf{x}_{k+1} = \Phi_{\mathrm{impl}}(\mathbf{x}_k) \) from the designed closed-loop map \( \mathbf{x}_{k+1} = \Phi_{\mathrm{des}}(\mathbf{x}_k) \).

Consider the (already derived) manipulator dynamics in joint coordinates \( \mathbf{q} \in \mathbb{R}^n \):

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{u} + \mathbf{d}(t) \]

With state \( \mathbf{x} = \begin{bmatrix} \mathbf{q} \\[2pt] \dot{\mathbf{q}} \end{bmatrix} \), we write

\[ \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}) + \mathbf{G}(\mathbf{x})\mathbf{u} + \mathbf{w}(t) \]

Near a nominal trajectory \( (\mathbf{x}^\star(t),\mathbf{u}^\star(t)) \), linearization (Chapter 1 and Chapter 3) gives

\[ \delta\dot{\mathbf{x}}(t) = \mathbf{A}_c(t)\,\delta\mathbf{x}(t) + \mathbf{B}_c(t)\,\delta\mathbf{u}(t), \quad \mathbf{A}_c(t) = \left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right|_{\star}, \quad \mathbf{B}_c(t) = \left.\frac{\partial \big(\mathbf{G}\mathbf{u}\big)}{\partial \mathbf{u}}\right|_{\star}. \]

Under a zero-order hold (ZOH) implementation with period \(T_s\), the exact sampled dynamics of the linearization are

\[ \delta\mathbf{x}_{k+1} = \mathbf{A}_d(k)\,\delta\mathbf{x}_k + \mathbf{B}_d(k)\,\delta\mathbf{u}_k, \quad \mathbf{A}_d(k) = e^{\mathbf{A}_c(t_k)T_s}, \quad \mathbf{B}_d(k) = \int_0^{T_s} e^{\mathbf{A}_c(t_k)\sigma}\,\mathbf{B}_c(t_k)\,d\sigma. \]

In many implementations we approximate this with first-order hold:

\[ \mathbf{A}_d(k) \approx \mathbf{I} + T_s \mathbf{A}_c(t_k), \quad \mathbf{B}_d(k) \approx T_s \mathbf{B}_c(t_k). \]

If the designed continuous-time closed-loop linearization is \( \delta\dot{\mathbf{x}} = \mathbf{A}_{\mathrm{cl}}\delta\mathbf{x} \), then for a ZOH implementation we expect

\[ \mathbf{A}_{d,\mathrm{des}} = e^{\mathbf{A}_{\mathrm{cl}}T_s}, \quad \rho\big(\mathbf{A}_{d,\mathrm{des}}\big) < 1 \]

with \( \rho(\cdot) \) the spectral radius. Debugging at this level asks: does the implemented closed-loop map actually have eigenvalues close to those of \( \mathbf{A}_{d,\mathrm{des}} \)?

Let the ideal sampled closed loop around an equilibrium \( \mathbf{x}_e \) be

\[ \delta\mathbf{x}_{k+1} = \mathbf{A}_{d,\mathrm{des}}\,\delta\mathbf{x}_k + \mathbf{w}_k, \quad \rho\big(\mathbf{A}_{d,\mathrm{des}}\big) < 1, \]

where \( \mathbf{w}_k \) is small (discretization, disturbance). The implemented loop, however, may correspond to

\[ \delta\mathbf{x}_{k+1} = \mathbf{A}_{d,\mathrm{impl}}\,\delta\mathbf{x}_k + \tilde{\mathbf{w}}_k, \quad \mathbf{A}_{d,\mathrm{impl}} = \mathbf{A}_{d,\mathrm{des}} + \Delta\mathbf{A}_d. \]

A common source of \( \Delta\mathbf{A}_d \) is an implementation error in the control law. For example, suppose the designed state-feedback in joint space is \( \mathbf{u}_k = -\mathbf{K}\,\mathbf{e}_k \) with \( \mathbf{e}_k = \mathbf{q}_k - \mathbf{q}^{\mathrm{ref}}_k \), but in code a sign mistake yields \( \mathbf{u}_k = +\mathbf{K}\,\mathbf{e}_k \). Then \( \mathbf{A}_{d,\mathrm{impl}} = \mathbf{A}_d + \mathbf{B}_d\mathbf{K} \) instead of \( \mathbf{A}_d - \mathbf{B}_d\mathbf{K} \), pushing eigenvalues outside the unit disc.

A powerful debugging strategy is to monitor a discrete-time Lyapunov function for the designed closed loop. Suppose you have computed a positive definite \( \mathbf{P} = \mathbf{P}^\top \succ 0 \) solving the discrete Lyapunov inequality

\[ \mathbf{A}_{d,\mathrm{des}}^{\top}\mathbf{P}\mathbf{A}_{d,\mathrm{des}} - \mathbf{P} \preceq -\mathbf{Q} \]

for some \( \mathbf{Q} \succ 0 \). Then the quantity

\[ V_k = \delta\mathbf{x}_k^{\top}\mathbf{P}\,\delta\mathbf{x}_k \]

should, in the absence of disturbances, satisfy

\[ V_{k+1} - V_k \le -\alpha \|\delta\mathbf{x}_k\|^2 \]

for some \( \alpha > 0 \). A logged trajectory in which \(V_k\) increases over many samples is a strong indication that your implemented loop does not realize the intended \( \mathbf{A}_{d,\mathrm{des}} \), pointing you toward gain, sign, or timing mistakes.

Additional invariants and checks:

• Joint limits: check that joint coordinates always satisfy \( \mathbf{q}_{\min} \le \mathbf{q}_k \le \mathbf{q}_{\max} \). A single violation suggests missing saturation or wrong units.
• Energy dissipation for damped controllers: for impedance control or PD control with sufficient damping, the mechanical energy plus potential should not grow unbounded without external work.
• Safety constraints (Chapter 13): if you use control barrier functions, verify numerically that the barrier function \( h(\mathbf{x}_k) \) stays nonnegative whenever the design guarantees forward invariance of the safe set.

The following diagram summarizes a typical implementation cycle for your capstone project, combining real-time execution with offline analysis.

flowchart TD
  A["Choose controller structure (from previous lesson)"]
    --> B["Fix sampling period Ts and priorities (real-time)"]
  B --> C["Implement sensing: read q, dq, contact, etc."]
  C --> D["Estimate state x_hat_k using observer"]
  D --> E["Compute control u_k = pi_d(x_hat_k, r_k)"]
  E --> F["Apply safety layer: saturations, CBF-QP, limits"]
  F --> G["Send command to actuators or simulator"]
  G --> H["Log data: t, x_hat_k, r_k, u_k, diagnostics"]
  H --> I["Offline: check invariants, eigenvalues, frequency content"]
  I --> J{"Performance ok?"}
  J -->|yes| K["Freeze design or move to next task"]
  J -->|no| L["Locate mismatch: model, gains, timing, code bug"]
  L --> B
        

A crucial design choice is the logging format. For debugging you should at least log time stamps, reference, estimated state, raw sensor values, control inputs, and key internal variables (e.g., constraint residuals in an MPC or QP controller).

Python is convenient for prototyping and offline debugging. For robotics, typical libraries include numpy and scipy for linear algebra, roboticstoolbox (Corke) or pin (Pinocchio bindings) for dynamics, and middleware such as ROS 2 (rclpy) for real robots. Below is a simplified joint-space PD implementation for a single revolute joint modeled as a damped double integrator, using a discrete-time approximation:

\[ \dot{e}(t) = v(t), \quad \dot{v}(t) = -\frac{k_p}{J} e(t) - \frac{k_d + b}{J} v(t) \]

With forward Euler discretization, the implemented error dynamics are

\[ \begin{bmatrix} e_{k+1} \\ v_{k+1} \end{bmatrix} = \underbrace{\begin{bmatrix} 1 & T_s \\ -\frac{T_s k_p}{J} & 1 - \frac{T_s(k_d + b)}{J} \end{bmatrix}}_{\mathbf{A}_{d,\mathrm{impl}}} \begin{bmatrix} e_k \\ v_k \end{bmatrix}. \]

Python skeleton:

import numpy as np

# Physical and control parameters
J = 0.05       # inertia
b = 0.01       # viscous damping
k_p = 50.0
k_d = 2.0

Ts = 0.001     # sampling period (s)
T_final = 5.0
N = int(T_final / Ts)

# Discrete-time matrices (forward Euler approximation)
A_d = np.array([
    [1.0, Ts],
    [-Ts * k_p / J, 1.0 - Ts * (k_d + b) / J]
])
B_d = np.zeros((2, 1))  # no explicit input when expressed in error coordinates

# Reference trajectory (constant)
q_ref = 1.0  # rad
e = np.array([0.0, 0.0])  # [position_error, velocity_error]

# Logging buffers
log_t = np.zeros(N)
log_e = np.zeros((N, 2))
log_u = np.zeros(N)

# Torque saturation
u_min, u_max = -5.0, 5.0

for k in range(N):
    t_k = k * Ts

    # Here q_meas, dq_meas would come from sensors (or a simulator)
    # For this simplified example, we reconstruct from error state:
    q_meas = q_ref - e[0]
    dq_meas = -e[1]

    # PD control law in joint coordinates
    u = -k_p * (q_meas - q_ref) - k_d * dq_meas

    # Saturation and logging
    u = float(np.clip(u, u_min, u_max))

    log_t[k] = t_k
    log_e[k, :] = e
    log_u[k] = u

    # Update error state using the derived matrix A_d
    e = A_d @ e

# Post-run: approximate discrete-time eigenvalues and check stability
eigvals = np.linalg.eigvals(A_d)
spectral_radius = max(abs(eigvals))

print("A_d:")
print(A_d)
print("Eigenvalues:", eigvals)
print("Spectral radius:", spectral_radius)

# Example invariant: energy-like quantity V_k = e^T P e
P = np.eye(2)
V = np.einsum("bi,ij,bj->b", log_e, P, log_e)  # V_k sequence
      

In a full robot implementation, you replace the simple error dynamics with calls to the actual dynamics (via a physics simulator or a robot driver), while keeping the logging and eigenvalue checks essentially unchanged. Using matplotlib you can inspect \(e_k\), \(u_k\), and \(V_k\) to see whether the behavior matches your theoretical expectations.

C++ is the typical choice for hard real-time or high-performance control. A common stack combines Eigen for linear algebra with ROS 2 (rclcpp) or a custom real-time framework. Below is a minimal pattern capturing the core control loop and logging; integration with ROS 2 publishers and subscribers would wrap the calls to readState() and sendTorque().

#include <iostream>
#include <fstream>
#include <chrono>
#include <thread>
#include <Eigen/Dense>

using Eigen::Matrix2d;
using Eigen::Vector2d;

struct State {
  double q;
  double dq;
};

State readState();           // to be implemented for robot or simulator
void sendTorque(double u);   // to be implemented

int main() {
  const double J = 0.05;
  const double b = 0.01;
  const double k_p = 50.0;
  const double k_d = 2.0;

  const double Ts = 0.001;
  const double T_final = 5.0;
  const int N = static_cast<int>(T_final / Ts);

  // Discrete-time error dynamics matrix A_d (forward Euler)
  Matrix2d A_d;
  A_d << 1.0, Ts,
          -Ts * k_p / J, 1.0 - Ts * (k_d + b) / J;

  Vector2d e;  // [position_error, velocity_error]
  e.setZero();
  const double q_ref = 1.0;

  std::ofstream log("log_pd_debug.txt");

  for (int k = 0; k < N; ++k) {
    auto t_start = std::chrono::steady_clock::now();
    double t_k = k * Ts;

    State s = readState();  // q, dq from sensors or simulator
    e(0) = s.q - q_ref;
    e(1) = s.dq;

    double u = -k_p * e(0) - k_d * e(1);

    // Simple saturation
    const double u_min = -5.0;
    const double u_max =  5.0;
    if (u < u_min) u = u_min;
    if (u > u_max) u = u_max;

    sendTorque(u);

    // Logging: time, q, dq, e0, e1, u
    if (log.is_open()) {
      log << t_k << " "
          << s.q << " " << s.dq << " "
          << e(0) << " " << e(1) << " "
          << u << "\n";
    }

    // Next expected sampling time
    auto t_end = t_start + std::chrono::duration<double>(Ts);
    std::this_thread::sleep_until(t_end);
  }

  log.close();

  // Offline: eigenvalue check
  Eigen::EigenSolver<Matrix2d> es(A_d);
  std::cout << "A_d:\n" << A_d << std::endl;
  std::cout << "Eigenvalues:\n" << es.eigenvalues() << std::endl;

  return 0;
}
      

Timing jitter can be diagnosed by logging the actual loop period \( \Delta t_k \) from consecutive timestamps; large deviations from \(T_s\) often explain discrepancies between simulation and hardware.

Java is less common in low-level robot control but appears in higher-level control and industrial environments. The EJML library (org.ejml) provides efficient matrix operations. Below is a conceptual pattern using ScheduledExecutorService to approximate a periodic control loop:

import java.util.concurrent.*;
import java.util.logging.*;
import org.ejml.simple.SimpleMatrix;

public class JointPDController {

    private static final Logger logger = Logger.getLogger("JointPD");

    private final double Ts = 0.001;
    private final double J = 0.05;
    private final double b = 0.01;
    private final double k_p = 50.0;
    private final double k_d = 2.0;
    private final double q_ref = 1.0;

    private final SimpleMatrix A_d;

    public JointPDController() {
        // 2x2 matrix
        double[][] Adata = {
            {1.0, Ts},
            {-Ts * k_p / J, 1.0 - Ts * (k_d + b) / J}
        };
        A_d = new SimpleMatrix(Adata);
    }

    private static class State {
        public double q;
        public double dq;
    }

    private State readState() {
        // TODO: connect to robot or simulator
        State s = new State();
        s.q = 0.0;
        s.dq = 0.0;
        return s;
    }

    private void sendTorque(double u) {
        // TODO: send to hardware or simulator
    }

    public void run(double Tfinal) {
        int N = (int) (Tfinal / Ts);
        ScheduledExecutorService exec = Executors.newSingleThreadScheduledExecutor();

        final double[] e = new double[]{0.0, 0.0}; // position, velocity errors

        Runnable task = new Runnable() {
            int k = 0;
            @Override
            public void run() {
                if (k >= N) {
                    exec.shutdown();
                    return;
                }
                double t_k = k * Ts;
                State s = readState();
                e[0] = s.q - q_ref;
                e[1] = s.dq;

                double u = -k_p * e[0] - k_d * e[1];
                double u_min = -5.0, u_max = 5.0;
                if (u < u_min) u = u_min;
                if (u > u_max) u = u_max;

                sendTorque(u);

                logger.info(String.format(
                    "t=%.4f q=%.3f dq=%.3f e=(%.3f,%.3f) u=%.3f",
                    t_k, s.q, s.dq, e[0], e[1], u
                ));

                k++;
            }
        };

        long periodNs = (long) (Ts * 1e9);
        exec.scheduleAtFixedRate(task, 0, periodNs, TimeUnit.NANOSECONDS);
    }

    public static void main(String[] args) {
        JointPDController ctrl = new JointPDController();
        ctrl.run(5.0);
    }
}
      

While the Java scheduler is not hard real-time, this pattern suffices to debug the logic of the controller and to verify that the observed time-discrete dynamics match the theoretical predictions before porting to a stricter environment.

MATLAB and Simulink are standard tools for rapid prototyping and validation of closed-loop robotic controllers. A common workflow is:

1. Derive a continuous-time closed-loop model in symbolic or numeric form.
2. Discretize it using c2d with a chosen method and period \(T_s\).
3. Simulate reference tracking using lsim or Simulink.
4. Compare logged hardware data to simulated trajectories.

Example: discretizing a linearized closed loop and checking eigenvalues in MATLAB.

% Continuous-time closed-loop system: x_dot = A_cl * x
J = 0.05;
b = 0.01;
k_p = 50;
k_d = 2;

A_cl = [ 0,       1;
        -k_p/J, -(k_d + b)/J ];

Ts = 1e-3;

% Discretization with zero-order hold
sys_c = ss(A_cl, [], eye(2), []);
sys_d = c2d(sys_c, Ts, 'zoh');

A_d = sys_d.A;
eigAd = eig(A_d);

disp('A_d = ');
disp(A_d);
disp('Eigenvalues(A_d) = ');
disp(eigAd);

% Simulated response for an initial error
x0 = [0.1; 0.0];
Tfinal = 5.0;
N = round(Tfinal / Ts);
t_vec = (0:N-1)' * Ts;

% Since no explicit input in error coordinates, we use zero input
u_seq = zeros(N, 1);
[y_sim, ~, x_sim] = lsim(sys_d, u_seq, t_vec, x0);

% Energy-like quantity V_k = x_k' P x_k with P = I
V = sum(x_sim.^2, 2);

figure;
subplot(3,1,1); plot(t_vec, x_sim(:,1)); ylabel('e_q');
subplot(3,1,2); plot(t_vec, x_sim(:,2)); ylabel('e_dq');
subplot(3,1,3); plot(t_vec, V); ylabel('V'); xlabel('t (s)');
      

In Simulink, you typically build a model where your controller is a subsystem driven by blocks representing sensors, kinematics, and dynamics. Enabling the Data Inspector and logging joint positions, velocities, and control effort gives you a direct comparison against hardware logs.

Wolfram Mathematica is well suited for symbolic derivations and high-precision discrete-time simulations. The snippet below implements the same scalar PD-controlled joint closed-loop dynamics and plots the error decay:

J = 0.05;
b = 0.01;
kp = 50.;
kd = 2.;
Ts = 0.001;

A = { {1., Ts}, {-Ts*kp/J, 1. - Ts*(kd + b)/J} };

(* initial error state: e_q, e_dq *)
x0 = {0.1, 0.0};
nSteps = Round[5./Ts];

x[0] = x0;
x[k_Integer?Positive] := x[k] = A.x[k - 1];

traj = Table[x[k], {k, 0, nSteps}];
tvec = Range[0, nSteps]*Ts;

eQ = traj[[All, 1]];
eDq = traj[[All, 2]];
V = eQ^2 + eDq^2;

ListLinePlot[
  {
    Transpose[{tvec, eQ}],
    Transpose[{tvec, eDq}],
    Transpose[{tvec, V}]
  },
  PlotLegends -> {"e_q", "e_dq", "V"},
  AxesLabel -> {"t (s)", "value"}
]
      

By symbolically computing \(e_k\) as a function of \(k\), \(k_p\), \(k_d\), and \(T_s\), you can analytically verify stability conditions and compare them to the numerical Jury or Schur criteria used in discrete-time control theory.

Problem 1 (Discrete-Time Stability Condition for PD-Controlled Joint): Consider the scalar joint model and PD control as in this lesson, with dynamics in error coordinates

\[ \dot{e}(t) = v(t), \quad \dot{v}(t) = -\frac{k_p}{J} e(t) - \frac{k_d + b}{J} v(t). \]

Using the forward Euler discretization with period \(T_s\), derive the discrete-time matrix \( \mathbf{A}_d \) and use the Schur stability conditions for a \(2\times 2\) matrix to obtain inequalities on \(k_p\), \(k_d\), and \(T_s\) guaranteeing that both eigenvalues lie inside the unit disc.

Solution:

The continuous-time system can be written as \( \dot{\mathbf{x}} = \mathbf{A}_c \mathbf{x} \) with

\[ \mathbf{x} = \begin{bmatrix} e \\ v \end{bmatrix},\quad \mathbf{A}_c = \begin{bmatrix} 0 & 1 \\ -\dfrac{k_p}{J} & -\dfrac{k_d + b}{J} \end{bmatrix}. \]

Forward Euler discretization yields \( \mathbf{A}_d = \mathbf{I} + T_s \mathbf{A}_c \), that is

\[ \mathbf{A}_d = \begin{bmatrix} 1 & T_s \\ -\dfrac{T_s k_p}{J} & 1 - \dfrac{T_s (k_d + b)}{J} \end{bmatrix}. \]

For a \(2\times 2\) matrix, Schur stability is equivalent to the conditions

\[ 1 + \det(\mathbf{A}_d) + \operatorname{tr}(\mathbf{A}_d) > 0,\quad 1 + \det(\mathbf{A}_d) - \operatorname{tr}(\mathbf{A}_d) > 0,\quad 1 - \det(\mathbf{A}_d) > 0. \]

Here

\[ \operatorname{tr}(\mathbf{A}_d) = 2 - \frac{T_s (k_d + b)}{J},\quad \det(\mathbf{A}_d) = 1 - \frac{T_s (k_d + b)}{J} + \frac{T_s^2 k_p}{J}. \]

Substituting and simplifying, you obtain inequalities of the form \( 0 < k_p < k_p^{\max}(T_s,J,b,k_d) \) and \( 0 < k_d + b < k_d^{\max}(T_s,J,k_p) \). In particular, for fixed \(k_p\) and \(k_d\), there exists a maximum admissible sampling period \(T_s^{\max}\) such that \(T_s < T_s^{\max}\) is required for Schur stability. In practice you choose \(T_s\) much smaller than this limit to reduce discretization error.

Problem 2 (Effect of Timing Jitter): Suppose that your implementation uses a nominal sampling period \(T_s\), but the actual inter-sample intervals satisfy \(T_s + \Delta T_k\), where \( |\Delta T_k| \le \Delta T_{\max}\). Using the first-order approximation \( \mathbf{A}_d(k) \approx \mathbf{I} + (T_s + \Delta T_k)\mathbf{A}_c \), show that the implemented closed-loop matrix can be written as

\[ \mathbf{A}_{d,\mathrm{impl}}(k) = \mathbf{A}_{d,\mathrm{des}} + \Delta T_k\,\mathbf{A}_c, \quad \mathbf{A}_{d,\mathrm{des}} = \mathbf{I} + T_s \mathbf{A}_c. \]

Explain how a bound on \( \|\mathbf{A}_c\| \) and \( \Delta T_{\max} \) can be used to guarantee that eigenvalues remain inside the unit disc.

Solution:

With jitter, the discrete-time matrix at step \(k\) becomes \( \mathbf{A}_d(k) = \mathbf{I} + (T_s + \Delta T_k)\mathbf{A}_c = \mathbf{I} + T_s\mathbf{A}_c + \Delta T_k\mathbf{A}_c \). The first two terms coincide with the designed matrix \( \mathbf{A}_{d,\mathrm{des}} \), hence

\[ \mathbf{A}_{d,\mathrm{impl}}(k) = \mathbf{A}_{d,\mathrm{des}} + \Delta T_k\,\mathbf{A}_c. \]

Using standard matrix perturbation results, if \( \rho(\mathbf{A}_{d,\mathrm{des}}) \le 1 - \varepsilon \) for some \( \varepsilon > 0 \), and if \( \|\Delta T_k \mathbf{A}_c\| \le \delta < \varepsilon \) for all \(k\), then the eigenvalues of \( \mathbf{A}_{d,\mathrm{impl}}(k) \) remain inside the unit disc. A sufficient condition is

\[ |\Delta T_k| \le \Delta T_{\max},\quad \Delta T_{\max} \|\mathbf{A}_c\| < \varepsilon. \]

Thus, once you have an estimate of \( \|\mathbf{A}_c\| \) from your model and of \( \varepsilon \) from the continuous-time design margin, you can derive an upper bound on admissible timing jitter. Measured loop times in the log can then be checked against this bound.

Problem 3 (Logging Frequency and Aliasing): Your inner control loop runs at \(T_s = 1\) ms (1 kHz), but for logging you decide to record only every \(M\)-th sample. Suppose you want to reliably detect oscillations up to 10 Hz in the joint error using the logged data. What is the maximum decimation factor \(M_{\max}\) you can choose without violating the Nyquist condition? What happens to higher-frequency oscillations?

Solution:

The logging period becomes \( T_{\mathrm{log}} = M T_s \), so the logging frequency is \( f_{\mathrm{log}} = 1 / T_{\mathrm{log}} = \frac{1}{M T_s} \). The Nyquist condition requires

\[ f_{\mathrm{log}} \ge 2 f_{\max}, \]

where \( f_{\max} = 10 \) Hz is the maximum frequency to be observed. Thus

\[ \frac{1}{M T_s} \ge 20 \quad\Rightarrow\quad M \le \frac{1}{20 T_s} = \frac{1}{20 \cdot 10^{-3}} = 50. \]

Hence \( M_{\max} = 50 \). If you choose a larger decimation, higher-frequency oscillations will be aliased: they will appear at lower frequencies in the logged data, potentially misleading you about the true dynamics. Therefore, for debugging and performance assessment it is often worth logging at a much higher effective rate than the bandwidth of interest.

Problem 4 (Lyapunov-Based Bug Detection): Suppose you know that for your linearized discrete-time closed loop, there exists \( \mathbf{P} \succ 0 \) and \( \mathbf{Q} \succ 0 \) such that

\[ \mathbf{A}_{d,\mathrm{des}}^{\top}\mathbf{P}\mathbf{A}_{d,\mathrm{des}} - \mathbf{P} = -\mathbf{Q}. \]

You log a trajectory \( \{\mathbf{x}_k\} \) and compute \( V_k = \mathbf{x}_k^{\top}\mathbf{P}\mathbf{x}_k \). Show that for the ideal closed-loop dynamics (with no disturbances) you must have \( V_{k+1} - V_k = -\mathbf{x}_k^{\top}\mathbf{Q}\mathbf{x}_k \le 0 \). Explain why persistent positive increments of \( V_k \) in the log indicate a bug or unmodeled destabilizing effect.

Solution:

Under the ideal dynamics \( \mathbf{x}_{k+1} = \mathbf{A}_{d,\mathrm{des}}\mathbf{x}_k \), we obtain

\[ V_{k+1} = \mathbf{x}_{k+1}^{\top}\mathbf{P}\mathbf{x}_{k+1} = \mathbf{x}_k^{\top}\mathbf{A}_{d,\mathrm{des}}^{\top} \mathbf{P}\mathbf{A}_{d,\mathrm{des}} \mathbf{x}_k. \]

Using the Lyapunov equation, \( \mathbf{A}_{d,\mathrm{des}}^{\top}\mathbf{P}\mathbf{A}_{d,\mathrm{des}} = \mathbf{P} - \mathbf{Q} \), hence

\[ V_{k+1} = \mathbf{x}_k^{\top}(\mathbf{P} - \mathbf{Q})\mathbf{x}_k = V_k - \mathbf{x}_k^{\top}\mathbf{Q}\mathbf{x}_k. \]

Therefore

\[ V_{k+1} - V_k = -\mathbf{x}_k^{\top}\mathbf{Q}\mathbf{x}_k \le 0, \]

with equality only at \( \mathbf{x}_k = \mathbf{0} \). If your logged \( V_k \) occasionally increases by numerical noise but shows a persistent trend of increase or large sporadic spikes, this contradicts the derived monotone decay and indicates either a mismatch between the implemented and designed matrices or the presence of disturbances or modeling errors that violate the assumptions under which the Lyapunov function was derived.

In this lesson we connected your continuous-time controller design to its discrete-time implementation, emphasizing the role of sampling, timing, and numerical approximation. We introduced the notion of a designed closed-loop map \( \mathbf{A}_{d,\mathrm{des}} \) and contrasted it with the implemented map \( \mathbf{A}_{d,\mathrm{impl}} \), showing how bugs manifest as perturbations in this matrix. We developed Lyapunov- and eigenvalue-based invariants for debugging, and presented concrete implementation patterns in Python, C++, Java, MATLAB/Simulink, and Mathematica, always with logging and diagnostic hooks.

These tools prepare you for the next lesson, where you will perform a systematic stability and robustness evaluation of your capstone controller, comparing theoretical guarantees to empirical performance on the final system.