{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "5a744bf4",
   "metadata": {},
   "source": [
    "# Introduction to the Python Control Systems Library (python-control)\n",
    "\n",
    "## Transfer Functions"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fd6d186c",
   "metadata": {},
   "source": [
    "Richard M. Murray, 30 Dec 2022\n",
    "\n",
    "This notebook works through a transfer function-based control design and\n",
    "analysis, corresponding to the planar vertical takeoff and landing (PVTOL) aircraft in Astrom and Murray, Chapter 11.  It is intended to demonstrate the basic functionality of the frequency domain tools in the python-control package."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "989e09b2",
   "metadata": {},
   "source": [
    "We start by importing the packages that we need to carry out control calculations.  The standard packages that we will use are NumPy, Matplotlib, and python-control."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ba36f36",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import the packages needed for the examples included in this notebook\n",
    "import numpy as np               # NumPy (array-based numerical calculations)\n",
    "import matplotlib.pyplot as plt  # MATLAB-like plotting functions\n",
    "import control as ct             # python-control package\n",
    "print(ct.__version__)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cdf19347",
   "metadata": {},
   "source": [
    "### Installation hints\n",
    "\n",
    "If you get an error importing the `control` package, it may be that it is not in your current Python path.  You can fix this by setting the PYTHONPATH environment variable to include the directory where the python-control package is located.  If you are invoking Jupyter from the command line, try using a command of the form\n",
    "\n",
    "    PYTHONPATH=/path/to/control jupyter notebook\n",
    "    \n",
    "If you are using [Google Colab](https://colab.research.google.com), use the following command at the top of the notebook to install the `control` package:\n",
    "\n",
    "    !pip install control\n",
    "    \n",
    "For the examples below, you will need version 0.9.3 or higher of the python-control toolbox.  You can find the version number using the command\n",
    "\n",
    "    print(ct.__version__)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6cc56644",
   "metadata": {},
   "source": [
    "## System definition\n",
    "\n",
    "To control the lateral dynamics of the vectored thrust aircraft, we make use of an “inner/outer” loop design methodology, as illustrated below.\n",
    "\n",
    "![PVTOL block diagram](https://fbswiki.org/wiki/images/3/3b/Pvtol-blockdiag.png)\n",
    "\n",
    "This diagram shows the process dynamics and controller divided into two components: an inner loop consisting of the roll dynamics and controller and an outer loop consisting of the lateral position dynamics and controller.  The \"inner loop\" transfer function $P_\\text{i}$ (representing the roll dynamics) and an \"outer loop\" transfer function $P_\\text{o}$ (representing the position dynamics) are given in FBS2e, Exercise 9.11:\n",
    "\n",
    "$$\n",
    "P_\\text{i} = \\dfrac{r}{J s^2}, \\qquad P_\\text{o} = \\dfrac{1}{m s^2 + c s}\n",
    "$$\n",
    "\n",
    "In python-control, transfer functions are created using the `tf` function, which takes as arguments the coefficients of the numerator and denominator polynomials of the transfer function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b8abc38b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# System parameters\n",
    "m = 4               # mass of aircraft\n",
    "J = 0.0475          # inertia around pitch axis\n",
    "r = 0.25            # distance to center of force\n",
    "g = 9.8             # gravitational constant\n",
    "c = 0.05            # damping factor (estimated)\n",
    "\n",
    "# Transfer functions for dynamics\n",
    "Pi = ct.tf([r], [J, 0, 0])  # inner loop (roll)\n",
    "Po = ct.tf([1], [m, c, 0])  # outer loop (position)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dd60845d",
   "metadata": {},
   "source": [
    "## Control design\n",
    "\n",
    "The approach that we take is to design a controller $C_\\text{i}$ for the inner loop so that the resulting closed loop system Hi assures that the roll angle $\\theta$ follows its reference θr quickly and accurately. We then design a controller for the lateral position $y$ that uses the approximation that we can directly control the roll angle as an input $\\theta$ to the dynamics controlling the position. Under the assumption that the dynamics of the roll controller are fast relative to the desired bandwidth of the lateral position control, we can then combine the inner and outer loop controllers to get a single controller for the entire system. As a performance specification for the entire system, we would like to have zero steady-state error in the lateral position, a bandwidth of approximately 1 rad/s, and a phase margin of $45^\\circ$.\n",
    "\n",
    "### Inner loop (pitch dynamics)\n",
    "\n",
    "For the inner loop, we choose our design specification to provide the outer loop with accurate and fast control of the roll. The inner loop dynamics are given by\n",
    "\n",
    "$$\n",
    "P_i(s) = H_{\\theta, u_1}(s) = \\dfrac{r}{J s^2}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c0706101",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Bode plot for the open loop process\n",
    "ct.bode_plot(Pi)\n",
    "plt.suptitle(\"Inner loop process dynamics\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "89e3f8a4",
   "metadata": {},
   "source": [
    "We choose the desired bandwidth to be 10 rad/s (10 times that of the outer loop) and the low-frequency error to be no more than 5%. This specification is satisfied using the lead compensator of FBS, Example 12.5, so we choose\n",
    "\n",
    "$$\n",
    "C_i (s) = k\\dfrac{s + a}{s + b}, \\qquad a = 2, b = 50, k = 200. \n",
    "$$\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e90ef27f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Design a simple lead controller for the system\n",
    "k, a, b = 200, 2, 50\n",
    "Ci = k * ct.tf([1, a], [1, b])  # lead compensator\n",
    "Li = Pi * Ci\n",
    "\n",
    "# Bode plot for the loop transfer function, with margins\n",
    "ct.bode_plot(Li)\n",
    "plt.suptitle(\"Loop transfer function, inner loop\")\n",
    "\n",
    "# Compute out the gain and phase margins\n",
    "gm, pm, wcg, wcp = ct.margin(Li)\n",
    "print(\"Gain margin:\", gm)\n",
    "print(\"Phase margin:\", pm)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "60b62b12",
   "metadata": {},
   "source": [
    "The closed loop dynamics for the system satisfy\n",
    "\n",
    "$$\n",
    "  H_\\text{i} = \\frac{C_\\text{i}}{1 + C_\\text{i} P_\\text{i}}\n",
    "    - mg \\frac{C_\\text{i} P_\\text{i}}{1 + C_\\text{i} P_\\text{i}}\n",
    "  = \\frac{C_\\text{i} (1 - m g P_\\text{i})}{1 + C_\\text{i} P_\\text{i}}.\n",
    "$$\n",
    "\n",
    "A plot of the magnitude of this transfer function is shown below, and we see that Hi ≈ −mg = −39.2 is a good approximation up to 10 rad/s."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e533f065",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute out the actual transfer function from u1 to v1 (see L8.2 notes)\n",
    "# Hi = Ci*(1-m*g*Pi)/(1+Ci*Pi)\n",
    "Hi = ct.parallel(ct.feedback(Ci, Pi), -m * g *ct.feedback(Ci * Pi, 1))\n",
    "\n",
    "ct.bode_plot(Hi)\n",
    "plt.suptitle(\"$H_{v_1, u_1}$\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "49002dcd",
   "metadata": {},
   "source": [
    "### Outer loop (lateral dynamics)\n",
    "\n",
    "To design the outer loop controller, we assume the inner loop roll control is perfect, so that we can take θr as the input to our lateral dynamics. Using the block diagram at the top of the notebook, the outer loop dynamics can be written as\n",
    "\n",
    "$$\n",
    "P(s) = H_\\text{i}(0) P_\\text{o}(s) = \\frac{H_\\text{i}(0)}{m s^2 + cs},\n",
    "$$\n",
    "\n",
    "where we replace $H_\\text{i}(s)$ with $H_\\text{i}(0)$ to reflect our approximation that the inner loop will eventually track our commanded input. Of course, this approximation may not be valid, and so we must verify this when we complete our design."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1c150248",
   "metadata": {},
   "source": [
    "Our control goal is now to design a controller that gives zero steady-state error in $y$ for a step input and has a bandwidth of 1 rad/s. The outer loop process dynamics are given by a double integrator, and we can again use a simple lead compensator to satisfy the specifications. We also choose the design such that the loop transfer function for the outer loop has $|L_\\text{o}| < 0.1$ for $\\omega  > 10$ rad/s, so that the Hi high-frequency dynamics can be neglected. We choose the controller to be of the form\n",
    "\n",
    "$$\n",
    "C_\\text{o}(s) = -k_\\text{o} \\frac{s+a_\\text{o}}{s+b_\\text{o}},\n",
    "$$\n",
    "\n",
    "with the negative sign to cancel the negative sign in the process\n",
    "dynamics.  To find the location of the poles, we note that the phase\n",
    "lead flattens out at approximately $b_\\text{o}/10$.  We desire phase\n",
    "lead at crossover, and we desire the crossover at\n",
    "$\\omega_\\text{gc}= 1$ rad/sec, so this gives\n",
    "$b_\\text{o} = 10$.  To ensure that we have adequate phase lead, we\n",
    "must choose $a_\\text{o}$ such that\n",
    "$b_\\text{o}/10 < 10 a_\\text{o} < b_\\text{o}$, which implies that\n",
    "$a_\\text{o}$ should be between 0.1 and 1.  We choose\n",
    "$a_\\text{o} = 0.3$.  Finally, we need to set the gain of the system\n",
    "such that at the desired crossover frequency the loop gain has\n",
    "magnitude 1 or more.  A simple calculation shows that $k_\\text{o} = 2$\n",
    "satisfies this objective.  Thus, the final outer loop controller\n",
    "becomes\n",
    "\n",
    "$$\n",
    "  C_\\text{o}(s) = -2 \\frac{s+0.3}{s+10}.\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ce46ca3d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now design the lateral control system\n",
    "a, b, K = 0.02, 5, 2\n",
    "Co = -K * ct.tf([1, 0.3], [1, 10])  # another lead compensator\n",
    "Lo = -m*g*Po*Co\n",
    "\n",
    "ct.bode_plot(Lo);  # margin(Lo)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "22d6c1c4",
   "metadata": {},
   "source": [
    "### Combined dynamics\n",
    "\n",
    "Finally, we can combine the inner and outer loop controllers and verify that the system has the desired closed loop performance.  The Bode and Nyquist plots with inner and outer loop controllers are shown below, and we see that the specifications are satisfied."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "83f29f40",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Finally compute the real outer-loop loop gain + responses\n",
    "L = Co * Hi * Po\n",
    "S = ct.feedback(1, L)\n",
    "T = ct.feedback(L, 1)\n",
    "\n",
    "# Compute stability margins\n",
    "gm, pm, wgc, wpc = ct.margin(L)\n",
    "print(\"Gain margin: %g at %g\" % (gm, wgc))\n",
    "print(\"Phase margin: %g at %g\" % (pm, wpc))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5d4a0827",
   "metadata": {},
   "source": [
    "#### Bode plot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "06daefae",
   "metadata": {},
   "outputs": [],
   "source": [
    "ct.bode_plot(L, np.logspace(-4, 3))\n",
    "\n",
    "# Add crossover line to the magnitude plot\n",
    "#\n",
    "# Note: in matplotlib before v2.1, the following code worked:\n",
    "#\n",
    "#   plt.subplot(211); hold(True);\n",
    "#   loglog([1e-4, 1e3], [1, 1], 'k-')\n",
    "#\n",
    "# In later versions of matplotlib the call to plt.subplot will clear the\n",
    "# axes and so we have to extract the axes that we want to use by hand.\n",
    "# In addition, hold() is deprecated so we no longer require it.\n",
    "#\n",
    "for ax in plt.gcf().axes:\n",
    "    if ax.get_label() == 'control-bode-magnitude':\n",
    "        break\n",
    "ax.semilogx([1e-4, 1e3], 20*np.log10([1, 1]), 'k-')\n",
    "\n",
    "#\n",
    "# Replot phase starting at -90 degrees\n",
    "#\n",
    "# Get the phase plot axes\n",
    "for ax in plt.gcf().axes:\n",
    "    if ax.get_label() == 'control-bode-phase':\n",
    "        break\n",
    "\n",
    "# Recreate the frequency response and shift the phase\n",
    "mag, phase, w = ct.freqresp(L, np.logspace(-4, 3))\n",
    "phase = phase - 360\n",
    "\n",
    "# Replot the phase by hand\n",
    "ax.semilogx([1e-4, 1e3], [-180, -180], 'k-')\n",
    "ax.semilogx(w, np.squeeze(phase), 'b-')\n",
    "ax.axis([1e-4, 1e3, -360, 0])\n",
    "plt.xlabel('Frequency [deg]')\n",
    "plt.ylabel('Phase [deg]');"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "19daa7dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "#### Nyquist plot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6adf2984",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Nyquist plot for complete design\n",
    "ct.nyquist_plot(L)\n",
    "\n",
    "# Add a box in the region we are going to expand\n",
    "plt.plot([-2, -2, 1, 1, -2], [-4, 4, 4, -4, -4], 'r-')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a50a224b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Expanded region\n",
    "ct.nyquist_plot(L)\n",
    "plt.axis([-2, 1, -4, 4])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3868c2b8",
   "metadata": {},
   "source": [
    "#### Gang of Four\n",
    "\n",
    "The gain curves of the Gang of Four show that the transfer functions\n",
    "between all inputs and outputs are reasonable.  The sensitivity to load disturbances $PS$ is large at low frequency because\n",
    "the controller does not have integral action."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b3b21199",
   "metadata": {},
   "outputs": [],
   "source": [
    "ct.gangof4_plot(Hi * Po, Co)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fcf47706",
   "metadata": {},
   "source": [
    "#### Step response\n",
    "\n",
    "The step response for the system shows that we obtain a fast transition in the lateral position of the aircraft, with small overshoot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d153d895",
   "metadata": {},
   "outputs": [],
   "source": [
    "Tvec, Yvec = ct.step_response(T, np.linspace(0, 20))\n",
    "plt.plot(Tvec, Yvec)\n",
    "plt.xlabel(\"Time $t$ [sec]\")\n",
    "plt.ylabel(\"Laternal position $y$ [m]\")\n",
    "plt.suptitle(\"Step response for the closed loop system\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "78b0a530",
   "metadata": {},
   "source": [
    "#### Pole/zero map"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df4e7bc3",
   "metadata": {},
   "outputs": [],
   "source": [
    "P, Z = ct.pzmap(T, plot=True, grid=True)\n",
    "print(\"Closed loop poles and zeros: \", P, Z)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cbeabd11",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}