Unlike pure software, a robot couples digital logic with continuous dynamics and noisy sensors. A bug can manifest as:

• Incorrect logic in the control or autonomy code.
• Timing violations (missed deadlines, jitter) in the control loop.
• Misconfigured sensors (scale, sign, frame) or actuators.
• Mechanical or electrical issues (loose cables, friction, saturation).

We model the closed-loop robot at a high level as a discrete-time linear system (students are assumed to know linear control):

\[ x_{k+1} = A x_k + B u_k + w_k,\quad y_k = C x_k + v_k \]

where \( x_k \) is the state, \( u_k \) the control input, \( y_k \) the measured output, and \( w_k, v_k \) represent process and measurement noise. The controller computes \( u_k = K(y_0,\dots,y_k,r_0,\dots,r_k) \) from past data and a reference signal \( r_k \).

A central debugging quantity is the tracking error \( e_k = r_k - y_k \). A robust debugging workflow will repeatedly ask: “Is the observed \( e_k \) compatible with our model and design, or does it indicate a fault?”

In model-based debugging, we compare measured behaviour to what a nominal model predicts. Let \( \hat{x}_k \) be a state estimate evolving under the ideal model (no fault), and \( \hat{y}_k = C \hat{x}_k \) the predicted measurement. Define the residual

\[ r_k = y_k - \hat{y}_k . \]

Under normal operation (no fault) and for a well-tuned observer, \( r_k \) is mainly noise. For simplicity, assume the scalar case and that \( r_k \sim \mathcal{N}(0,\sigma_r^2) \) under the null hypothesis “no fault”.

We trigger an alarm when the residual magnitude exceeds a threshold:

\[ H_0: \text{no fault},\quad H_1: \text{fault},\quad \text{alarm if } |r_k| > \gamma . \]

The false alarm probability for a given threshold \( \gamma \) is

\[ \mathbb{P}(|r_k| > \gamma \mid H_0) = 2\left(1 - \Phi\!\left(\frac{\gamma}{\sigma_r}\right)\right), \]

where \( \Phi \) is the standard normal CDF. If we require a false alarm rate \( \alpha \) (for example \( \alpha = 0.01 \)), we solve

\[ 2\left(1 - \Phi\!\left(\frac{\gamma}{\sigma_r}\right)\right) = \alpha \quad\Rightarrow\quad \gamma = \sigma_r\, \Phi^{-1}\!\left(1 - \frac{\alpha}{2}\right). \]

This gives a principled way to set thresholds in monitoring code and plots: choose \( \gamma \) according to the noise level \( \sigma_r \) and the tolerable false alarm probability \( \alpha \).

In practice, you estimate \( \sigma_r \) from a short calibration run in safe conditions, then fix \( \gamma \) and use it consistently in your debugging visualizations and assertions.

Integrated robots are multi-layer systems. A robust workflow proceeds by narrowing down layers systematically instead of guessing. One possible layering is:

• Layer 0: Power and wiring.
• Layer 1: Sensors and actuators (raw signals).
• Layer 2: Drivers and middleware (e.g., ROS nodes, topics).
• Layer 3: Control loops.
• Layer 4: Autonomy / decision logic.

For each bug, follow a repeatable flow:

flowchart TD
  S["Symptom observed (e.g. robot drifts right)"]
    --> C["Characterize: repeatable? conditions? logs?"]
  C --> R["Reproduce in controlled scenario (low speed, simple command)"]
  R --> L["Localize layer: check power, then signals, then software"]
  L --> M["Measure key signals: ref r_k, output y_k, error e_k"]
  M --> CMP["Compare with model prediction y_hat_k"]
  CMP -->|compatible| SW["Focus on higher level software logic"]
  CMP -->|incompatible| HW["Focus on hardware, calibration, low level"]
  SW --> FIX["Form hypothesis, change single thing, test again"]
  HW --> FIX
  FIX --> LOOP["Log results, update checklist for future runs"]
        

The mathematical model from Section 2 enters in the step “compare with model prediction”: if the measured \( y_k \) deviates from \( \hat{y}_k \) beyond the allowed threshold \( \gamma \), then the bug is likely below the controller (sensor, actuator, or plant model mismatch).

A powerful debugging technique is to encode known properties of correct behaviour as invariants. For a discrete-time closed-loop system

\[ x_{k+1} = A_{\text{cl}} x_k, \]

consider the safe region

\[ \mathcal{S} = \{ x \in \mathbb{R}^n : \|x\|_2 \leq R_{\max} \}. \]

Suppose we have designed a stabilizing controller so that the induced 2-norm of the closed-loop matrix satisfies \( \|A_{\text{cl}}\|_2 \leq \rho \) with \( \rho < 1 \). Then

\[ \|x_{k+1}\|_2 = \|A_{\text{cl}} x_k\|_2 \leq \|A_{\text{cl}}\|_2 \, \|x_k\|_2 \leq \rho \, \|x_k\|_2 . \]

By induction, if \( \|x_0\|_2 \leq R_{\max} \), we have

\[ \|x_k\|_2 \leq \rho^k R_{\max} \leq R_{\max} \quad \forall k \geq 0, \]

so the set \( \mathcal{S} \) is forward invariant. In practice this means:

• Joint angles should remain within known safe bounds.
• Velocities should not grow unbounded during normal operation.

We can implement run-time monitors that check if measured states violate known invariants; any violation is a strong indicator of a bug or fault. For example, if joint speed \( \dot{q}(t) \) is known to satisfy \( |\dot{q}(t)| \leq \omega_{\max} \) under all scheduled commands, we can set a software assertion at \( \omega_{\max} \).

These invariants come directly from control design specifications and provide mathematically justified thresholds for debugging, rather than arbitrary “looks wrong” judgments.

Debugging a complete stack requires tools that expose the signals in the mathematical model: \( r_k, y_k, e_k \), loop period, and key internal states. Below we sketch simple patterns in Python, C++, Java, and MATLAB/Simulink, using robotics-relevant libraries where possible.

5.1 Python (ROS 2 with rclpy)

A minimal ROS 2 node that measures loop timing and error magnitude \( |e_k| \) and logs an alarm if it crosses a threshold \( \gamma \):

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32

class DebugController(Node):
    def __init__(self):
        super().__init__('debug_controller')
        self.ref = 0.0
        self.y = 0.0
        self.gamma = 0.5  # threshold for |e_k|
        self.last_time = self.get_clock().now()

        self.create_subscription(Float32, 'ref', self.ref_callback, 10)
        self.create_subscription(Float32, 'y', self.y_callback, 10)
        self.timer = self.create_timer(0.02, self.control_step)  # 50 Hz

    def ref_callback(self, msg):
        self.ref = msg.data

    def y_callback(self, msg):
        self.y = msg.data

    def control_step(self):
        now = self.get_clock().now()
        dt = (now - self.last_time).nanoseconds * 1e-9
        self.last_time = now

        if dt > 0.05:
            self.get_logger().warn(f'Loop overrun: dt={dt:.3f} s')

        e = self.ref - self.y
        if abs(e) > self.gamma:
            self.get_logger().warn(f'Tracking error too large: e={e:.3f}')

def main(args=None):
    rclpy.init(args=args)
    node = DebugController()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
      

This directly monitors the quantities used in Section 2: thresholding the residual/error and the loop period.

5.2 C++ (ROS 2 with rclcpp)

The same idea can be implemented in C++ using rclcpp:

#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

class DebugController : public rclcpp::Node {
public:
  DebugController() : Node("debug_controller"), ref_(0.0f), y_(0.0f),
                      gamma_(0.5f) {
    ref_sub_ = this->create_subscription<std_msgs::msg::Float32>(
      "ref", 10, std::bind(&DebugController::refCallback, this, _1));
    y_sub_ = this->create_subscription<std_msgs::msg::Float32>(
      "y", 10, std::bind(&DebugController::yCallback, this, _1));
    last_time_ = now();
    timer_ = this->create_wall_timer(
      20ms, std::bind(&DebugController::controlStep, this));
  }

private:
  void refCallback(const std_msgs::msg::Float32::SharedPtr msg) {
    ref_ = msg->data;
  }

  void yCallback(const std_msgs::msg::Float32::SharedPtr msg) {
    y_ = msg->data;
  }

  void controlStep() {
    auto current = now();
    double dt = (current - last_time_).seconds();
    last_time_ = current;

    if (dt > 0.05) {
      RCLCPP_WARN(this->get_logger(), "Loop overrun: dt=%.3f s", dt);
    }

    float e = ref_ - y_;
    if (std::fabs(e) > gamma_) {
      RCLCPP_WARN(this->get_logger(), "Tracking error too large: e=%.3f", e);
    }
  }

  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr ref_sub_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr y_sub_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Time last_time_;

  float ref_;
  float y_;
  float gamma_;
};

int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DebugController>());
  rclcpp::shutdown();
  return 0;
}
      

5.3 Java (Timed Loop with Logging)

In many educational robot platforms (e.g. WPILib for mobile robots), Java is used to implement periodic control loops. A minimal pattern using a logging framework might look like:

import java.util.logging.Logger;

public class DebugController {
    private static final Logger log = Logger.getLogger("DebugController");
    private double ref = 0.0;
    private double y = 0.0;
    private double gamma = 0.5;
    private long lastTimeNs = System.nanoTime();

    public void periodic() {
        long now = System.nanoTime();
        double dt = (now - lastTimeNs) * 1e-9;
        lastTimeNs = now;

        if (dt > 0.05) {
            log.warning(String.format("Loop overrun: dt=%.3f s", dt));
        }

        double e = ref - y;
        if (Math.abs(e) > gamma) {
            log.warning(String.format("Tracking error too large: e=%.3f", e));
        }
    }

    public void setReference(double r) { this.ref = r; }
    public void setMeasurement(double y) { this.y = y; }
}
      

The exact interfaces to sensors and actuators are platform-specific, but the debugging logic (monitor \( dt \) and \( |e_k| \)) is independent of the specific robot.

5.4 MATLAB/Simulink

In MATLAB, one may simulate the closed loop and add assertions to ensure that invariants derived in Section 4 hold:

Rmax = 1.0;      % safe radius for state
gamma = 0.5;     % allowed tracking error
for k = 1:length(t)
    xk = x(:,k);
    ek = r(k) - y(k);

    if norm(xk, 2) > Rmax
        warning('State left safe set: ||x||_2 = %.3f > Rmax', norm(xk, 2));
    end
    if abs(ek) > gamma
        warning('Tracking error too large at k=%d: e=%.3f', k, ek);
    end
end
      

In Simulink, the same logic can be implemented with Assertion and Detect Increase blocks, connected to scopes or data logs. This allows debugging in simulation before trying code on the physical robot.

Because robot experiments are time-consuming, it is useful to reason quantitatively about how effective your debugging workflow is. Two simple metrics are:

• Mean time to detection (MTTD): average time from bug injection or appearance to detection.
• Mean time to repair (MTTR): average time from detection to successful fix.

Model the time to detection as a discrete random variable \( T_D \in \{1,2,\dots\} \) counting control cycles between a fault and its detection. Then

\[ \mathrm{MTTD} = \mathbb{E}[T_D] = \sum_{k=1}^{\infty} k \, \mathbb{P}(T_D = k). \]

If, for example, each control cycle detects an existing bug independently with probability \( p \), then \( T_D \) is geometrically distributed and

\[ \mathbb{P}(T_D = k) = (1-p)^{k-1} p,\quad \mathrm{MTTD} = \frac{1}{p}. \]

This simple calculation shows that increasing the per-cycle detection probability (for example, by adding more informative monitors) directly reduces the expected time until a bug is detected.

Similarly, if we have \( n \) independent diagnostic tests for a bug, where test \( i \) detects the bug with probability \( p_i \), then the probability that at least one test detects the bug is

\[ P_{\text{detect}} = 1 - \prod_{i=1}^{n} (1 - p_i). \]

This encourages designs with multiple, independent checks (e.g., both model residuals and sanity checks on raw sensor ranges), as long as they do not create excessive false alarms.

In this lesson we translated the intuitive idea of “debugging a robot” into a structured, mathematically informed workflow. By defining residuals and thresholds, exploiting invariants from linear control design, and instrumenting the robot software stack to monitor key signals, we obtain a repeatable process that localizes faults efficiently and safely. Quantitative metrics such as false alarm probability and mean time to detection help evaluate and refine debugging strategies, while multi-language examples show how to put these ideas into practice in a modern robotics stack.