LQR Controller
Derivation, argument reference, and worked example for the lqr block.
Overview
The Linear Quadratic Regulator (LQR) is the optimal state-feedback controller for linear systems. It minimises the infinite-horizon quadratic cost:
The optimal gain \(K\) is computed once offline by solving the discrete algebraic Riccati equation (DARE), then held constant during operation. At runtime, control is a simple matrix-vector multiply:
The lqr block is stateless — f is not used; only lqr_h is needed.
Derivation
For a discrete-time linear system \(x_{k+1} = A x_k + B u_k\), the DARE gives the unique positive semi-definite solution \(P\) to:
The optimal gain follows directly:
Intuition: \(Q\) penalises state deviation; \(R\) penalises control effort. A large \(Q_{ii}\) relative to \(R\) drives the \(i\)-th state component to zero aggressively. A large \(R\) produces a gentler, more energy-efficient response.
Argument Reference
lqr_gain helper
Call once before the loop to compute \(K\). Takes the system matrices and cost weights.
A — state matrix Float[np.ndarray, "n n"]
Shape (n, n). The discrete-time state transition matrix of the linearised plant:
\(x_{k+1} = A x_k + B u_k\).
B — input matrix Float[np.ndarray, "n m"]
Shape (n, m). Maps the control input \(u_k \in \mathbb{R}^m\) into the state space.
Example — double integrator (x = [position, velocity], scalar input):
B = np.array([[0.0], [dt]]) # shape (2, 1)
Q — state cost matrix Float[np.ndarray, "n n"]
Shape (n, n), symmetric positive semi-definite. Penalises state deviation from the reference.
Diagonal \(Q\) is the most common choice — each entry weights the corresponding state component:
Q = np.diag([0.0, 100.0]) # penalise velocity error only
Setting a diagonal entry to zero means that state component is not penalised (but may still be indirectly controlled via coupling).
R — control cost matrix Float[np.ndarray, "m m"]
Shape (m, m), symmetric positive definite. Penalises control effort. Must be invertible.
A scalar system with one input:
R = np.array([[1e-2]]) # low R → aggressive control; high R → gentle control
lqr_h inputs
xhatk — state estimate Float[np.ndarray, "n"]
Shape (n,). The current state estimate from an estimator block (KF, EKF, UKF, or direct measurement).
xrefk — reference state Float[np.ndarray, "n"]
Shape (n,). The desired target state at step \(k\). The controller drives \(\hat{x}_k \to x^\text{ref}_k\).
For regulation (drive to zero), pass xrefk = np.zeros(n).
K — gain matrix Float[np.ndarray, "m n"]
Shape (m, n). The precomputed optimal gain from lqr_gain(A, B, Q, R).
Compute once and reuse every step.
Implementation
lqr_gain solves the DARE and returns \(K\) — call this once before the simulation loop.
lqr_h computes \(u_k = -K(\hat{x}_k - x^\text{ref}_k)\) — this is the only function
used at runtime. The lqr block is stateless so f is omitted.
import numpy as np
from scipy.linalg import solve_discrete_are
from jaxtyping import Float, jaxtyped
from beartype import beartype
from dynamicalnodes import DynamicalSystem
@jaxtyped(typechecker=beartype)
def lqr_gain(
A: Float[np.ndarray, "n n"],
B: Float[np.ndarray, "n m"],
Q: Float[np.ndarray, "n n"],
R: Float[np.ndarray, "m m"],
) -> Float[np.ndarray, "m n"]:
"""Compute the discrete-time LQR gain K = (R + B'PB)^{-1} B'PA via DARE.
Args:
A: State transition matrix (n, n) of the linearised plant.
B: Control input matrix (n, m) — maps u_k into state space.
Q: State cost matrix (n, n), symmetric positive semi-definite.
R: Control cost matrix (m, m), symmetric positive definite.
"""
P = solve_discrete_are(A, B, Q, R)
return np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
@jaxtyped(typechecker=beartype)
def lqr_h(
xhatk: Float[np.ndarray, "n"],
xrefk: Float[np.ndarray, "n"],
K: Float[np.ndarray, "m n"],
) -> Float[np.ndarray, "m"]:
"""Compute the LQR control output u_k = -K(x̂_k - x_ref_k).
Args:
xhatk: Current state estimate (n,) from an estimator or sensor.
xrefk: Reference (target) state (n,).
K: Precomputed LQR gain matrix (m, n) from lqr_gain.
"""
return -K @ (xhatk - xrefk)
lqr = DynamicalSystem(h=lqr_h)
Worked Example — Double Integrator
State x = [position, velocity], scalar input, track a velocity reference.
dt = 0.1
A = np.array([[1.0, dt], [0.0, 1.0]])
B = np.array([[0.0], [dt]])
Q = np.diag([0.0, 100.0]) # penalise velocity error only
R = np.array([[1e-2]])
K = lqr_gain(A, B, Q, R)
print("LQR gain K:", K)
xhatk = np.array([0.0, 0.0]) # current estimate
xrefk = np.array([0.0, 5.0]) # reference: 5 m/s
uk = lqr.step(xhatk=xhatk, xrefk=xrefk, K=K)
print("control output u:", uk)
DynamicalSystem → ROSNode
from dynamicalnodes import ROSNode
from dynamicalnodes.ros2py_py2ros import ros2py_float64_multiarray, py2ros_float64_multiarray
from std_msgs.msg import Float64MultiArray
lqr_node = ROSNode(
dynamical_system=lqr,
subscribes_to=[
{
"topic": "/state_estimate",
"msg_type": Float64MultiArray,
"arg": "xhatk",
"ros2py": ros2py_float64_multiarray,
},
{
"topic": "/reference",
"msg_type": Float64MultiArray,
"arg": "xrefk",
"ros2py": ros2py_float64_multiarray,
},
],
publishes_to=[
{
"topic": "/control",
"msg_type": Float64MultiArray,
"py2ros": py2ros_float64_multiarray,
},
],
)
ROSNode.write_ROSNode_to_rclpy()
dt_lqr = 0.1
A = np.array([[1.0, dt_lqr], [0.0, 1.0]])
B = np.array([[0.0], [dt_lqr]])
Q = np.diag([0.0, 100.0])
R = np.array([[1e-2]])
K = lqr_gain(A, B, Q, R)
lqr_node.write_ROSNode_to_rclpy(
"lqr_node.py",
node_name="lqr_controller",
static_params={"K": K},
initial_inputs={
"xhatk": np.array([0.0, 0.0]),
"xrefk": np.array([0.0, 0.0]),
},
)