PID Controller

← f_h Functions

Derivation, argument reference, and worked example for the pid block.

Overview

The PID controller computes a control input \(u_k\) from three terms:

Proportional — reacts to the current error
Integral — corrects steady-state offset by accumulating past error
Derivative — anticipates future error by reacting to its rate of change

It requires no plant model, making it the most widely deployed controller in practice.

Equations

At each step, compute the error between the reference and current estimate:

\[e_k = r_k - \hat{x}_k\]

The control output is:

\[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}\]

The internal state \(c_k\) carries the two quantities needed across steps:

\[c_k = (e_{k-1},\; e^\text{int}_k) = \left(e_{k-1},\; \sum_{j=0}^{k} e_j\,\Delta t\right)\]

pid_f advances the state:

\[c_{k+1} = (e_k,\; e^\text{int}_k + e_k\,\Delta t)\]

pid_h computes the output from the pre-update state and the current error:

\[u_k = K_P\,e_k + K_I\,e^\text{int}_{k-1} + K_D\,\frac{e_k - e_{k-1}}{\Delta t}\]

Argument Reference

`ck` — PID state `tuple[float, float]`

Field	Symbol	Description
`ck[0]`	\(e_{k-1}\)	Error from the previous step
`ck[1]`	\(e^\text{int}\)	Accumulated integral \(\sum e_j\,\Delta t\)

Initialisation: ck = (0.0, 0.0) — zero prior error and zero integral.

`rk` — reference `float`

The setpoint or desired value at step \(k\). The controller drives \(\hat{x}_k \to r_k\).

`xhatk` — state estimate `float`

The current scalar state estimate, typically the output of an estimator block. The error is \(e_k = r_k - \hat{x}_k\).

`KP` — proportional gain `float`

Scales the current error. Higher \(K_P\) gives faster response but risks overshoot and oscillation.

`KI` — integral gain `float`

Scales the accumulated error. Eliminates steady-state offset; too large causes integral windup and instability. Set KI=0.0 to disable the integral term.

`KD` — derivative gain `float`

Scales the finite-difference approximation of \(\dot{e}\). Damps oscillations; sensitive to measurement noise. Set KD=0.0 to disable the derivative term.

`dt` — timestep `float`

The time interval between steps in seconds. Used for both the integral accumulation (\(e_k\,\Delta t\)) and the derivative approximation (\((e_k - e_{k-1}) / \Delta t\)). Must match the actual loop rate.

Implementation

pid_f advances the PID state — updating the stored error and integral. pid_h computes the control output \(u_k\) from the pre-update state.

Per DynamicalSystem convention, h sees the state before f updates it, so pid_h uses ck[1] (the integral accumulated up to the previous step) and ck[0] (the previous error). This is consistent: the output \(u_k\) is computed from information available at the start of step \(k\).

from jaxtyping import jaxtyped
from beartype import beartype
from dynamicalnodes import DynamicalSystem

@jaxtyped(typechecker=beartype)
def pid_f(
    ck: tuple[float, float],
    rk: float,
    xhatk: float,
    KP: float,
    KI: float,
    KD: float,
    dt: float,
) -> tuple[float, float]:
    """Advance the PID state; returns (current error, updated integral).

    Args:
        ck:    PID state — 2-tuple of (e_{k-1}, e_int), previous error and accumulated integral.
        rk:    Reference (setpoint) at step k.
        xhatk: Current scalar state estimate.
        KP:    Proportional gain.
        KI:    Integral gain.
        KD:    Derivative gain.
        dt:    Timestep in seconds.
    """
    e_prev, e_int = ck
    ek = rk - xhatk
    return (ek, e_int + ek * dt)


@jaxtyped(typechecker=beartype)
def pid_h(
    ck: tuple[float, float],
    rk: float,
    xhatk: float,
    KP: float,
    KI: float,
    KD: float,
    dt: float,
) -> float:
    """Compute the PID control output u_k = KP*e + KI*e_int + KD*de/dt.

    Args:
        ck:    PID state — 2-tuple of (e_{k-1}, e_int), previous error and accumulated integral.
        rk:    Reference (setpoint) at step k.
        xhatk: Current scalar state estimate.
        KP:    Proportional gain.
        KI:    Integral gain.
        KD:    Derivative gain.
        dt:    Timestep in seconds.
    """
    e_prev, e_int = ck
    ek = rk - xhatk
    return KP * ek + KI * e_int + KD * (ek - e_prev) / dt


pid = DynamicalSystem(f=pid_f, h=pid_h)

Worked Example — Single Step

Reference 10 m/s, current estimate 0 m/s.

ck = (0.0, 0.0)   # (e_prev, e_int)
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)
print("control output u:", uk)
print("updated PID state:", ck_next)   # (e_k, e_int + e_k*dt)

`DynamicalSystem` → `ROSNode`

from dynamicalnodes import ROSNode
from dynamicalnodes.ros2py_py2ros import ros2py_float64, py2ros_float64
from std_msgs.msg import Float64

pid_node = ROSNode(
    dynamical_system=pid,
    subscribes_to=[
        {
            "topic": "/reference",
            "msg_type": Float64,
            "arg": "rk",
            "ros2py": ros2py_float64,
        },
        {
            "topic": "/state_estimate",
            "msg_type": Float64,
            "arg": "xhatk",
            "ros2py": ros2py_float64,
        },
    ],
    publishes_to=[
        {
            "topic": "/control",
            "msg_type": Float64,
            "py2ros": py2ros_float64,
        },
    ],
)

`ROSNode.write_ROSNode_to_rclpy()`

pid_node.write_ROSNode_to_rclpy(
    "pid_node.py",
    node_name="pid_controller",
    initial_state=(0.0, 0.0),  # (e_prev, e_int)
    static_params={"dt": 0.05},
    dynamic_params={
        "KP": 2.0,  # ros2 param set /pid_controller KP 3.0
        "KI": 0.5,
        "KD": 0.1,
    },
    initial_inputs={
        "rk": 0.0,
        "xhatk": 0.0,
    },
)

← f_h Functions

PID Controller

Overview

Equations

Argument Reference

ck — PID state tuple[float, float]

rk — reference float

xhatk — state estimate float

KP — proportional gain float

KI — integral gain float

KD — derivative gain float

dt — timestep float