{ "cells": [ { "cell_type": "markdown", "id": "cell-0", "metadata": {}, "source": "[\u2190 Algorithms as Dynamical Systems](../../getting_started/theory_to_python/algorithms_as_dynamical_systems.rst)" }, { "cell_type": "markdown", "id": "fcd1288a", "metadata": {}, "source": "# Example: PID Controller\n\nThe PID (Proportional-Integral-Derivative) controller is one of the most widely used feedback control algorithms in industrial and robotics applications. This controller adjusts a control input based on three terms to achieve desired system behavior: proportional response to current error, integral action to eliminate steady-state error, and derivative action for damping.\n\nBy alternating between error computation, integral accumulation, and derivative estimation, the PID controller produces a control signal that drives the plant output toward the desired setpoint. The controller's performance depends on careful gain tuning to balance responsiveness, steady-state accuracy, and stability.\n\nFor further information, read the [Wikipedia article](https://en.wikipedia.org/wiki/PID_controller). For the classic paper on PID tuning, see [Ziegler and Nichols (1942)](https://doi.org/10.1115/1.3662552).\n\nThe PID controller dynamical system as implemented in this notebook may be found in the API reference under \"Controllers\": [PID Controller](../../api/algorithm_library_controllers.rst)." }, { "cell_type": "markdown", "id": "cf99433f", "metadata": {}, "source": "## Definition of Algorithm\n\n### Notation and Assumptions\n\nWe consider a discrete-time control problem where we wish to regulate a plant output $y_k$ to track a reference signal $r_k$. The PID controller computes a control input $u_k$ based on the tracking error\n$$\ne_k = r_k - y_k,\n$$\nwhere:\n- $r_k \\in \\mathbb{R}$ is the reference (setpoint) at time step $k$,\n- $y_k \\in \\mathbb{R}$ is the measured plant output,\n- $e_k \\in \\mathbb{R}$ is the tracking error.\n\nThe discrete-time PID control law is given by\n$$\nu_k = K_P e_k + K_I I_k + K_D (e_k - e_{k-1}),\n$$\nwhere:\n- $K_P$ is the proportional gain,\n- $K_I$ is the integral gain,\n- $K_D$ is the derivative gain,\n- $I_k$ is the accumulated integral of error,\n- $e_{k-1}$ is the previous error.\n\nThe integral term evolves according to\n$$\nI_{k+1} = I_k + e_k.\n$$\n\n### How it works\n\nThe PID controller combines three control actions to achieve desired system performance:\n\n**Proportional Term**\n\nThe proportional term $K_P e_k$ provides immediate corrective action proportional to the current error. A larger proportional gain increases responsiveness and reduces rise time, but excessive gain can cause overshoot and sustained oscillations. This term provides the primary driving force toward the setpoint.\n\n**Integral Term**\n\nThe integral term $K_I I_k$ accumulates past tracking errors over time. This accumulation ensures that steady-state errors are eventually eliminated, even in the presence of constant disturbances or model uncertainties. The integral action drives the steady-state error to zero by continuing to increase the control effort as long as any error persists. However, excessive integral gain can cause overshoot and slow oscillations, and the accumulation of error during transients can lead to integral windup when control limits are reached.\n\n**Derivative Term**\n\nThe derivative term $K_D (e_k - e_{k-1})$ responds to the rate of change of the tracking error, providing anticipatory control action. This term acts as a predictor of future error trends, adding damping to reduce overshoot and improve stability. The derivative action opposes rapid changes in the plant output, smoothing the system response. However, because it amplifies high-frequency content, the derivative term can be sensitive to measurement noise and may require filtering in practice.\n\n**Combined Action**\n\nTogether, these three terms provide a versatile control strategy: the proportional term drives toward the setpoint, the integral term eliminates steady-state offset, and the derivative term provides damping and reduces overshoot. The relative weighting of these terms (the gains $K_P$, $K_I$, $K_D$) determines the closed-loop performance and must be selected through tuning.\n\nFor details on Ziegler-Nichols tuning, see [the original paper](https://doi.org/10.1115/1.3662552) or [this tutorial](https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method)." }, { "cell_type": "markdown", "id": "9a1f988b", "metadata": {}, "source": "## Algorithm as an $(f,h)$-representation\n\nTo represent the PID controller as a **discrete-time dynamical system**, we define the *algorithm state* to be the tuple\n$$\nc_k := (e_k, I_k, e_{k-1}),\n$$\nwhere:\n- $e_k \\in \\mathbb{R}$ is the current tracking error,\n- $I_k \\in \\mathbb{R}$ is the accumulated integral of error,\n- $e_{k-1} \\in \\mathbb{R}$ is the previous error (for derivative computation).\n\nThe controller is driven by inputs $(r_k, \\hat{x}_k)$ where:\n- $r_k$ is the reference (setpoint),\n- $\\hat{x}_k$ is the measured (or estimated) plant output.\n\nWith this setup, the PID controller is the recursion\n$$\nc_{k+1} = f(c_k, r_k, \\hat{x}_k; \\theta),\n$$\nwhere $\\theta = (K_P, K_I, K_D)$ are the controller gains. The state transition function $f$ computes:\n\n**Error computation**\n$$\ne_k = r_k - \\hat{x}_k.\n$$\n\n**Integral update**\n$$\nI_{k+1} = I_k + e_k.\n$$\n\n**State update**\n$$\nc_{k+1} = (e_k, I_{k+1}, e_k).\n$$\n\nThe controller output (control input to the plant) is\n$$\nu_k = h(c_k, r_k, \\hat{x}_k; \\theta) = K_P e_k + K_I I_k + K_D (e_k - e_{k-1}).\n$$\n\nIn `pykal`, this corresponds to:\n- **State transition**: `f(...)` implements the state evolution, taking the current state and inputs and returning the next state.\n- **Output map**: `h(...)` implements the control law, computing the control input $u_k$ from the current state and gains." }, { "cell_type": "code", "execution_count": null, "id": "99f62cf5", "metadata": {}, "outputs": [], "source": "from typing import Tuple\nfrom numpy.typing import NDArray\n\n\ndef f(\n *,\n ck: Tuple[float, float, float],\n rk: float,\n xhat_k: float) -> Tuple[float, float, float]:\n \"\"\"\n Perform one step of the PID controller state evolution.\n\n Parameters\n ----------\n ck : Tuple[float, float, float]\n Current controller state ``(e_k, I_k, e_{k-1})``:\n - ``e_k`` : current tracking error\n - ``I_k`` : accumulated integral of error\n - ``e_{k-1}`` : previous error (for derivative)\n\n rk : float\n Reference (setpoint) at time k.\n\n xhat_k : float\n Measured or estimated plant output at time k.\n\n Returns\n -------\n Tuple[float, float, float]\n Updated controller state ``(e_{k+1}, I_{k+1}, e_k)``:\n - ``e_{k+1}`` : new tracking error\n - ``I_{k+1}`` : updated integral\n - ``e_k`` : current error (becomes previous error for next step)\n\n Notes\n -----\n This function implements the PID controller state transition:\n\n Error computation:\n ``e_k = rk - xhat_k``\n\n Integral update:\n ``I_{k+1} = I_k + e_k``\n\n State update:\n ``ck_new = (e_k, I_{k+1}, e_k)``\n\n The control output is computed separately by the ``h`` function.\n \"\"\"\n e_k_prev, I_k, _ = ck\n \n # Compute current error\n e_k = rk - xhat_k\n \n # Update integral\n I_k_new = I_k + e_k\n \n # Return updated state: (current_error, new_integral, prev_error)\n return (e_k, I_k_new, e_k_prev)\n\n\ndef h(\n *,\n ck: Tuple[float, float, float],\n rk: float,\n xhat_k: float,\n KP: float,\n KI: float,\n KD: float) -> float:\n \"\"\"\n Compute the PID control output.\n\n Parameters\n ----------\n ck : Tuple[float, float, float]\n Current controller state ``(e_k, I_k, e_{k-1})``.\n\n rk : float\n Reference (setpoint) at time k.\n\n xhat_k : float\n Measured or estimated plant output at time k.\n\n KP : float\n Proportional gain.\n\n KI : float\n Integral gain.\n\n KD : float\n Derivative gain.\n\n Returns\n -------\n float\n Control input ``u_k`` to be applied to the plant.\n\n Notes\n -----\n This function implements the standard PID control law:\n\n Error:\n ``e_k = rk - xhat_k``\n\n Control output:\n ``u_k = KP * e_k + KI * I_k + KD * (e_k - e_{k-1})``\n\n The control output combines:\n - Proportional term: responds to current error\n - Integral term: eliminates steady-state error\n - Derivative term: provides damping based on error rate\n \"\"\"\n e_k_prev, I_k, e_k_old = ck\n \n # Compute current error\n e_k = rk - xhat_k\n \n # PID control law\n u_k = KP * e_k + KI * I_k + KD * (e_k - e_k_old)\n \n return u_k" }, { "cell_type": "markdown", "id": "example_intro", "metadata": {}, "source": "### Example: First-Order System Tracking\n\nWe demonstrate the PID controller on a simple first-order system (representing a DC motor, thermal system, or RC circuit). The controller tracks step changes in the setpoint, demonstrating its ability to achieve zero steady-state error and reject disturbances.\n\nWe consider the continuous-time first-order system\n$$\n\\dot{x}(t) = -a x(t) + b u(t),\n$$\nwhere $x(t)$ is the state, $u(t)$ is the control input, $a > 0$ is the decay rate (inverse time constant), and $b > 0$ is the control gain.\n\nUsing Euler discretization with time step $\\Delta t$, we obtain\n$$\nx_{k+1} = x_k + \\Delta t(-a x_k + b u_k).\n$$\n\nThe output is the state itself: $y_k = x_k$.\n\nNote that even though this is a simple first-order system, the PID controller must account for both immediate error (P term), accumulated error (I term), and error rate (D term) to achieve good tracking performance. This demonstrates the controller's versatility across different system dynamics." }, { "cell_type": "markdown", "id": "tikz_diagram", "metadata": {}, "source": "
\n\n
\n\nThe diagram above shows the closed-loop control structure: the reference $r_k$ is compared with the measured output (feedback) to compute the error $e_k$. The **PID Controller** block maintains internal state $c_k = (e_k, I_k, e_{k-1})$ and computes the control input $u_k$. The **Plant** block represents the system being controlled (with state $x_k$), which produces the output $y_k$ that is fed back for error computation." }, { "cell_type": "code", "execution_count": null, "id": "imports", "metadata": {}, "outputs": [], "source": "from pykal import DynamicalSystem\nimport numpy as np\nimport matplotlib.pyplot as plt" }, { "cell_type": "code", "execution_count": null, "id": "f4b6df9c", "metadata": {}, "outputs": [], "source": "def plant_f(x, u, a, b, dt):\n \"\"\"\n First-order system state evolution.\n \n Parameters:\n x: Current state (scalar)\n u: Control input (scalar)\n a: Decay rate parameter\n b: Control gain parameter\n dt: Time step\n \n Returns:\n Next state (scalar)\n \"\"\"\n return x + dt * (-a * x + b * u)\n\n\ndef plant_h(x):\n \"\"\"\n Plant output function.\n \n Parameters:\n x: Current state (scalar)\n \n Returns:\n Output (state itself)\n \"\"\"\n return x\n\n\nplant = DynamicalSystem(f=plant_f, h=plant_h)\ncontroller = DynamicalSystem(f=f, h=h)" }, { "cell_type": "code", "execution_count": null, "id": "9de7a233", "metadata": {}, "outputs": [], "source": "# Simulation parameters\ndt = 0.01 # Time step (10 ms)\nT = 10.0 # Total time (10 seconds)\ntime = np.arange(0, T, dt)\nN = len(time)\n\n# Plant parameters\na = 2.0 # Decay rate\nb = 3.0 # Control gain\n\n# Calculate Ziegler-Nichols PID gains\nK_system = b / a # System gain: 1.5\ntau_system = 1 / a # Time constant: 0.5 s\n\nKP = 1.2 / K_system # 0.8\nKI = 2 * KP / tau_system # 3.2\nKD = KP * tau_system / 2 # 0.2\n\nprint(f\"System parameters: K = {K_system:.2f}, \u03c4 = {tau_system:.2f} s\")\nprint(f\"PID gains (Ziegler-Nichols): KP = {KP:.3f}, KI = {KI:.3f}, KD = {KD:.3f}\")\n\n# Initial conditions\nx0 = 0.0 # Plant starts at zero\nck0 = (0.0, 0.0, 0.0) # Controller: (error, integral, prev_error)\n\n# Storage\nx_history = []\nu_history = []\nr_history = []\nck_history = []\n\n# Initial state\nxk = x0\nck = ck0" }, { "cell_type": "code", "execution_count": null, "id": "bf31d69c", "metadata": {}, "outputs": [], "source": "# Closed-loop simulation\nfor k, tk in enumerate(time):\n # Update setpoint (step changes)\n if tk < 3.0:\n rk = 1.0\n elif tk < 6.0:\n rk = 2.0\n else:\n rk = 0.5\n \n # Measure plant output (perfect measurement)\n yk = plant.h(x=xk)\n \n # Controller computes control input and updates state\n ck, uk = controller.step(\n params={\n 'ck': ck,\n 'rk': rk,\n 'xhat_k': yk,\n 'KP': KP,\n 'KI': KI,\n 'KD': KD\n }\n )\n \n # Apply control to plant\n xk = plant.f(x=xk, u=uk, a=a, b=b, dt=dt)\n \n # Store history\n x_history.append(yk)\n u_history.append(uk)\n r_history.append(rk)\n ck_history.append(ck)\n\n# Convert to arrays\nx_history = np.array(x_history)\nu_history = np.array(u_history)\nr_history = np.array(r_history)\ne_history = r_history - x_history\n\nprint(\"Simulation complete!\")" }, { "cell_type": "code", "execution_count": null, "id": "95245edd", "metadata": {}, "outputs": [], "source": "fig, axes = plt.subplots(3, 1, figsize=(12, 9))\n\n# Plot 1: Output tracking\naxes[0].plot(time, r_history, 'r--', linewidth=2, label='Setpoint $r(t)$')\naxes[0].plot(time, x_history, 'b-', linewidth=1.5, label='Output $y(t)$')\naxes[0].set_ylabel('Output', fontsize=11)\naxes[0].set_title('PID Control: First-Order System Tracking', fontsize=13, fontweight='bold')\naxes[0].legend(fontsize=10)\naxes[0].grid(True, alpha=0.3)\n\n# Plot 2: Control input\naxes[1].plot(time, u_history, 'g-', linewidth=1.5, label='Control $u(t)$')\naxes[1].set_ylabel('Control Input $u(t)$', fontsize=11)\naxes[1].legend(fontsize=10)\naxes[1].grid(True, alpha=0.3)\n\n# Plot 3: Tracking error\naxes[2].plot(time, e_history, 'm-', linewidth=1.5, label='Error $e(t)$')\naxes[2].axhline(y=0, color='k', linestyle=':', alpha=0.3)\naxes[2].set_ylabel('Tracking Error $e(t)$', fontsize=11)\naxes[2].set_xlabel('Time (seconds)', fontsize=11)\naxes[2].legend(fontsize=10)\naxes[2].grid(True, alpha=0.3)\n\nplt.tight_layout()\nplt.show()\n\n# Performance metrics\nprint(\"\\nPerformance Metrics:\")\nprint(f\" Final tracking error: {e_history[-1]:.6f}\")\nprint(f\" Max control effort: {np.max(np.abs(u_history)):.3f}\")\nprint(f\" RMS tracking error: {np.sqrt(np.mean(e_history**2)):.6f}\")" }, { "cell_type": "markdown", "id": "4720b78e", "metadata": {}, "source": "## Notes on Usage\n\nThe PID controller is appropriate when **most** of the following conditions hold:\n\n1. **Single-input single-output (SISO) system**: PID works best for controlling one output with one input\n2. **Stable or marginally stable plant**: The open-loop system should not be highly unstable\n3. **Smooth dynamics**: The plant responds reasonably to control inputs without excessive delays\n4. **Measurement availability**: The output (or an estimate) can be measured frequently\n\nIf your system is **multivariable (MIMO)**, consider using LQR, MPC, or decoupled PID loops. If your system has **significant nonlinearities**, consider gain scheduling, adaptive control, or nonlinear control methods. For **optimal control** with constraints, use MPC instead.\n\n### Common Applications\n\nThe PID controller is widely used in:\n\n- **Industrial process control**: Temperature, pressure, flow, level control\n- **Robotics**: Motor speed/position control, altitude control for drones\n- **Automotive**: Cruise control, engine control\n- **HVAC systems**: Temperature and humidity regulation\n- **Manufacturing**: CNC machines, assembly line automation\n\n### Tuning Guidance\n\nThe performance of the PID controller depends critically on the gain selection:\n\n- **Proportional gain $K_P$**: Larger values \u2192 faster response but more overshoot and oscillations\n- **Integral gain $K_I$**: Larger values \u2192 faster steady-state error elimination but more overshoot and potential instability\n- **Derivative gain $K_D$**: Larger values \u2192 more damping and reduced overshoot but amplifies measurement noise\n\n**Tuning methods**:\n- **Ziegler-Nichols**: Use step response or ultimate gain method (see [original paper](https://doi.org/10.1115/1.3662552))\n- **Cohen-Coon**: Better for systems with dead time\n- **Manual tuning**: Start with $K_I = K_D = 0$, increase $K_P$ until oscillations, then add $K_D$ for damping and $K_I$ to eliminate offset\n- **Model-based**: If plant model is known, use pole placement or optimization\n\nFor a practical guide to PID tuning, see [this tutorial](https://en.wikipedia.org/wiki/PID_controller#Manual_tuning).\n\n### Implementation Details\n\n**State representation**: The controller state is a tuple `(e_k, I_k, e_{k-1})` where:\n- `e_k`: current tracking error\n- `I_k`: accumulated integral\n- `e_{k-1}`: previous error for derivative\n\n**Controller inputs**: The controller requires:\n- `rk`: reference signal (setpoint)\n- `xhat_k`: measured or estimated plant output\n\n**Derivative on measurement**: This implementation computes the derivative on the error. An alternative is to compute the derivative on the measurement only (to avoid derivative kick when setpoint changes). This can be implemented by modifying the `h` function to use `-(xhat_k - xhat_{k-1})` instead of `(e_k - e_{k-1})`.\n\n**Anti-windup**: This basic implementation does not include anti-windup protection. For systems with control input saturation, consider adding integral clamping or back-calculation to prevent windup.\n\n**Filtering**: For noisy measurements, consider low-pass filtering the derivative term or using a filtered derivative approximation." }, { "cell_type": "markdown", "id": "cell-14", "metadata": {}, "source": "[\u2190 Algorithms as Dynamical Systems](../../getting_started/theory_to_python/algorithms_as_dynamical_systems.rst)" } ], "metadata": { "language_info": { "name": "python", "version": "3.12.0", "mimetype": "text/x-python", "codemirror_mode": { "name": "ipython", "version": 3 }, "pygments_lexer": "ipython3", "nbconvert_exporter": "python", "file_extension": ".py" } }, "nbformat": 4, "nbformat_minor": 5 }