{ "nbformat": 4, "nbformat_minor": 5, "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "name": "python", "version": "3.12.0" }, "orphan": true }, "cells": [ { "cell_type": "markdown", "id": "pid-title", "metadata": {}, "source": [ "# PID Controller\n", "\n", "\u2190 [f_h Functions](../controllers.ipynb)\n", "\n", "Derivation, argument reference, and worked example for the `pid` block." ] }, { "cell_type": "markdown", "id": "pid-overview", "metadata": {}, "source": [ "## Overview\n", "\n", "The PID controller computes a control input $u_k$ from three terms:\n", "\n", "- **Proportional** \u2014 reacts to the current error\n", "- **Integral** \u2014 corrects steady-state offset by accumulating past error\n", "- **Derivative** \u2014 anticipates future error by reacting to its rate of change\n", "\n", "It requires no plant model, making it the most widely deployed controller in practice." ] }, { "cell_type": "markdown", "id": "pid-equations", "metadata": {}, "source": [ "## Equations\n", "\n", "At each step, compute the error between the reference and current estimate:\n", "\n", "$$e_k = r_k - \\hat{x}_k$$\n", "\n", "The control output is:\n", "\n", "$$u_k = K_P\\,e_k + K_I\\sum_{j=0}^{k} e_j\\,\\Delta t + K_D\\,\\frac{e_k - e_{k-1}}{\\Delta t}$$\n", "\n", "The internal state $c_k$ carries the two quantities needed across steps:\n", "\n", "$$c_k = (e_{k-1},\\; e^\\text{int}_k) = \\left(e_{k-1},\\; \\sum_{j=0}^{k} e_j\\,\\Delta t\\right)$$\n", "\n", "**`pid_f`** advances the state:\n", "\n", "$$c_{k+1} = (e_k,\\; e^\\text{int}_k + e_k\\,\\Delta t)$$\n", "\n", "**`pid_h`** computes the output from the pre-update state and the current error:\n", "\n", "$$u_k = K_P\\,e_k + K_I\\,e^\\text{int}_{k-1} + K_D\\,\\frac{e_k - e_{k-1}}{\\Delta t}$$" ] }, { "cell_type": "markdown", "id": "pid-args", "metadata": {}, "source": [ "## Argument Reference\n", "\n", "### `ck` \u2014 PID state   `tuple[float, float]`\n", "\n", "| Field | Symbol | Description |\n", "|-------|--------|-------------|\n", "| `ck[0]` | $e_{k-1}$ | Error from the previous step |\n", "| `ck[1]` | $e^\\text{int}$ | Accumulated integral $\\sum e_j\\,\\Delta t$ |\n", "\n", "**Initialisation:** `ck = (0.0, 0.0)` \u2014 zero prior error and zero integral.\n", "\n", "---\n", "\n", "### `rk` \u2014 reference   `float`\n", "\n", "The setpoint or desired value at step $k$. The controller drives $\\hat{x}_k \\to r_k$.\n", "\n", "---\n", "\n", "### `xhatk` \u2014 state estimate   `float`\n", "\n", "The current scalar state estimate, typically the output of an estimator block. The error is $e_k = r_k - \\hat{x}_k$.\n", "\n", "---\n", "\n", "### `KP` \u2014 proportional gain   `float`\n", "\n", "Scales the current error. Higher $K_P$ gives faster response but risks overshoot and oscillation.\n", "\n", "---\n", "\n", "### `KI` \u2014 integral gain   `float`\n", "\n", "Scales the accumulated error. Eliminates steady-state offset; too large causes integral windup and instability.\n", "Set `KI=0.0` to disable the integral term.\n", "\n", "---\n", "\n", "### `KD` \u2014 derivative gain   `float`\n", "\n", "Scales the finite-difference approximation of $\\dot{e}$. Damps oscillations; sensitive to measurement noise.\n", "Set `KD=0.0` to disable the derivative term.\n", "\n", "---\n", "\n", "### `dt` \u2014 timestep   `float`\n", "\n", "The time interval between steps in seconds. Used for both the integral accumulation ($e_k\\,\\Delta t$)\n", "and the derivative approximation ($(e_k - e_{k-1}) / \\Delta t$). Must match the actual loop rate." ] }, { "cell_type": "markdown", "id": "pid-impl-header", "metadata": {}, "source": [ "## Implementation\n", "\n", "`pid_f` advances the PID state \u2014 updating the stored error and integral.\n", "`pid_h` computes the control output $u_k$ from the pre-update state.\n", "\n", "Per `DynamicalSystem` convention, `h` sees the state *before* `f` updates it, so\n", "`pid_h` uses `ck[1]` (the integral accumulated up to the previous step) and `ck[0]` (the previous error).\n", "This is consistent: the output $u_k$ is computed from information available at the start of step $k$." ] }, { "cell_type": "code", "id": "pid-imports", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "from jaxtyping import jaxtyped\n", "from beartype import beartype\n", "from dynamicalnodes import DynamicalSystem" ] }, { "cell_type": "code", "id": "pid-impl", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "@jaxtyped(typechecker=beartype)\n", "def pid_f(\n", " ck: tuple[float, float],\n", " rk: float,\n", " xhatk: float,\n", " KP: float,\n", " KI: float,\n", " KD: float,\n", " dt: float,\n", ") -> tuple[float, float]:\n", " \"\"\"Advance the PID state; returns (current error, updated integral).\n", "\n", " Args:\n", " ck: PID state \u2014 2-tuple of (e_{k-1}, e_int), previous error and accumulated integral.\n", " rk: Reference (setpoint) at step k.\n", " xhatk: Current scalar state estimate.\n", " KP: Proportional gain.\n", " KI: Integral gain.\n", " KD: Derivative gain.\n", " dt: Timestep in seconds.\n", " \"\"\"\n", " e_prev, e_int = ck\n", " ek = rk - xhatk\n", " return (ek, e_int + ek * dt)\n", "\n", "\n", "@jaxtyped(typechecker=beartype)\n", "def pid_h(\n", " ck: tuple[float, float],\n", " rk: float,\n", " xhatk: float,\n", " KP: float,\n", " KI: float,\n", " KD: float,\n", " dt: float,\n", ") -> float:\n", " \"\"\"Compute the PID control output u_k = KP*e + KI*e_int + KD*de/dt.\n", "\n", " Args:\n", " ck: PID state \u2014 2-tuple of (e_{k-1}, e_int), previous error and accumulated integral.\n", " rk: Reference (setpoint) at step k.\n", " xhatk: Current scalar state estimate.\n", " KP: Proportional gain.\n", " KI: Integral gain.\n", " KD: Derivative gain.\n", " dt: Timestep in seconds.\n", " \"\"\"\n", " e_prev, e_int = ck\n", " ek = rk - xhatk\n", " return KP * ek + KI * e_int + KD * (ek - e_prev) / dt\n", "\n", "\n", "pid = DynamicalSystem(f=pid_f, h=pid_h)" ] }, { "cell_type": "markdown", "id": "pid-usage-header", "metadata": {}, "source": [ "## Worked Example \u2014 Single Step\n", "\n", "Reference 10 m/s, current estimate 0 m/s." ] }, { "cell_type": "code", "id": "pid-usage", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "ck = (0.0, 0.0) # (e_prev, e_int)\n", "ck_next, uk = pid.step(ck=ck, rk=10.0, xhatk=0.0, KP=2.0, KI=0.5, KD=0.1, dt=0.05)\n", "print(\"control output u:\", uk)\n", "print(\"updated PID state:\", ck_next) # (e_k, e_int + e_k*dt)" ] }, { "cell_type": "markdown", "id": "8431832d", "source": [ "#", "#", " ", "`", "D", "y", "n", "a", "m", "i", "c", "a", "l", "S", "y", "s", "t", "e", "m", "`", " ", "\u2192", " ", "`", "R", "O", "S", "N", "o", "d", "e", "`" ], "metadata": {} }, { "cell_type": "code", "id": "a3064129", "source": [ "f", "r", "o", "m", " ", "d", "y", "n", "a", "m", "i", "c", "a", "l", "n", "o", "d", "e", "s", " ", "i", "m", "p", "o", "r", "t", " ", "R", "O", "S", "N", "o", "d", "e", "\n", "f", "r", "o", "m", " ", "d", "y", "n", "a", "m", "i", "c", "a", "l", "n", "o", "d", "e", "s", ".", "r", "o", "s", "2", "p", "y", "_", "p", "y", "2", "r", "o", "s", " ", "i", "m", "p", "o", "r", "t", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", ",", " ", "p", "y", "2", "r", "o", "s", "_", "f", "l", "o", "a", "t", "6", "4", "\n", "f", "r", "o", "m", " ", "s", "t", "d", "_", "m", "s", "g", "s", ".", "m", "s", "g", " ", "i", "m", "p", "o", "r", "t", " ", "F", "l", "o", "a", "t", "6", "4", "\n", "\n", "p", "i", "d", "_", "n", "o", "d", "e", " ", "=", " ", "R", "O", "S", "N", "o", "d", "e", "(", "\n", " ", " ", " ", " ", "d", "y", "n", "a", "m", "i", "c", "a", "l", "_", "s", "y", "s", "t", "e", "m", "=", "p", "i", "d", ",", "\n", " ", " ", " ", " ", "s", "u", "b", "s", "c", "r", "i", "b", "e", "s", "_", "t", "o", "=", "[", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "t", "o", "p", "i", "c", "\"", ":", " ", "\"", "/", "r", "e", "f", "e", "r", "e", "n", "c", "e", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "m", "s", "g", "_", "t", "y", "p", "e", "\"", ":", " ", "F", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "a", "r", "g", "\"", ":", " ", "\"", "r", "k", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "r", "o", "s", "2", "p", "y", "\"", ":", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "t", "o", "p", "i", "c", "\"", ":", " ", "\"", "/", "s", "t", "a", "t", "e", "_", "e", "s", "t", "i", "m", "a", "t", "e", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "m", "s", "g", "_", "t", "y", "p", "e", "\"", ":", " ", "F", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "a", "r", "g", "\"", ":", " ", "\"", "x", "h", "a", "t", "k", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "r", "o", "s", "2", "p", "y", "\"", ":", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", "]", ",", "\n", " ", " ", " ", " ", "p", "u", "b", "l", "i", "s", "h", "e", "s", "_", "t", "o", "=", "[", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "t", "o", "p", "i", "c", "\"", ":", " ", "\"", "/", "c", "o", "n", "t", "r", "o", "l", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "m", "s", "g", "_", "t", "y", "p", "e", "\"", ":", " ", "F", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "p", "y", "2", "r", "o", "s", "\"", ":", " ", "p", "y", "2", "r", "o", "s", "_", "f", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", "]", ",", "\n", ")" ], "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "47302017", "source": [ "#", "#", " ", "`", "R", "O", "S", "N", "o", "d", "e", ".", "w", "r", "i", "t", "e", "_", "R", "O", "S", "N", "o", "d", "e", "_", "t", "o", "_", "r", "c", "l", "p", "y", "(", ")", "`" ], "metadata": {} }, { "cell_type": "code", "id": "11668908", "source": [ "p", "i", "d", "_", "n", "o", "d", "e", ".", "w", "r", "i", "t", "e", "_", "R", "O", "S", "N", "o", "d", "e", "_", "t", "o", "_", "r", "c", "l", "p", "y", "(", "\n", " ", " ", " ", " ", "\"", "p", "i", "d", "_", "n", "o", "d", "e", ".", "p", "y", "\"", ",", "\n", " ", " ", " ", " ", "n", "o", "d", "e", "_", "n", "a", "m", "e", "=", "\"", "p", "i", "d", "_", "c", "o", "n", "t", "r", "o", "l", "l", "e", "r", "\"", ",", "\n", " ", " ", " ", " ", "i", "n", "i", "t", "i", "a", "l", "_", "s", "t", "a", "t", "e", "=", "(", "0", ".", "0", ",", " ", "0", ".", "0", ")", ",", " ", " ", "#", " ", "(", "e", "_", "p", "r", "e", "v", ",", " ", "e", "_", "i", "n", "t", ")", "\n", " ", " ", " ", " ", "s", "t", "a", "t", "i", "c", "_", "p", "a", "r", "a", "m", "s", "=", "{", "\"", "d", "t", "\"", ":", " ", "0", ".", "0", "5", "}", ",", "\n", " ", " ", " ", " ", "d", "y", "n", "a", "m", "i", "c", "_", "p", "a", "r", "a", "m", "s", "=", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "K", "P", "\"", ":", " ", "2", ".", "0", ",", " ", " ", "#", " ", "r", "o", "s", "2", " ", "p", "a", "r", "a", "m", " ", "s", "e", "t", " ", "/", "p", "i", "d", "_", "c", "o", "n", "t", "r", "o", "l", "l", "e", "r", " ", "K", "P", " ", "3", ".", "0", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "K", "I", "\"", ":", " ", "0", ".", "5", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "K", "D", "\"", ":", " ", "0", ".", "1", ",", "\n", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", "i", "n", "i", "t", "i", "a", "l", "_", "i", "n", "p", "u", "t", "s", "=", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "r", "k", "\"", ":", " ", "0", ".", "0", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "x", "h", "a", "t", "k", "\"", ":", " ", "0", ".", "0", ",", "\n", " ", " ", " ", " ", "}", ",", "\n", ")" ], "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "pid-backlink", "metadata": {}, "source": [ "\u2190 [f_h Functions](../controllers.ipynb)" ] } ] }