{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "af1717f2",
   "metadata": {},
   "source": [
    "# LQR Tracking Example\n",
    "\n",
    "Richard M. Murray, 25 Jan 2022\n",
    "\n",
    "This example uses a linear system to show how to implement LQR based tracking and some of the tradeoffs between feedfoward and feedback.  Integral action is also implemented."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50d5c4d3",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a23d6f89",
   "metadata": {},
   "source": [
    "## System definition\n",
    "\n",
    "We use a simple linear system to illustrate the concepts.  This system corresponds to the linearized lateral dynamics of a vehicle driving down a road at 10 m/s."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b5923c88",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define a simple linear system that we want to control\n",
    "sys = ct.ss([[0, 10], [-1, 0]], [[0], [1]], np.eye(2), 0, name='sys')\n",
    "print(sys)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dba5ea2b",
   "metadata": {},
   "source": [
    "## Controller design\n",
    "\n",
    "We start by defining the equilibrium point that we plan to stabilize."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "874c1479",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define the desired equilibrium point for the system\n",
    "x0 = np.array([2, 0])\n",
    "u0 = np.array([2])\n",
    "Tf = 4"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "99f036ea",
   "metadata": {},
   "source": [
    "Then construct a simple LQR controller (gain matrix) and create the controller + closed loop system models:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ce6a230",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Construct an LQR controller for the system\n",
    "K, _, _ = ct.lqr(sys, np.eye(sys.nstates), np.eye(sys.ninputs))\n",
    "ctrl, clsys = ct.create_statefbk_iosystem(sys, K)\n",
    "print(ctrl)\n",
    "print(clsys)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5c711b56",
   "metadata": {},
   "source": [
    "Note that the name of the second system is `u[0]`. This is a bug in control-0.9.3 that will be fixed in a [future release](https://github.com/python-control/python-control/pull/849)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "84422c3f",
   "metadata": {},
   "source": [
    "## System simulations\n",
    "\n",
    "### Baseline controller\n",
    "\n",
    "To see how the baseline controller performs, we ask it to track a step change in (xd, ud):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b763b91b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the step response with respect to the reference input\n",
    "tvec = np.linspace(0, Tf, 100)\n",
    "xd = x0\n",
    "ud = u0\n",
    "\n",
    "# U = np.hstack([xd, ud])\n",
    "U = np.outer(np.hstack([xd, ud]), np.ones_like(tvec))\n",
    "time, output = ct.input_output_response(clsys, tvec, U)\n",
    "plt.plot(time, output[0], time, output[1])\n",
    "plt.plot([time[0], time[-1]], [xd[0], xd[0]], '--');\n",
    "plt.legend(['x[0]', 'x[1]']);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "84ee7635",
   "metadata": {},
   "source": [
    "### Disturbance rejection\n",
    "\n",
    "We add a disturbance to the system by modifying ud (since this enters directly at the system input u)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1ecbb3a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Resimulate with a disturbance input\n",
    "delta = 0.5\n",
    "U = np.outer(np.hstack([xd, ud + delta]), np.ones_like(tvec))\n",
    "time, output = ct.input_output_response(clsys, tvec, U)\n",
    "plt.plot(time, output[0], time, output[1])\n",
    "plt.plot([time[0], time[-1]], [xd[0], xd[0]], '--')\n",
    "plt.legend(['x[0]', 'x[1]']);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ea2d1c59",
   "metadata": {},
   "source": [
    "We see that this leads to steady state error, since some amount of system error is required to generate the force to offset the disturbance."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "84a9e61c",
   "metadata": {},
   "source": [
    "### Integral feedback\n",
    "\n",
    "A standard approach to compensate for constant disturbances is to use integral feedback.  To do this, we have to decide what output we want to track and create a new controller with integral feedback.\n",
    "\n",
    "We do this by creating an \"augmented\" system that includes the dynamics of the process along with the dynamics of the controller (= integrators for the errors that we choose):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ee2ecc51",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create a controller with integral feedback\n",
    "C = np.array([[1, 0]])\n",
    "\n",
    "# Define an augmented state space for use with LQR\n",
    "A_aug = np.block([\n",
    "    [sys.A, np.zeros((sys.nstates, 1))], \n",
    "    [C, 0]\n",
    "])\n",
    "B_aug = np.vstack([sys.B, 0])\n",
    "print(\"A =\", A_aug, \"\\nB =\", B_aug)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "463d9b85",
   "metadata": {},
   "source": [
    "Now generate an LQR controller for the the augmented system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3dd3479f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create an LQR controller for the augmented system\n",
    "K_aug, _, _ = ct.lqr(\n",
    "    A_aug, B_aug, np.diag([1, 1, 1]), np.eye(sys.ninputs))\n",
    "print(K_aug)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "19bb6592",
   "metadata": {},
   "source": [
    "We can think about this gain as `K_aug = [K, ki]` and the resulting contoller becomes\n",
    "\n",
    "$$\n",
    "u = u_\\text{d} - K(x - x_\\text{d}) - k_\\text{i} \\int_0^t (y - y_\\text{d})\\, d\\tau.\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e183a822",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Construct an LQR controller for the system\n",
    "integral_ctrl, sys_integral = ct.create_statefbk_iosystem(sys, K_aug, integral_action=C)\n",
    "print(integral_ctrl)\n",
    "print(sys_integral)\n",
    "\n",
    "# Resimulate with a disturbance input\n",
    "delta = 0.5\n",
    "U = np.outer(np.hstack([xd, ud + delta]), np.ones_like(tvec))\n",
    "time, output = ct.input_output_response(sys_integral, tvec, U)\n",
    "plt.plot(time, output[0], time, output[1])\n",
    "plt.plot([time[0], time[-1]], [xd[0], xd[0]], '--')\n",
    "plt.legend(['x[0]', 'x[1]']);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "437487da",
   "metadata": {},
   "source": [
    "## Things to try\n",
    "* Play around with the gains and see whether you can reduce the overshoot (50%!)\n",
    "* Try following more complicated trajectories (hint: linear systems are differentially flat...)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99394ace",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}