{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": "# Control Engineering with Python\n\n# Controllability\n\n\ud83d\udc64 [S\u00e9bastien Boisg\u00e9rault](sebastien.boisgerault@minesparis.psl.eu)\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### Control Engineering with Python\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- \ud83d\udcd6 [Documents\n (GitHub)](https://github.com/boisgera/control-engineering-with-python)\n\n- \u00a9\ufe0f [License CC BY 4.0](https://creativecommons.org/licenses/by/4.0/)\n\n- \ud83c\udfe6 [Mines ParisTech, PSL University](https://mines-paristech.eu/)\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## Symbols\n" }, { "cell_type": "markdown", "metadata": {}, "source": "| | | | |\n|-----|-------------|-----|-------------------|\n| \ud83d\udc0d | Code | \ud83d\udd0d | Worked Example |\n| \ud83d\udcc8 | Graph | \ud83e\udde9 | Exercise |\n| \ud83c\udff7\ufe0f | Definition | \ud83d\udcbb | Numerical Method |\n| \ud83d\udc8e | Theorem | \ud83e\uddee | Analytical Method |\n| \ud83d\udcdd | Remark | \ud83e\udde0 | Theory |\n| \u2139\ufe0f | Information | \ud83d\udddd\ufe0f | Hint |\n| \u26a0\ufe0f | Warning | \ud83d\udd13 | Solution |\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udc0d Imports\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "from numpy import *\nimport matplotlib; matplotlib.use(\"nbAgg\")\n%matplotlib notebook\nfrom matplotlib.pyplot import *", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83c\udff7\ufe0f Controllability\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The system $\\dot{x} = f(x,u)$ is **controllable** if\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- for any $t_0 \\in \\mathbb{R}$, $x_0 \\in \\mathbb{R}^n$ and\n $x_f \\in \\mathbb{R}^n$,\n\n- there are $t_f > t_0$ and $u: [t_0, t_f] \\to \\mathbb{R}^m$ such that\n\n- the solution $x(t)$ such that $x(t_0)=x_0$ satisfies\n\n $$\n x(t_f) = x_f.\n $$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "![](images/static/controllability.svg)\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83c\udff7\ufe0f Admissible Trajectory\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Let $x_r$ be a reference trajectory of the system state: $$\n x_r(t), \\, t\\in[t_0, t_f].\n $$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "It is **admissible** if there is a function $u_r(t)$, $t \\in [t_0, t_f]$\nsuch that the solution $x$ of the IVP $$\n \\dot{x} = f(x, u_r), \\; x(t_0) = x_r(t_0)\n $$ is the function $x_r$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udd0d Car\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The position $d$ (in meters) of a car of mass $m$ (in kg) on a straight\nroad is governed by\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nm \\ddot{d} = u\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "where $u$ the force (in Newtons) generated by its motor.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The car is initially at the origin of a road and motionless.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "We would like to cross the end of the road (location $d_f > 0$) at time\n$t_f > 0$ and speed $v_f$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "**Numerical values:**\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- $m=1500 \\, \\mbox{kg}$,\n\n- $t_f=10 \\, \\mbox{s}$, $d_f=100 \\, \\mbox{m}$ and\n $v_f=100 \\, \\mbox{km/h}$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### Step 1 \u2013 Trajectory Planning\n" }, { "cell_type": "markdown", "metadata": {}, "source": "We search for a **reference trajectory** for the state\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$x_r(t) = (d_r(t), \\dot{d}_r(t))$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "such that:\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- $d_r(0)=0$, $\\dot{d}_r(0) = 0$,\n\n- $d_r(t_f) = x_f$, $\\dot{d}_r(t_f) = v_f$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### Step 2 \u2013 Admissibility\n" }, { "cell_type": "markdown", "metadata": {}, "source": "We check that this reference trajectory is **admissible**, i.e.\u00a0that we\ncan find a control $u_r(t)$ such that the solution of the IVP is\n$x(t) = x_r(t)$ when $x(0) = x_r(t)$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### Admissible Trajectory\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Here, if $d_r$ is smooth and if we apply the control\n$u(t) = m\\ddot{d}_r(t)$,\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n m \\frac{d^2}{dt^2} (d-d_r) = 0, \n $$ $$\n (d-d_r)(0) = 0, \\; \\frac{d}{dt} (d-d_r)(0)= 0.\n $$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Thus, $d(t) = d_r(t)$ \u2013 and thus $\\dot{d}(t) = \\dot{d}_r(t)$ \u2013 for every\n$t\\geq 0$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### Reference Trajectory\n" }, { "cell_type": "markdown", "metadata": {}, "source": "We can find $d_r$ as a third-order polynomial in $t$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nd_r(t) = \\alpha t^3 + \\beta t^2 + \\gamma t + \\delta\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "with\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\alpha = \\frac{v_f}{t_f^2}- 2\\frac{d_f}{t_f^3}, \\;\n\\beta = 3\\frac{d_f}{t_f^2} - \\frac{v_f}{tf},\\;\n\\gamma=0, \\; \\delta=0.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "(equivalently, with $u(t)$ as an affine function of $t$).\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udc0d Constants\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "m = 1500.0\nxf = 100.0\nvf = 100.0 * 1000 / 3600 # m/s\ntf = 10.0\nalpha = vf/tf**2 - 2*xf/tf**3\nbeta = 3*xf/tf**2 - vf/tf", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udc0d State & Input Evolution\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "def x(t):\n return alpha * t**3 + beta * t**2\ndef d2_x(t):\n return 6 * alpha * t + 2 * beta\ndef u(t):\n return m * d2_x(t)", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udc0d \ud83d\udcbb Simulation\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "y0 = [0.0, 0.0]\ndef fun(t, y):\n x, d_x = y\n d2_x = u(t) / m\n return [d_x, d2_x]\nresult = solve_ivp(\n fun, [0.0, tf], y0, dense_output=True\n)", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udcca Graph of the Distance\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "figure()\nt = linspace(0, tf, 1000)\nxt = result[\"sol\"](t)[0]\nplot(t, xt)\ngrid(True); xlabel(\"$t$\"); title(\"$d(t)$\")", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udcca Graph of the velocity\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "figure()\nvt = result[\"sol\"](t)[1]\nplot(t, 3.6 * vt)\ngrid(True); xlabel(\"$t$\")\ntitle(r\"$\\dot{d}(t)$ km/h\")", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83e\udde9 Non-admissible trajectory\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Let $\\dot{x} = A x + B u$ with $x\\in \\mathbb{R}^2$, $u \\in \\mathbb{R}$,\n$$\nA = \\left[\n\\begin{array}{cc}\n0 & 1 \\\\\n0 & 0\n\\end{array}\n\\right], \\; \nB = \\left[\n\\begin{array}{cc}\n0 \\\\\n1\n\\end{array}\n\\right].\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83e\udde0\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Find a smooth reference trajectory $x_r(t)$, $t\\in[0, 1]$ which is not\nadmissible.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udd13 Non-admissible trajectory\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The first line of the vector equation $\\dot{x} = A x + Bu$ is\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\dot{x}_1 = x_2.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Any trajectory $x_r$ that does not satisfy this equation is not\nadmissible; for example\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nx_r(t) :=\n\\left[ \n \\begin{array}{c}\n 0 \\\\\n 1\n \\end{array}\n\\right].\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83e\udde9 Pendulum\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Consider the pendulum with dynamics:\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n m \\ell^2 \\ddot{\\theta} + b \\dot{\\theta} + mg \\ell \\sin \\theta = u\n $$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83e\udde0 \ud83e\uddee\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Find a smooth reference trajectory that leads the pendulum from the\nbottom configuration\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\theta(0)=0, \\; \\dot{\\theta}(0) = 0\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "to the top configuration\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\theta(t_f) = \\pi, \\; \\dot{\\theta}(t_f) = 0.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 2. \ud83e\udde0 \ud83e\uddee\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Show that the reference trajectory is admissible and compute the\ncorresponding input $u_r(t)$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 3. \ud83d\udcbb \ud83e\udde0\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Simulate the result and visualize the solution.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "What should theoretically happen at $t=t_f$ if $u(t)=0$ is applied when\n$t \\geq t_f$? What does happen in reality ? Why ? How can we mitigate\nthis issue?\n" }, { "cell_type": "markdown", "metadata": {}, "source": "**Numerical Values:**\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nm = 1.0, \\, l = 1.0, \\, b = 0.1,\\, g = 9.81, \\, t_f = 10.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udd13 Pendulum\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Since the initial and final conditions form a set of 4 equations, we\nsearch for an admissible trajectory defined by a 3rd-order polynomial\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\theta_r(t) := a t_f^3 + bt_f^2 + c t_f +d\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "with unknown coefficients $a, b, c, d$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The initial conditions $\\theta_r(0)=0$ and $\\dot{\\theta}_r(0)=0$ are\nequivalent to\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nb = 0, \\; c= 0.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The final condition $\\theta_r(t_f) = \\pi$ is equivalent to\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nat_f^3 + bt_f^2 = \\pi\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "and $\\dot{\\theta}_r(t_f) = 0$ to\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n3a t_f^2 + 2b t_f = 0.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The latter equation can be transformed into\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nb = -\\frac{3}{2} a t_f\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "and the former becomes\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nat_f^3 - \\frac{3}{2} a t_f^3 = \\pi\n\\; \\Leftrightarrow \\;\na = -\\frac{2\\pi}{t_f^3}\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "and thus $b = \\frac{3\\pi}{t_f^2}.$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Finally, the following trajectory $(\\theta_r(t),\\dot{\\theta}_t(t))$ is\nsmooth and meets the required initial and final conditions:\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\theta_r(t) = -2\\pi \\left(\\frac{t}{t_f}\\right)^3 + 3\\pi \\left(\\frac{t}{t_f}\\right)^2\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\dot{\\theta}_r(t) = -\\frac{6\\pi}{t_f} \\left(\\frac{t}{t_f}\\right)^2 + \\frac{6\\pi}{t_f} \\left(\\frac{t}{t_f}\\right)\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 2. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "By construction our reference trajectory meets the initial condition.\nThere is a unique input that makes the solution follows this reference;\nits is defined by\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nu_r(t) := m \\ell^2 \\ddot{\\theta}_r(t) + b \\dot{\\theta}_r(t) + mg \\ell \\sin \\theta_r(t) \n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "where\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\ddot{\\theta}_r(t) = -\\frac{12\\pi}{t_f^2} \\left(\\frac{t}{t_f}\\right) + \\frac{6\\pi}{t_f^2}.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 3. \ud83d\udd13\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "m = 1.0\nl = 1.0\nb = 0.1\ng = 9.81 \nt0, tf = 0.0, 10.0", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "def theta_r(t):\n s = t/tf\n return -2*pi*s**3 + 3*pi*s**2\ndef dtheta_r(t):\n s = t/tf\n return -6*pi/tf*s**2 + 6*pi/tf*s", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "def d2theta_r(t):\n s = t/tf\n return -12*pi/(tf*tf)*s + 6*pi/(tf*tf)\ndef u_r(t):\n return (m*l*l*d2theta_r(t) + \n b*dtheta_r(t) + \n m*g*l*sin(theta_r(t)))", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "def fun(t, theta_dtheta):\n theta, dtheta = theta_dtheta\n d2theta = ((-b*dtheta - m*g*l*sin(theta) + u_r(t)) \n / (m * l * l))\n return dtheta, d2theta\nt_span = [t0, tf]\ny0 = [0.0, 0.0]", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "r = solve_ivp(fun, t_span, y0, dense_output=True)\nt = linspace(t0, tf, 1000)\nsolt = r[\"sol\"](t)\nthetat, dthetat = solt", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "figure()\nplot(t, theta_r(t), \"k--\", label=r\"$\\theta_r(t)$\")\nplot(t, thetat, label=r\"$\\theta(t)$\")\nyticks([0, 0.5*pi, pi], [\"$0$\", r\"$\\pi/2$\", r\"$\\pi$\"])\ntitle(r\"Simulation of $\\theta(t)$\")\ngrid(True); legend()", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Theoretically, we should have $\\theta(t_f) = \\pi$ and\n$\\dot{\\theta}(t_f)=0$, but the simulation is quite far from that.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Since the top equilibrium is unstable, small (numerical) errors in the\nstate may cause large deviations of the trajectory.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "We may reduce the simulation error thresholds to alleviate this problem.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "rhp = solve_ivp(fun, t_span, y0, \n dense_output=True, \n rtol=1e-6, atol=1e-9)\nt = linspace(t0, tf, 1000)\nsolt_hp = rhp[\"sol\"](t)\nthetat_hp, dthetat_hp = solt_hp", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "figure()\nplot(t, theta_r(t), \"k--\", label=r\"$\\theta_r(t)$\")\nplot(t, thetat, label=r\"$\\theta(t)$ (standard)\")\nplot(t, thetat_hp, label=r\"$\\theta(t)$ (low error)\")\nyticks([0, 0.5*pi, pi], [\"$0$\", r\"$\\pi/2$\", r\"$\\pi$\"])\ntitle(r\"Simulation of $\\theta(t)$\")\ngrid(True); legend()", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udc8e Controllability of LTI systems\n" }, { "cell_type": "markdown", "metadata": {}, "source": "A system $\\dot{x} = Ax + Bu$ is controllable iff\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- from the origin $x_0 = 0$ at $t_0=0$,\n\n- we can reach any state $x_f \\in \\mathbb{R}^n$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83c\udff7\ufe0f \ud83d\udc8e Kalman Criterion\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The system $\\dot{x} = Ax+Bu$ ($x\\in \\mathbb{R}^n$) is controllable iff:\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n \\mathrm{rank} \\, \\left[B, AB, \\dots, A^{n-1} B\\right] = n\n $$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$[B, \\dots, A^{n-1}B]$ is the **controllability matrix**.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udd0d Controllability Matrix\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nA = \n\\left[ \n\\begin{array}{ccc}\n0 & 1 & 0 \\\\\n0 & 0 & 1 \\\\\n0 & 0 & 0\n\\end{array}\n\\right], \\;\nB = \n\\left[ \n\\begin{array}{c}\n0 \\\\\n0 \\\\\n1 \n\\end{array}\n\\right]\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udc0d Computation\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "def KCM(A, B):\n n = shape(A)[0]\n mp = matrix_power\n cs = column_stack\n return cs([mp(A, k) @ B for k in range(n)])", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udc0d LTI System\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "n = 3 \nA = zeros((n, n))\nfor i in range(0, n-1):\n A[i,i+1] = 1.0\n\nB = zeros((n, 1))\nB[n-1, 0] = 1.0", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### \ud83d\udc0d Rank Condition\n" }, { "cell_type": "code", "metadata": {}, "outputs": [], "source": "C = KCM(A, B)\n\nC_expected = [[0, 0, 1], [0, 1, 0], [1, 0, 0]]\nassert_almost_equal(C, C_expected)\n\nassert matrix_rank(C) == n", "execution_count": null }, { "cell_type": "markdown", "metadata": {}, "source": "## \u26a0\ufe0f Warnings\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- This implementation of `KCM` is not optimized: fewer computations\n are achievable using the identity:\n\n $$\n A^n B = A \\times (A^{n-1}B).\n $$\n\n- Rank computations are subject to (catastrophic) numerical errors;\n sensitivity analysis or symbolic computations may alleviate the\n problem.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83e\udde9 Fully Actuated System\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Consider $\\dot{x} = A x + Bu$ with\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- $x \\in \\mathbb{R}^n$,\n\n- $u \\in\\mathbb{R}^m$ and,\n\n- $\\mathrm{rank} \\, B = n$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83e\udde0\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Show that $m \\geq n$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 2. \ud83e\uddee\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Is the system controllable ?\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 3. \ud83e\udde0 \ud83e\uddee\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Given $x_0$, $x_f$ and $t_f > 0$, show that any smooth trajectory that\nleads from $x_0$ at time $t_0$ to $x_f$ at time $t_f$ is admissible.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udd13 Fully Actuated System\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The shape of $B$ is $n \\times m$, thus\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\mbox{rank} \\, B \\leq \\min(n, m).\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "By assumption $\\mbox{rank} \\, B = n$, thus $n \\leq m$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 2. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The system is controllable since\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\mbox{rank} \\, [B, AB, \\dots] \\leq \\mbox{rank} \\, B = n.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 3. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Since $\\mbox{rank} \\, B = n$, matrix $B$ contains a $n \\times n$\ninvertible matrix $R$. Let\u2019s assume for the sake of simplicity that $R$\nis made of the first $n$ columns of $B$ and let $$\nS: = \n\\left[\n \\begin{array}{c}\n R^{-1} \\\\\n 0\n \\end{array}\n\\right] \\in \\mathbb{R}^{m \\times n}.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Then by construction $B \\times S = I \\in \\mathbb{R}^{n\\times n}$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "If we define $u$ as a function of some auxiliary control\n$v \\in \\mathbb{R}^n$ by $$\nu(t) = S v(t),\n$$ then $$\n\\dot{x} = A x + v.\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "To join $x_0$ at $t_0$ and $x_f$ at $t_f$, we can apply the control $$\nv(t) := -A x_r(t) + \\dot{x}_r(t)\n$$ where $$\nx_r(t) := x_0 + \\frac{t-t_0}{t_f-t_0} (x_f - x_0).\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Indeed, that leads to $$\n\\dot{x}(t) = A x(t) - A x_r(t) + \\dot{x}_r(t), \\; x(t_0) = x_r(t_0)\n$$ or if we denote $e := x - x_r$, $$\n\\dot{e}(t) = Ae(t), \\; e(t_0) =0\n$$ and thus yields $x(t) = x_r(t)$ for any $t\\geq t_0$.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83e\udde9 Integrator Chain\n" }, { "cell_type": "markdown", "metadata": {}, "source": "![](images/static/integrator-chain.svg)\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\\dot{x}_n = u, \\, \\dot{x}_{n-1} = x_n, \\, \\cdots \\,, \\dot{x}_1 = x_2.$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83e\udde0 \ud83e\uddee\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Show that the system is controllable\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udd13 Integrator Chain\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "We have\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nA = \\left[\n \\begin{array}{c}\n 0 & 1 & 0 & \\cdots & 0\\\\\n 0 & 0 & 1 & \\cdots & 0 \\\\\n 0 & 0 & \\cdots & \\ddots & \\vdots \\\\\n \\vdots & \\vdots & \\vdots & \\vdots & 1 \\\\\n 0 & 0 & 0 & 0 & 0 \\\\\n \\end{array}\n \\right],\n \\;\n B =\\left[\n \\begin{array}{c}\n 0 \\\\\n 0 \\\\\n \\vdots \\\\\n 0 \\\\\n 1\n \\end{array}\n \\right]\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Thus,\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nB =\\left[\n\\begin{array}{c}\n0 \\\\\n0 \\\\\n\\vdots \\\\\n0 \\\\\n1\n\\end{array}\n\\right], \\;\nAB =\\left[\n\\begin{array}{c}\n0 \\\\\n0 \\\\\n\\vdots \\\\\n1 \\\\\n0\n\\end{array}\n\\right], \n\\, \\dots, \\,\nA^{n-1} B =\n\\left[\n\\begin{array}{c}\n1 \\\\\n0 \\\\\n\\vdots \\\\\n0 \\\\\n0\n\\end{array}\n\\right].\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The controllability matrix has full rank and the system is controllable.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83e\udde9 Heat Equation\n" }, { "cell_type": "markdown", "metadata": {}, "source": "![](images/static/heat-simple.svg)\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- $d T_1/dt = u + (T_2 - T_1)$\n\n- $d T_2/dt = (T_1 - T_2) + (T_3 - T_2)$\n\n- $d T_3/dt = (T_2 - T_3) + (T_4 - T_3)$\n\n- $d T_4/dt = (T_3 - T_4)$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83e\udde0 \ud83e\uddee\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Show that the system is controllable.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 2. \ud83e\udde0 \ud83e\uddee\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Assume now that the four cells are organized as a square.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "- Is the system still controllable?\n\n- Why?\n\n- How could you solve this problem?\n" }, { "cell_type": "markdown", "metadata": {}, "source": "## \ud83d\udd13 Heat Equation\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 1. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "We have\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nA = \n\\left[\n\\begin{array}{rrrr}\n-1 & 1 & 0 & 0 \\\\\n 1 & -2 & 1 & 0 \\\\\n 0 & 1 & -2 & 1 \\\\\n 0 & 0 & 1 & -1\n\\end{array}\n\\right], \\;\nB = \n\\left[\n\\begin{array}{r}\n1 \\\\\n0 \\\\\n0 \\\\\n0\n\\end{array}\n\\right].\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nA B =\n\\left[\n\\begin{array}{r}\n-1 \\\\\n1 \\\\\n0 \\\\\n0\n\\end{array}\n\\right], \\;\nA^2 B =\n\\left[\n\\begin{array}{r}\n2 \\\\\n-3 \\\\\n1 \\\\\n0\n\\end{array}\n\\right], \\;\nA^3 B =\n\\left[\n\\begin{array}{r}\n-5 \\\\\n 9 \\\\\n-5 \\\\\n1\n\\end{array}\n\\right].\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The controllability matrix\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\left[\n\\begin{array}{rrrr}\n 1 & -1 & 2 & -5 \\\\\n 0 & 1 & -3 & 9 \\\\\n 0 & 0 & 1 & -5 \\\\\n 0 & 0 & 0 & 1\n\\end{array}\n\\right]\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "is full rank, thus the system is controllable.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "### 2. \ud83d\udd13\n" }, { "cell_type": "markdown", "metadata": {}, "source": "If the system is organized as a square\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nA = \n\\left[\n\\begin{array}{rrrr}\n-2 & 1 & 1 & 0 \\\\\n 1 & -2 & 0 & 1 \\\\\n 1 & 0 & -2 & 1 \\\\\n 0 & 1 & 1 & -2\n\\end{array}\n\\right], \\;\nB = \n\\left[\n\\begin{array}{r}\n1 \\\\\n0 \\\\\n0 \\\\\n0\n\\end{array}\n\\right].\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The controllability matrix is\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\left[\n\\begin{array}{rrrr}\n 1 & -2 & 6 & -20 \\\\\n 0 & 1 & -4 & 16 \\\\\n 0 & 1 & -4 & 16 \\\\\n 0 & 0 & 2 & -12\n\\end{array}\n\\right]\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "It is **not** full rank since the second and the third rows are equal.\nThus the system is not controllable.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "The lack of controllability is due to symmetry.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "If the initial temperature in cell 2 and 3 are equal, since they play\nsymmetric role in the system, their temperature will be equal for any\nsubsequent time.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "Hence, it will be impossible to reach a state with different temperature\nin cell 2 and 3, no matter what the input is.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "To break this symmetry and restore controllability, we may for example\ntry to add a second independent heat source sink in cell 2.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "This leads to\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\nA = \n\\left[\n\\begin{array}{rrrr}\n-2 & 1 & 1 & 0 \\\\\n 1 & -2 & 0 & 1 \\\\\n 1 & 0 & -2 & 1 \\\\\n 0 & 1 & 1 & -2\n\\end{array}\n\\right], \\;\nB = \n\\left[\n\\begin{array}{rr}\n1 & 0 \\\\\n0 & 1 \\\\\n0 & 0\\\\\n0 & 0\n\\end{array}\n\\right].\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "------------------------------------------------------------------------\n" }, { "cell_type": "markdown", "metadata": {}, "source": "and the controllability matrix\n" }, { "cell_type": "markdown", "metadata": {}, "source": "$$\n\\left[\n\\begin{array}{rrrr}\n1 & 0 & -2 & 1 & 6 & -4 & -20 & 16 \\\\\n 0 & 1 & 1 & -2 & -4 & 6 & 16 & -20\\\\\n 0 & 0 & 1 & 0 & -4 & 2 & 16 & -12 \\\\\n 0 & 0 & 0 & 1 & 2 & -4 & -12 & 16\n\\end{array}\n\\right]\n$$\n" }, { "cell_type": "markdown", "metadata": {}, "source": "which has a full-rank.\n" }, { "cell_type": "markdown", "metadata": {}, "source": "\n" }, { "cell_type": "markdown", "metadata": {}, "source": "\n" }, { "cell_type": "markdown", "metadata": {}, "source": "\n" } ], "metadata": { "kernelspec": { "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.9.7" } }, "nbformat": 4, "nbformat_minor": 5 }