{ "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": "lqr-title", "metadata": {}, "source": [ "# LQR Controller\n", "\n", "\u2190 [f_h Functions](../controllers.ipynb)\n", "\n", "Derivation, argument reference, and worked example for the `lqr` block." ] }, { "cell_type": "markdown", "id": "lqr-overview", "metadata": {}, "source": [ "## Overview\n", "\n", "The Linear Quadratic Regulator (LQR) is the **optimal state-feedback controller** for linear systems.\n", "It minimises the infinite-horizon quadratic cost:\n", "\n", "$$J = \\sum_{k=0}^{\\infty} \\left( x_k^\\top Q\\, x_k + u_k^\\top R\\, u_k \\right)$$\n", "\n", "The optimal gain $K$ is computed **once offline** by solving the discrete algebraic Riccati equation (DARE),\n", "then held constant during operation. At runtime, control is a simple matrix-vector multiply:\n", "\n", "$$u_k = -K\\,(\\hat{x}_k - x^\\text{ref}_k)$$\n", "\n", "The `lqr` block is **stateless** \u2014 `f` is not used; only `lqr_h` is needed." ] }, { "cell_type": "markdown", "id": "lqr-derivation", "metadata": {}, "source": [ "## Derivation\n", "\n", "For a discrete-time linear system $x_{k+1} = A x_k + B u_k$, the DARE gives the unique\n", "positive semi-definite solution $P$ to:\n", "\n", "$$P = A^\\top P A - A^\\top P B\\,(R + B^\\top P B)^{-1} B^\\top P A + Q$$\n", "\n", "The optimal gain follows directly:\n", "\n", "$$K = (R + B^\\top P B)^{-1} B^\\top P A$$\n", "\n", "**Intuition:** $Q$ penalises state deviation; $R$ penalises control effort. A large $Q_{ii}$ relative\n", "to $R$ drives the $i$-th state component to zero aggressively. A large $R$ produces a gentler,\n", "more energy-efficient response." ] }, { "cell_type": "markdown", "id": "lqr-args", "metadata": {}, "source": [ "## Argument Reference\n", "\n", "### `lqr_gain` helper\n", "\n", "Call once before the loop to compute $K$. Takes the system matrices and cost weights.\n", "\n", "### `A` \u2014 state matrix   `Float[np.ndarray, \"n n\"]`\n", "\n", "Shape `(n, n)`. The discrete-time state transition matrix of the linearised plant:\n", "$x_{k+1} = A x_k + B u_k$.\n", "\n", "---\n", "\n", "### `B` \u2014 input matrix   `Float[np.ndarray, \"n m\"]`\n", "\n", "Shape `(n, m)`. Maps the control input $u_k \\in \\mathbb{R}^m$ into the state space.\n", "\n", "**Example \u2014 double integrator** (`x = [position, velocity]`, scalar input):\n", "\n", "```python\n", "B = np.array([[0.0], [dt]]) # shape (2, 1)\n", "```\n", "\n", "---\n", "\n", "### `Q` \u2014 state cost matrix   `Float[np.ndarray, \"n n\"]`\n", "\n", "Shape `(n, n)`, symmetric positive semi-definite. Penalises state deviation from the reference.\n", "Diagonal $Q$ is the most common choice \u2014 each entry weights the corresponding state component:\n", "\n", "```python\n", "Q = np.diag([0.0, 100.0]) # penalise velocity error only\n", "```\n", "\n", "Setting a diagonal entry to zero means that state component is not penalised (but may still\n", "be indirectly controlled via coupling).\n", "\n", "---\n", "\n", "### `R` \u2014 control cost matrix   `Float[np.ndarray, \"m m\"]`\n", "\n", "Shape `(m, m)`, symmetric positive definite. Penalises control effort. Must be invertible.\n", "A scalar system with one input:\n", "\n", "```python\n", "R = np.array([[1e-2]]) # low R \u2192 aggressive control; high R \u2192 gentle control\n", "```\n", "\n", "---\n", "\n", "### `lqr_h` inputs\n", "\n", "### `xhatk` \u2014 state estimate   `Float[np.ndarray, \"n\"]`\n", "\n", "Shape `(n,)`. The current state estimate from an estimator block (KF, EKF, UKF, or direct measurement).\n", "\n", "---\n", "\n", "### `xrefk` \u2014 reference state   `Float[np.ndarray, \"n\"]`\n", "\n", "Shape `(n,)`. The desired target state at step $k$. The controller drives $\\hat{x}_k \\to x^\\text{ref}_k$.\n", "For regulation (drive to zero), pass `xrefk = np.zeros(n)`.\n", "\n", "---\n", "\n", "### `K` \u2014 gain matrix   `Float[np.ndarray, \"m n\"]`\n", "\n", "Shape `(m, n)`. The precomputed optimal gain from `lqr_gain(A, B, Q, R)`.\n", "Compute once and reuse every step." ] }, { "cell_type": "markdown", "id": "lqr-impl-header", "metadata": {}, "source": [ "## Implementation\n", "\n", "`lqr_gain` solves the DARE and returns $K$ \u2014 call this once before the simulation loop.\n", "`lqr_h` computes $u_k = -K(\\hat{x}_k - x^\\text{ref}_k)$ \u2014 this is the only function\n", "used at runtime. The `lqr` block is stateless so `f` is omitted." ] }, { "cell_type": "code", "id": "lqr-imports", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "import numpy as np\n", "from scipy.linalg import solve_discrete_are\n", "from jaxtyping import Float, jaxtyped\n", "from beartype import beartype\n", "from dynamicalnodes import DynamicalSystem" ] }, { "cell_type": "code", "id": "lqr-impl", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "@jaxtyped(typechecker=beartype)\n", "def lqr_gain(\n", " A: Float[np.ndarray, \"n n\"],\n", " B: Float[np.ndarray, \"n m\"],\n", " Q: Float[np.ndarray, \"n n\"],\n", " R: Float[np.ndarray, \"m m\"],\n", ") -> Float[np.ndarray, \"m n\"]:\n", " \"\"\"Compute the discrete-time LQR gain K = (R + B'PB)^{-1} B'PA via DARE.\n", "\n", " Args:\n", " A: State transition matrix (n, n) of the linearised plant.\n", " B: Control input matrix (n, m) \u2014 maps u_k into state space.\n", " Q: State cost matrix (n, n), symmetric positive semi-definite.\n", " R: Control cost matrix (m, m), symmetric positive definite.\n", " \"\"\"\n", " P = solve_discrete_are(A, B, Q, R)\n", " return np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A\n", "\n", "\n", "@jaxtyped(typechecker=beartype)\n", "def lqr_h(\n", " xhatk: Float[np.ndarray, \"n\"],\n", " xrefk: Float[np.ndarray, \"n\"],\n", " K: Float[np.ndarray, \"m n\"],\n", ") -> Float[np.ndarray, \"m\"]:\n", " \"\"\"Compute the LQR control output u_k = -K(x\u0302_k - x_ref_k).\n", "\n", " Args:\n", " xhatk: Current state estimate (n,) from an estimator or sensor.\n", " xrefk: Reference (target) state (n,).\n", " K: Precomputed LQR gain matrix (m, n) from lqr_gain.\n", " \"\"\"\n", " return -K @ (xhatk - xrefk)\n", "\n", "\n", "lqr = DynamicalSystem(h=lqr_h)" ] }, { "cell_type": "markdown", "id": "lqr-usage-header", "metadata": {}, "source": [ "## Worked Example \u2014 Double Integrator\n", "\n", "State `x = [position, velocity]`, scalar input, track a velocity reference." ] }, { "cell_type": "code", "id": "lqr-usage", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "dt = 0.1\n", "A = np.array([[1.0, dt], [0.0, 1.0]])\n", "B = np.array([[0.0], [dt]])\n", "Q = np.diag([0.0, 100.0]) # penalise velocity error only\n", "R = np.array([[1e-2]])\n", "\n", "K = lqr_gain(A, B, Q, R)\n", "print(\"LQR gain K:\", K)\n", "\n", "xhatk = np.array([0.0, 0.0]) # current estimate\n", "xrefk = np.array([0.0, 5.0]) # reference: 5 m/s\n", "uk = lqr.step(xhatk=xhatk, xrefk=xrefk, K=K)\n", "print(\"control output u:\", uk)" ] }, { "cell_type": "markdown", "id": "1e420a6a", "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": "037c1058", "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", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", " ", "p", "y", "2", "r", "o", "s", "_", "f", "l", "o", "a", "t", "6", "4", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", "\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", "M", "u", "l", "t", "i", "A", "r", "r", "a", "y", "\n", "\n", "l", "q", "r", "_", "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", "=", "l", "q", "r", ",", "\n", " ", " ", " ", " ", "s", "u", "b", "s", "c", "r", "i", "b", "e", "s", "_", "t", "o", "=", "[", "\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", "M", "u", "l", "t", "i", "A", "r", "r", "a", "y", ",", "\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", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\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", "M", "u", "l", "t", "i", "A", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "a", "r", "g", "\"", ":", " ", "\"", "x", "r", "e", "f", "k", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "r", "o", "s", "2", "p", "y", "\"", ":", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", "\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", "M", "u", "l", "t", "i", "A", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "p", "y", "2", "r", "o", "s", "\"", ":", " ", "p", "y", "2", "r", "o", "s", "_", "f", "l", "o", "a", "t", "6", "4", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", "]", ",", "\n", ")" ], "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "44d6fe2b", "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": "be2efd9b", "source": [ "d", "t", "_", "l", "q", "r", " ", "=", " ", "0", ".", "1", "\n", "A", " ", "=", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "[", "1", ".", "0", ",", " ", "d", "t", "_", "l", "q", "r", "]", ",", " ", "[", "0", ".", "0", ",", " ", "1", ".", "0", "]", "]", ")", "\n", "B", " ", "=", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "[", "0", ".", "0", "]", ",", " ", "[", "d", "t", "_", "l", "q", "r", "]", "]", ")", "\n", "Q", " ", "=", " ", "n", "p", ".", "d", "i", "a", "g", "(", "[", "0", ".", "0", ",", " ", "1", "0", "0", ".", "0", "]", ")", "\n", "R", " ", "=", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "[", "1", "e", "-", "2", "]", "]", ")", "\n", "K", " ", "=", " ", "l", "q", "r", "_", "g", "a", "i", "n", "(", "A", ",", " ", "B", ",", " ", "Q", ",", " ", "R", ")", "\n", "\n", "l", "q", "r", "_", "n", "o", "d", "e", ".", "w", "r", "i", "t", "e", "_", "R", "O", "S", "N", "o", "d", "e", "_", "t", "o", "_", "r", "c", "l", "p", "y", "(", "\n", " ", " ", " ", " ", "\"", "l", "q", "r", "_", "n", "o", "d", "e", ".", "p", "y", "\"", ",", "\n", " ", " ", " ", " ", "n", "o", "d", "e", "_", "n", "a", "m", "e", "=", "\"", "l", "q", "r", "_", "c", "o", "n", "t", "r", "o", "l", "l", "e", "r", "\"", ",", "\n", " ", " ", " ", " ", "s", "t", "a", "t", "i", "c", "_", "p", "a", "r", "a", "m", "s", "=", "{", "\"", "K", "\"", ":", " ", "K", "}", ",", "\n", " ", " ", " ", " ", "i", "n", "i", "t", "i", "a", "l", "_", "i", "n", "p", "u", "t", "s", "=", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "x", "h", "a", "t", "k", "\"", ":", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "0", ".", "0", ",", " ", "0", ".", "0", "]", ")", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "x", "r", "e", "f", "k", "\"", ":", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "0", ".", "0", ",", " ", "0", ".", "0", "]", ")", ",", "\n", " ", " ", " ", " ", "}", ",", "\n", ")" ], "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "lqr-backlink", "metadata": {}, "source": [ "\u2190 [f_h Functions](../controllers.ipynb)" ] } ] }