Control.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Section IV. DYNAMICS AND CONTROL\n",
    "    \n",
    "# Chapter 15. Stabilizing Controlled Dynamical Systems"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this chapter we return to the question of control, with a given\n",
    "dynamical system with equations of motion $\\dot{x}= f(x,u)$, with\n",
    "$x\\in \\mathbb{R}^n$ and $u\\in \\mathbb{R}^m$. We now are faced with both\n",
    "the descriptive question of whether a given control function $u(x)$ is\n",
    "stable, and the prescriptive question of how to generate a stable\n",
    "closed-loop control. This chapter will provide an overview of classical\n",
    "techniques; in the next chapter we shall discuss model-based and\n",
    "predictive methods.\n",
    "\n",
    "(It should be noted that this book stays entirely in the domain of\n",
    "state-space models, whereas linear control theory texts extensively use\n",
    "the frequency-domain notation induced by the Laplace transform.)\n",
    "\n",
    "Control philosophies\n",
    "--------------------\n",
    "\n",
    "There are many techniques available for controlling a dynamic system.\n",
    "The philosophical approaches behind these techniques can be roughly\n",
    "categorized along three axes: prescriptive/descriptive, model free/model\n",
    "based, and myoptic/predictive.\n",
    "\n",
    "### Descriptive vs prescriptive\n",
    "\n",
    "Descriptive approaches assume that a controller has been given, and the\n",
    "goal is to *analyze* whether the controlled system satisfies some\n",
    "desired properties. Examples of such properties could be stability,\n",
    "disturbance rejection, or some other performance metric. Empirical\n",
    "testing could involve simply simulating or running the system under many\n",
    "operational conditions and observing the results, but the more\n",
    "sophisticated techniques described below provide stronger guarantees.\n",
    "\n",
    "Prescriptive approaches, on the other hand, attempt to *synthesize* a\n",
    "control function given some desired properties. In general synthesis is\n",
    "more difficult than analysis, because any method with performance\n",
    "guarantees on the synthesized controller will also suppose to be able to\n",
    "quantitatively describe the level of performance achieved.\n",
    "\n",
    "### Model-free vs model-based\n",
    "\n",
    "Model-free controllers exhibit operate under no assumptions about the\n",
    "form of $f(x,u)$, besides perhaps general notions of the directions of\n",
    "certain effects, e.g., increasing $u_1$ will result in a general\n",
    "increase in $x_1$. These controllers exhibit some degree of\n",
    "functionality regardless of how accurately the dynamics function has\n",
    "been determined and are hence easier to apply at first glance to a\n",
    "poorly characterized system. The use of *feedback* is essential to guide\n",
    "the state toward a desired point. Usually, they are also less\n",
    "computationally expensive and are almost certainly myopic rather than\n",
    "predictive. However, they typically require extensive tuning to achieve\n",
    "desired levels of performance.\n",
    "\n",
    "One simple example is a standard thermostat, which operates under the\n",
    "assumption that turning on the heat will increase the temperature of a\n",
    "room, and turning on air conditioning will decrease temperature. It is\n",
    "not necessary to know the exact rate at which heating / cooling affects\n",
    "temperature.\n",
    "\n",
    "Model-based controllers by contrast use a mathematical or computational\n",
    "model of $f(x,u)$ in order to output the control $u$. These methods can\n",
    "largely attain superior performance than model-free approaches, but at\n",
    "greater difficulty at identifying a model of $f$, and greater\n",
    "computational expense. An example would be an advanced thermostat which\n",
    "measures internal and external temperatures and regulates the rate of\n",
    "heating / cooling to minimize energy consumption, e.g., not heating the\n",
    "room when the external temperature is higher than desired. At an\n",
    "extreme, a *feedforward*, open-loop controller simply calculates a\n",
    "trajectory of controls $u(t)$ and executes them without the use of state\n",
    "feedback.\n",
    "\n",
    "### Myoptic vs predictive\n",
    "\n",
    "Myoptic (aka greedy) methods reason about controls only looking at a\n",
    "single instant in time. At most, a myopic method will examine the\n",
    "direction of motion in state space after the control is chosen at the\n",
    "current point in time. The time evolution of the system into the future\n",
    "is not explicitly considered.\n",
    "\n",
    "Predictive methods attempt to extrapolate the current state into the\n",
    "future under some hypothesized controls. As a result they must have a\n",
    "model of the dynamics in order to simulate potential solution\n",
    "trajectories. Predictive methods are also typically more computationally\n",
    "expensive than myopic methods, but can obtain much better performance.\n",
    "Usually, these methods have a reconfigurable parameter called the\n",
    "*horizon*, which specifies the duration of time into the future for\n",
    "which predictions will be made. The choice of horizon induces a\n",
    "tradeoff, with longer horizons resulting in increased performance but\n",
    "also greater computational expense."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "PID Control\n",
    "-----------\n",
    "\n",
    "Proportional-Integral-Derivative (PID) control is the workhorse of 1D\n",
    "control systems. It is the most widely studied class of controllers due\n",
    "to its simplicity, and is certainly the first thing one might try on any\n",
    "given new system. Despite the fact that it is model-free and myopic, it\n",
    "can achieve good performance with some amount of manual tuning.\n",
    "\n",
    "### Definition\n",
    "\n",
    "We are given a 1D problem with 1D control. Perhaps little is known about\n",
    "the dynamics $f(x,u)$ except that a positive control $u$ generally acts\n",
    "to increase $x$, and a negative control acts to decrease it. Suppose the\n",
    "desired state (the *setpoint*) is $x_d$.\n",
    "\n",
    "PID control is a sum of three terms, weighted with *gain constants*\n",
    "$k_P$, $k_I$, and $k_D$. The first *proportional term* is defined as:\n",
    "$$u_P = -k_P (x - x_d).$$ The purpose of this term is to act as a\n",
    "\"spring\" that guides the state toward $x_d$\n",
    "([Fig. 1](#fig:PID_P)).\n",
    "\n",
    "*************\n",
    "\n",
    "|(a) Normal value of $k_P$ | (b) $k_P$ reduced by 1/3 |\n",
    "|-----|-----|\n",
    "| ![fig:PID_P_a](figures/control/pid_p_1storder.svg) | ![fig:PID_P_b](figures/control/pid_p_1storder2.svg) |\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:PID_P\">Figure 1</a>.\n",
    "    Error plotted as a function of time for a damped system under\n",
    "proportional (P) control. Left: The P term acts like a spring to pull\n",
    "the error toward 0. Right: The magnitude of the gain $k_P$ influences\n",
    "the stiffness of the \"spring.\" Here $k_P$ is reduced by a factor of 3,\n",
    "leading to a slower recovery. Arrows indicate the magnitude of the $P$\n",
    "term.\n",
    "    </div>\n",
    "\n",
    "*************\n",
    "\n",
    "The term $x - x_d$ is the *error* of the\n",
    "current state, and the gain term $k_P > 0$ dictates the strength of the\n",
    "spring. Larger values of $k_P$ producing stronger (stiffer) springs,\n",
    "which drives the system faster toward 0. Stiffer systems can be more\n",
    "precise than softer ones. However, stiffer systems can sometimes:\n",
    "\n",
    "-   Overshoot a desired setpoint if not tuned correctly.\n",
    "\n",
    "-   Go unstable when used with a discrete-time controller with large\n",
    "    time step.\n",
    "\n",
    "-   Generate extreme controls, which may cause motor damage, loss of\n",
    "    comfort in vehicles, and may be more dangerous to humans and objects\n",
    "    in the environment.\n",
    "\n",
    "\n",
    "If the control consists of purely a proportional term, the system runs a\n",
    "risk of arriving at a nonzero *steady state error*. A steady state is a\n",
    "value $x$ and control $u$ such that the dynamics are 0, that is,\n",
    "$f(x,u) = 0$. We would hope that $x_d$ is a steady state, but this might\n",
    "not hold where there is a biasing effect in the dynamics, such that\n",
    "$f(x_d,0)\\neq 0$. For example, a robot arm holding a load is going to\n",
    "observe a bias due to the force of gravity. More specifically, suppose a\n",
    "proportional controller $u = -k_P x$ with setpoint 0 is given to the the\n",
    "system $\\dot{x} = a x + b + u$ where $a$ and $b$ are nonzero constants.\n",
    "Then, replacing $u$ into the dynamics equation and setting $\\dot{x}=0$,\n",
    "we observe that a steady state exists at the value $x = b/(k_P-a)$. If\n",
    "we wish to drive this error toward 0, we could perhaps make $k_P$ very\n",
    "large\\... but there is a better way!\n",
    "\n",
    "The *integral term* is designed to counteract steady state errors\n",
    "([Fig. 2](#fig:PID_PI)). It is defined as a function of the *integral\n",
    "error* $I(t)$ as follows: $$\\begin{gathered}\n",
    "u_I = -k_I I(t) \\\\\n",
    "I(t) = \\int_0^t (x(t)-x_d(t)) dx \n",
    "\\end{gathered}$$ Setting $u = u_P + u_I$ helps handle steady state error\n",
    "because the integral error is a function that grows positively if the\n",
    "steady state error is positive, and grows negatively if the steady state\n",
    "error is negative. When steady state error is positive, the value of\n",
    "$u_I$ becomes increasingly negative to the point where the error should\n",
    "start to be pushed toward 0. The same is true when steady state error is\n",
    "negative.\n",
    "\n",
    "*************\n",
    "\n",
    "| (a) P control only | (b) PI control |\n",
    "|-----|-----|\n",
    "| ![fig:PID_PI_a](figures/control/pid_p_1storder_bias.svg) | ![fig:PID_PI_b](figures/control/pid_pi_1storder_bias.svg) |\n",
    "\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:PID_PI\">Figure 2</a>.\n",
    "    Left: When the system exhibits bias, using only a P term leads to a\n",
    "steady-state error. Right: Adding an integral (I) term lets the system\n",
    "compensate for bias. Arrows indicate the magnitude of the $P$ term and\n",
    "$I$ term.\n",
    "    </div>\n",
    "\n",
    "*************\n",
    "\n",
    "\n",
    "It is strange to see an integral in a control function, and on first\n",
    "glance it would appear contradictory to our claim that PID control is\n",
    "simple to implement! However, it is straightforward to approximate an\n",
    "integral at a fixed control frequency using a *running total*\n",
    "$$I(t) = I(t-\\Delta t) + \\Delta t (x(t) - x_d(t))$$ where $\\Delta t$ is\n",
    "the control period (the reciprocal of the control frequency). So, the\n",
    "controller simply stores the current value of $I(t)$ and updates it each\n",
    "control period.\n",
    "\n",
    "*************\n",
    "\n",
    "|(a) P control only | (b) PD control |\n",
    "|-------|------|\n",
    "| ![fig:PID_PD_a](figures/control/pid_p.svg) | ![fig:PID_PD_b](figures/control/pid_pd.svg) |\n",
    "\n",
    "\n",
    "<div class=\"figcaption\"><a name=\"fig:PID_PD\">Figure 3</a>.\n",
    "     A controlled system with second order dynamics. (a) Using P control leads\n",
    "to oscillations, and is\n",
    "unstable or lightly damped. (b) the D term adds a damping effect to\n",
    "reduce oscillations. Arrows indicate the magnitude of the $P$ term and\n",
    "$D$ term.\n",
    "</div>\n",
    "\n",
    "*************\n",
    "\n",
    "The final *derivative term* is designed to accommodate second-order\n",
    "systems, which oscillate under the P and I terms only unless they are\n",
    "naturally damped\n",
    "([Fig. 3](#fig:PID_PD)). The D term adds a form of damping which helps\n",
    "stabilize such systems. Moreover, we can specify a *desired velocity*\n",
    "$\\dot{x}_d$ so that the D term can help the system track desired changes\n",
    "faster than the P term alone. The expression for the D term is\n",
    "$$u_D = -k_D (\\dot{x} - \\dot{x}_d).$$\n",
    "\n",
    "Adding $u = u_P + u_I + u_D$ provides the PID controller.\n",
    "\n",
    "### PID gain tuning\n",
    "\n",
    "Step change\n",
    "\n",
    "Overshoot\n",
    "\n",
    "Frequency response\n",
    "\n",
    "### Stability analysis\n",
    "\n",
    "Now we wish to perform stability analysis of a given controller in order\n",
    "to determine whether the system in the long run obeys stability (errors\n",
    "stay bounded), divergence (errors grow), or convergence (errors shrink\n",
    "toward 0). The general method for performing stability analysis is to\n",
    "assume that we have a (simple) system model and substitute the\n",
    "definition of the control into the equations of motion to obtain an\n",
    "ordinary differential equation (ODE).\n",
    "\n",
    "To introduce this approach, we start by considering a first order LTI\n",
    "system without drift $$\\dot{x} = a x + b u$$ and a P controller with\n",
    "setpoint at 0 $$u = -k_P x.$$ Substituting the control into the\n",
    "dynamics, we obtain $$\\dot{x} = (a-bk_P) x.$$ This is now a linear ODE,\n",
    "which has solutions of the form $c_0 e^{c_1 t}$. Matching the\n",
    "coefficient of the ODE and the initial condition $x(0)$, we obtain the\n",
    "solution trajectory $$x(t) = x(0) e^{t(a-bk_P)}.$$\n",
    "\n",
    "The stability of the system is therefore determined entirely by the\n",
    "exponent $a-bk_P$. If $a-bk_P > 0$, then the system will diverge from 0\n",
    "over time. If $a-bk_P = 0$, then the system will not move, and if\n",
    "$a-bk_P < 0$, then the system will converge to 0 over time. Moreover,\n",
    "the speed of convergence depends on its value, with faster convergence\n",
    "for more strongly negative values. This then suggests that if $b$ is\n",
    "positive (matching our alignment assumption) then faster convergence is\n",
    "achieved by making $k_P$ larger.\n",
    "\n",
    "Let us now study an LTI system with a drift term\n",
    "$$\\dot{x} = a x + b u + c$$ with the PI controller\n",
    "$$u = -k_P x - k_I I.$$ Substituting the control into the dynamics, we\n",
    "obtain $$\\dot{x} = (a-bk_P) x - b k_I I + c$$ If we take the time\n",
    "derivative of both sides, we obtain\n",
    "$$\\ddot{x} = (a-bk_P) \\dot{x} - b k_I x$$ where we have noted that\n",
    "$\\dot{I}(t) = x(t)$. It turns out that the solution to this ODE is a\n",
    "*damped harmonic oscillator*, which has generally five possible solution\n",
    "classes: divergent, undamped, overdamped, underdamped, and critically\n",
    "damped.\n",
    "\n",
    "To derive these solutions, we again can use a basis of exponential\n",
    "functions of the form $e^{c_1 t}$. This requires solving the\n",
    "characteristic equation $$c_1^2 = (a-bk_P) c_1 - b k_I$$ to obtain the\n",
    "two solutions (to a quadratic equation)\n",
    "$$c_1 = d \\pm \\sqrt{d^2 - b k_I}$$ with $d=\\frac{a-bk_P}{2}$. Let us\n",
    "call the two solutions $c_1^-$ and $c_1^+$. The resulting general\n",
    "solution is: $$x(t) = c_0^- e^{t c_1^-} +c_0^+ e^{t c_1^+}$$ with\n",
    "matching initial conditions $$\\begin{gathered}\n",
    "x(0) = c_0^- + c_0^+ \\\\\n",
    "\\dot{x}(0) = (a-bk_P) x(0) + c = c_0^- c_1^- + c_0^+ c_1^+\n",
    "\\end{gathered}$$ which is a linear system of equations in $c_0^-$ and\n",
    "$c_0^+$:\n",
    "$$\\begin{bmatrix}{x(0)}\\\\{\\dot{x}(0)}\\end{bmatrix} = \\begin{bmatrix}{1}&{1}\\\\{c_1^-}&{c_1^+}\\end{bmatrix} \\begin{bmatrix}{c_0^-}\\\\{c_0^+}\\end{bmatrix}.$$\n",
    "This can be solved using matrix inversion except for the case where\n",
    "$c_1^- = c_1^+$, which we shall ignore for just a moment. First, let us\n",
    "examine whether $x(t)$ converges to 0 as $t \\rightarrow \\infty$, which\n",
    "requires the exponential terms to converge to 0. This requires that\n",
    "$d = \\frac{a-bk_P}{2} < 0$. If, on the other hand, $d > 0$ we have a\n",
    "*divergent* condition, and if $d=0$ we have an *undamped* condition.\n",
    "\n",
    "Next, let us examine the *overdamped* case in which $b k_I < d^2$,\n",
    "giving the two solutions $c_1^- = d - \\sqrt{d^2 - b k_I}$ and\n",
    "$c_1^+ = d + \\sqrt{d^2 - b k_I}$. We would need\n",
    "$-d > \\sqrt{d^2 - b k_I}$ to prevent $c_1^+$ from going positive, but if\n",
    "we assume $b > 0$ then this will certainly hold for all $d < 0$.\n",
    "Overall, in the overdamped case $x(t)$ is a linear combination of two\n",
    "exponentially decreasing functions with different rates.\n",
    "\n",
    "Returning to the case where $c_1^- = c_1^+$, the ODE has a different\n",
    "solution trajectory class\n",
    "$x(t) = c_0 \\exp(t c_1) + c_0^\\prime t \\exp(c_1 t)$. If $c_1<0$ then the\n",
    "system converges and is known as *critically damped*.\n",
    "\n",
    "The last *underdamped* case where $d < 0$ and $b k_I > d^2$ is\n",
    "interesting because the term inside the square root becomes negative. In\n",
    "this situation, the two solution coefficients $c_1^-$ and $c_1^+$ become\n",
    "complex numbers with an *imaginary* component. (This may sound strange\n",
    "at first, but eventually everything will work out!)\n",
    "\n",
    "To derive this, we use the following equation which can be derived via\n",
    "Euler's identity: $$e^{(a + bi)t} = e^{at}(\\cos (bt) + i \\sin (bt)).$$\n",
    "Hence, setting $\\omega = \\sqrt{b K_i - d^2}$ and substituting this into\n",
    "the $$\\begin{aligned}\n",
    "x(t) &= c_0^- e^{t d}(\\cos (-\\omega t) + i \\sin(-\\omega t)) + c_0^+ e^{t d} (\\cos(\\omega t) + i \\sin (\\omega t)) \\\\\n",
    " &= e^{t d} ((c_0^-+c_0^+) \\cos (\\omega t) + i (c_0^+-c_0^-) \\sin (\\omega t))\n",
    "\\end{aligned}$$ Since the initial condition has 0 imaginary component,\n",
    "the imaginary component of $c_0^-+c_0^+$ must be 0, while the real\n",
    "component of $c_0^+-c_0^-$ must be 0. Ultimately, we have the solution\n",
    "$$x(t) = e^{t d} (x(0) \\cos (\\omega t) + \\frac{\\dot{x}(0)}{\\omega} \\sin(\\omega t)).$$\n",
    "This oscillatory behavior means that for any nonzero starting state, the\n",
    "oscillation will overshoot the setpoint. Moreover, the frequency of\n",
    "oscillation $\\omega^2 = bk_I - (a-bk_P)^2/4$ depends on both gain\n",
    "coefficients and the coefficients of the system. Lower values of $k_I$\n",
    "will reduce and eventually eliminate oscillation, at the cost of\n",
    "potentially slower recovery from steady state error.\n",
    "\n",
    "Applying similar analysis of the PD control problem for a second order\n",
    "system similarly leads to the damped harmonic oscillator system, and is\n",
    "left as an exercise. The analysis for the PID case is a bit more\n",
    "complex, involving a third-order linear system. We shall see more\n",
    "general methods for solving these systems in\n",
    "Sec. [4.1](#sec:Stability-in-Linear-Time-Invariant-systems).\n",
    "\n",
    "### Summary\n",
    "\n",
    "For a system with measured state $x$ and derivative $\\dot{x}$, desired\n",
    "setpoint $x_d$ and derivative $\\dot{x}_d$ (optional; can be set to 0),\n",
    "and fixed control frequency $1/\\Delta t$, a PID controller calculates\n",
    "the control $u$ using the following update equations: $$\\begin{gathered}\n",
    "I \\gets I + \\Delta t (x - x_d) \\\\\n",
    "u \\gets -k_P (x - x_d) - k_I I - k_D (\\dot{x} - \\dot{x}_d).\n",
    "\\end{gathered}$$ Here $k_P > 0$, $k_I \\geq 0$, and $k_D \\geq 0$ are the\n",
    "control gains, which are chosen to be positive under the alignment\n",
    "assumption.\n",
    "\n",
    "Practical issues include:\n",
    "\n",
    "-   Control saturation\n",
    "\n",
    "-   Derivative estimation\n",
    "\n",
    "-   Integral wind-up\n",
    "\n",
    "-   Finite control frequency\n",
    "\n",
    "-   Time delay\n",
    "\n",
    "**Control saturation** occurs when the control is subject to bounds\n",
    "$u_{min} \\leq u \\leq u_{max}$, like minimum/maximum torque, or\n",
    "minimum/maximum throttle. If the PID control produces a value of $u$\n",
    "outside the bounds, it should be clamped to produce a valid value. This\n",
    "clamping will slow down convergence and in some cases, cause\n",
    "instability.\n",
    "\n",
    "**Derivative estimation** errors are a problem because derivatives may be\n",
    "approximated using finite differencing:\n",
    "$\\dot{x} \\approx \\frac{x(t)-x(t-\\Delta t)}{\\Delta t}$. However, since\n",
    "$\\Delta t$ is small and in the denominator, the derivative is more\n",
    "sensitive to measurement noise than position estimates. This causes the\n",
    "$D$ term to vary, which in turn causing the control to track less\n",
    "precisely and in an irregular fashion. To improve derivative estimation\n",
    "accuracy, a low-pass filter is typically applied to the signal $x(t)$.\n",
    "\n",
    "**Integral wind-up** occurs when the control cannot be made large enough to\n",
    "counteract a steady-state error, such as when the control is saturated.\n",
    "In this case, $I$ proceeds toward infinity, which then causes delays\n",
    "responding to changes in setpoint. Suppose the system reaches a steady\n",
    "state with positive error due to control limits, causing $I$ to grow\n",
    "large over time. If the setpoint changes to a more reasonable value that\n",
    "can actually be reached, the control still remains dominated by $I$, and\n",
    "the system will not budge until the high positive value of $I$ is\n",
    "\"erased\" by negative errors $x-x_d$. This problem is typically addressed\n",
    "by capping the magnitude of $I$ to ensure bounded controls and\n",
    "sufficiently fast response.\n",
    "\n",
    "**Finite control frequency** can cause instability when gains are stiff in a\n",
    "manner similar to errors in Euler integration. The control computed at\n",
    "one time step is applied for $\\Delta t$ time in the continuous dynamics\n",
    "of the system. The controller cannot respond to changes faster than this\n",
    "rate. Hence, if $\\Delta t$ is large, the system could be forced to a\n",
    "state with worse error than the previous time step, leading to\n",
    "instability. Similarly, time delays in state measurement can cause the\n",
    "controller to be counteracting errors at a past time, which may no\n",
    "longer exist in reality.\n",
    "\n",
    "### Applying PID to coupled systems\n",
    "\n",
    "An industrial robot can be considered as a $n$-D second order dynamical\n",
    "system with an $n$-D control, one for each joints. We can observe that\n",
    "each element of the control is aligned with the corresponding element of\n",
    "the state, which means that it could be possible to apply a PID\n",
    "controller separately for each joint. This is not a bad approach for\n",
    "stiff, highly-geared industrial robots.\n",
    "\n",
    "\n",
    "However, as we have seen in the prior chapter, each movement of a joint\n",
    "has inertial effects on the other joints. This is known as *coupling* in\n",
    "the dynamics. Due to this coupling, robot arms with softer PID gains\n",
    "may end up performing unstable or even chaotic motion with this\n",
    "approach, and the robot will not necessarily track paths in an optimal\n",
    "way. The PID approach may also perform quite poorly for vehicles when\n",
    "the disturbance from the setpoint is large.\n",
    "\n",
    "For example, consider the simple coupled LTI system described in\n",
    "[Fig. 4](#fig:PIDCoupled), with the initial error $(1,0)$. If the\n",
    "system were decoupled, the controller for $u_1$ would certainly dampen\n",
    "out the error. But due to coupling in the system, the error in $x_2$\n",
    "starts to increase, which starts to also increase the negative velocity\n",
    "in $x_1$, and so on. When $k_D=2$, the controller dampens out the error\n",
    "quickly, only overshooting once. When $k_D=1$, the controller spirals\n",
    "around the origin multiple times but is ultimately stable. When\n",
    "$k_D=0.5$, the controller ends up in an unstable trajectory.\n",
    "\n",
    "\n",
    "**********\n",
    "\n",
    "![fig:PIDCoupled](figures/control/pid_coupled.svg)\n",
    "\n",
    "<div class=\"divcaption\"><a name=\"fig:PIDCoupled\">Figure 4</a>.\n",
    "Element-wise PID control applied to a coupled system\n",
    "$\\ddot{x}_1 = -0.4 x_2 + u_1$, $\\ddot{x}_2 = 0.4 x_1 + u_2$.\n",
    "Trajectories starting from $x=(1,0)$ are shown for $k_P=1$, $k_I=0.5$,\n",
    "and different values of $k_D$ are\n",
    "shown.\n",
    "    </div>\n",
    "    \n",
    "**********\n",
    "\n",
    "Simulation analysis\n",
    "-------------------\n",
    "\n",
    "Frequency response\n",
    "\n",
    "Step change\n",
    "\n",
    "Monte-Carlo analysis\n",
    "\n",
    "Analyzing stability and convergence\n",
    "-----------------------------------\n",
    "\n",
    "First we shall describe methods for analyzing the stability of a dynamic\n",
    "system under a given closed loop control. The most well-understood\n",
    "methods for doing so are in linear time-invariant (LTI) systems, in\n",
    "which solution trajectories can be calculated analytically. In nonlinear\n",
    "systems, solution trajectories are harder to come by, and methods for\n",
    "analyzing stability are usually restricted to the local neighborhood of\n",
    "a single point in state space, and often require some art to apply\n",
    "successfully.\n",
    "\n",
    "Without loss of generality, we will assume that the desired equilibrium\n",
    "point $x^\\star$ lies at the origin, $x^\\star =0$. If $x^\\star \\neq 0$,\n",
    "we can easily *recenter* the problem by defining the dynamics in terms\n",
    "of the error $e = x - x^\\star$. Hence\n",
    "$\\dot{e} = g(e,u) \\equiv f(e+x^\\star,u)$ is a dynamical system that is\n",
    "essentially equivalent to the original one, but has an equilibrium point\n",
    "at 0.\n",
    "\n",
    "### Stability in Linear Time Invariant systems\n",
    "\n",
    "Recall that LTI systems are given by equations of the form\n",
    "$$\\dot{x} = Ax + Bu$$ where $A$ and $B$ are constant matrices of size\n",
    "$n \\times n$ and $n \\times m$, respectively. We shall consider stability\n",
    "under a closed-loop control function of the form $u=-Kx$, where $K$ is\n",
    "an $m \\times n$ *gain matrix*.\n",
    "\n",
    "As an aside, one might question whether this is the only appropriate\n",
    "form of the control. Note that it is certainly necessary that $u=0$ when\n",
    "$x=0$, because otherwise equilibrium would not hold at 0. Also, any\n",
    "smooth nonlinear control function $u(x)$ can be approximated by its\n",
    "Taylor series at $x=0$, leading to an expression of the form\n",
    "$u(x) = \\frac{\\partial u}{\\partial x}x + O(\\|x\\|^2)$. In this case,\n",
    "$K$ is the negative of its Jacobian. Hence, the results derived here\n",
    "will apply approximately in a neighborhood of the equilibrium point.\n",
    "\n",
    "It may not be clear at first glance, but this class of controller\n",
    "actually *subsumes* PID control! To observe this, for any given\n",
    "second-order single-input single-output system\n",
    "$\\ddot{x} = ax+b\\dot{x}+cu$ we build an equivalent 1-input 3-output\n",
    "system $y = (I,x,\\dot{x})$ such that\n",
    "\n",
    "$$\n",
    "\\dot{y} = \\begin{bmatrix}x\\\\ \\dot{x}\\\\ \\ddot{x} \\end{bmatrix} =\n",
    "\\begin{bmatrix}\n",
    "0 & 1 & 0 \\\\\n",
    "0 & 0 & 1 \\\\\n",
    "0 & a & b\n",
    "\\end{bmatrix} y + \\begin{bmatrix}0\\\\0\\\\c\\end{bmatrix} u\n",
    "$$\n",
    "\n",
    "Then, we can define a\n",
    "the 3$\\times$1 matrix $K$ as\n",
    "$$K=\\begin{bmatrix}k_I & k_P & k_D\\end{bmatrix}$$ so that\n",
    "$u=-Kx$ is the PID control.\n",
    "\n",
    "With this form of closed loop control, we can write the velocity of the\n",
    "state in terms of a matrix times $x$: $$\\dot{x} = Ax - BKx = (A-BK)x.$$\n",
    "Hence, the matrix $A-BK$ is of primary importance.\n",
    "\n",
    "The set of solutions to such linear ODEs are of the form:\n",
    "$$x(t) = \\sum_{k=1}^n c_k v_k e^{t\\lambda_k}$$ where\n",
    "$\\lambda_k \\in \\mathbb{C}$ is an eigenvalue of $A-BK$ and\n",
    "$v_k \\in \\mathbb{C}^n$ is an eigenvector. The coefficients\n",
    "$c_k \\in \\mathbb{C}$ are chosen to match the initial condition $x(0)$.\n",
    "The eigenvalues are also known as the *poles* of the system.\n",
    "\n",
    "Recall that an eigenvalue / eigenvector pair $(\\lambda_k, v_k)$ satisfy\n",
    "the conditions that $(A - BK)v_k = \\lambda_k v_k$, and $v_k \\neq 0$.\n",
    "Eigenvectors corresponding to distinct eigenvalues will be orthogonal,\n",
    "and if a subspace of eigenvectors exists for a single eigenvalue, then\n",
    "an orthogonal basis for this subspace can be determined. An $n \\times n$\n",
    "matrix can have most $n$ distinct eigenvalues.\n",
    "\n",
    "To verify the solution, it is a simple matter to see that\n",
    "$$\\dot{x}(t) = \\sum_{k=1}^n c_k \\lambda_k v_k e^{t\\lambda_k} = \\sum_{k=1}^n c_k (A-BK)v_k e^{t\\lambda_k} = (A-BK) x(t)$$\n",
    "as desired. (If all eigenvalues are distinct, this equation is an\n",
    "expression of all solution trajectories. The process of proving this is\n",
    "a bit more involved, and hence we omit it here.)\n",
    "\n",
    "Note that because $A-BK$ is not generally a symmetric matrix, all of\n",
    "these quantities may be complex numbers. This is at first glance an odd\n",
    "result, since state space trajectories should not have imaginary\n",
    "components! Ultimately, we shall see that the form of the eigenvalues\n",
    "permits real-valued solution trajectories.\n",
    "\n",
    "Let us examine a single time-varying basis function $e^{t\\lambda}$.\n",
    "Since $\\lambda$ is potentially complex, we can write it in terms of its\n",
    "real component $a$ and its complex component $b$, i.e.,\n",
    "$\\lambda = a + b i$. Then, $e^{t\\lambda} = e^{ta}e^{itb}$. Using Euler's\n",
    "formula, $e^{ix} = \\cos x + i \\sin x$, we see that\n",
    "$$e^{t\\lambda} = e^{ta}(\\cos tb + i \\sin tb)$$ The portion of this\n",
    "expression in parentheses oscillates at frequency $b$ and has magnitude\n",
    "1, so the exponent term is the one that regulates the convergence of the\n",
    "basis function as $t \\rightarrow \\infty$. The first thing to note is\n",
    "that if $a > 0$, then the basis function is unstable. If $a = 0$, then\n",
    "the basis function oscillates forever. If $a < 0$, then the basis\n",
    "function decays to 0, and moreover the more negative $a$ is, the faster\n",
    "the decay. As a result, we can state the following principle:\n",
    "\n",
    "*A control $u=-Ku$ of an LTI system is convergent if all poles of $A-BK$\n",
    "have negative real component; is stable if all poles have non-positive\n",
    "real component; and is unstable if any pole has positive real\n",
    "component.*\n",
    "\n",
    "As a result, it is customary to plot the poles of an LTI system as\n",
    "points on the complex plane. Verifying that the system is stable is a\n",
    "matter of checking whether the poles lie on the left half of the complex\n",
    "plane. The speed of convergence is also dictated by the distance from\n",
    "the vertical axis. It is also simple to visually inspect the oscillatory\n",
    "behavior of solutions by examining their imaginary components: as the\n",
    "imaginary magnitude increases, so does the frequency of oscillation.\n",
    "\n",
    "Returning to the question of complex eigenvalues, it can be shown that\n",
    "for all complex eigenvalues $\\lambda_k = a_k + i b_k$, another\n",
    "eigenvalue will be the *complex conjugate* $\\lambda_j = a_k - i b_k$\n",
    "formed by negating all of the imaginary components. Moreover, if $v_k$\n",
    "is the eigenvector corresponding to $\\lambda_k$, the eigenvector $v_j$\n",
    "corresponding to $\\lambda_j$ will be defined with entries equal to the\n",
    "complex conjugate of every entry of $v_k$.\n",
    "\n",
    "We can observe that both the real parts and the imaginary parts of these\n",
    "solutions are real-valued solutions to the original ODE. If\n",
    "$$x(t) = Re(x(t)) + i\\cdot Im(x(t))$$ is a complex solution trajectory\n",
    "then\n",
    "$$Re(\\dot{x}(t)) + i\\cdot Im(\\dot{x}(t)) = \\dot{x}(t) = Ax(t) = A\\cdot Re(x(t)) + i A\\cdot Im(x(t)).$$\n",
    "Since $A$ is real, both the real and imaginary terms must match between\n",
    "the left and right hand sides of the equation. That is,\n",
    "$Re(\\dot{x}(t)) = A\\cdot Re(x(t))$ and\n",
    "$Im(\\dot{x}(t)) = A\\cdot Im(x(t))$. As a result, if we find complex\n",
    "coefficients to match the real-valued initial condition, then the\n",
    "solution trajectory will be real-valued for all $t$.\n",
    "\n",
    "Note that LTI systems with a drift term $$\\dot{x} = Ax + Bu + c,$$ with\n",
    "$c \\neq 0$ a constant vector do not satisfy the requirements for the\n",
    "above analysis to apply directly. Examples of systems with drift include\n",
    "fixed-wing aircraft, satellites in orbit, and robots in a potential\n",
    "field. However, systems with drift can be rewritten as a driftless LTI\n",
    "system in terms of a modified dynamics equation with a state variable\n",
    "$y = (x,1)$ augmented by a constant 1:\n",
    "$$\\dot{y} = \\tilde{A} y + \\tilde{B} u$$ with\n",
    "$$\\tilde{A} = \\begin{bmatrix}\n",
    "A & c \\\\\n",
    "0_{1\\times n} & 0\n",
    "\\end{bmatrix}$$ and $$\\tilde{B} = \\begin{bmatrix}{B}\\\\{0_{1\\times m}}\\end{bmatrix}$$\n",
    "(using block matrix notation).\n",
    "\n",
    "These systems may be stable, but will not be convergent with a control\n",
    "of the form $u=-Kx$ because the $n+1$'th dimension has an eigenvalue\n",
    "with value 0. In some systems we may augment the control with a constant\n",
    "offset $u = -Kx-j$, which may indeed lead to convergence if the offset\n",
    "canceling out the drift term by satisfying $Bj=c$. However, if the drift\n",
    "term cannot be canceled out by the control, then the system is unable to\n",
    "converge, and the best that can be hoped for is convergence to a\n",
    "periodic trajectory (called an orbit, discussed briefly below).\n",
    "\n",
    "### Stability in nonlinear systems\n",
    "\n",
    "Unlike the LTI case, stability analysis for nonlinear systems is largely\n",
    "limited to a neighborhood of the equilibrium point. In other words, we\n",
    "can only hope to prove stability / convergence locally; once the state\n",
    "is disturbed out of that neighborhood no stability guarantees can be\n",
    "made.\n",
    "\n",
    "#### Linearization\n",
    "\n",
    "The simplest method for examining the stability of a nonlinear system is\n",
    "to linearize the system about the equilibrium point and apply standard\n",
    "LTI control theory. This approach can be effective for some systems, but\n",
    "due to the use of linearization, it assumes second order effects are\n",
    "negligible. As a result it is only approximate and the approximation\n",
    "worsens as the state deviates from the equilibrium point.\n",
    "\n",
    "The general method linearizes about the 0 state and the 0 control to\n",
    "determine an LTI system as follows:\n",
    "$$A = \\frac{\\partial f}{\\partial x}(0,0)$$\n",
    "$$B = \\frac{\\partial f}{\\partial u}(0,0).$$ If the system has drift,\n",
    "$f(0,0) \\neq 0$, then a drift vector must also be included in the LTI\n",
    "model $$c = f(0,0).$$ Given a gain matrix $K$, the LTI stability methods\n",
    "above can then be used to determine stability of the system.\n",
    "\n",
    "However, oftentimes the approximation errors introduced by linearization\n",
    "are so severe that this method fails to verify that a control is stable\n",
    "/ convergent. As an example, consider the 1D system: $$\\dot{x} = xu.$$\n",
    "The linearization has the $A$ and $B$ matrices identically 0, hence the\n",
    "pole is at 0 and the LTI analysis determines the system to be stable but\n",
    "not convergent. However, if we set $u = -x$, we see that\n",
    "$\\dot{x} = -x^2$ which has the analytical solution $x(t) = 1/(t+C)$.\n",
    "Obviously this converges to 0.\n",
    "\n",
    "#### Lyapunov stability\n",
    "\n",
    "Lyapunov's method is the most commonly used method for directly studying\n",
    "the stability of nonlinear systems. The main difficulty of nonlinear\n",
    "systems is that it is challenging to determine a closed-form expression\n",
    "for trajectories given an initial state. The idea behind this method is\n",
    "to prove the existence of a function that 1) is minimal at the\n",
    "equilibrium point, and 2) via the dynamics of the system its value will\n",
    "be monotonically decreasing over time.\n",
    "\n",
    "First, let us suppose a closed-loop control $u(x)$ is given. We can then\n",
    "rewrite the dynamics of the system only in terms of $x$ as\n",
    "$\\dot{x} = g(x) \\equiv f(x,u(x))$.\n",
    "\n",
    "Specifically, suppose we are given a *Lyapunov function*\n",
    "$V(x):\\mathbb{R}^n \\rightarrow \\mathbb{R}$ with the following\n",
    "properties:\n",
    "\n",
    "-   $V(0) = 0$.\n",
    "\n",
    "-   $V(x) > 0$ for $x \\neq 0$.\n",
    "\n",
    "We can then examine how the value of the Lyapunov function behaves over\n",
    "time. Taking the time derivative $\\frac{d}{dt} V(x(t))$, we get the\n",
    "relation\n",
    "$$\\frac{d}{dt} V(x) = \\frac{\\partial V(x)}{\\partial x} \\dot{x} = \\frac{\\partial V(x)}{\\partial x} g(x).$$\n",
    "Now if we can prove that $\\frac{\\partial V(x)}{\\partial x} g(x) \\leq 0$\n",
    "for all $x$ in the neighborhood of the origin $\\|x\\| \\leq \\delta$, then\n",
    "the system is *Lyapunov stable*. Intuitively, for any starting state in\n",
    "the $\\delta$-neighborhood of the equilibrium point, $V$ stays bounded,\n",
    "and hence the system state cannot stray too far from the equilibrium\n",
    "point.\n",
    "\n",
    "If the inequality is replaced by a strict inequality\n",
    "$\\frac{\\partial V(x)}{\\partial x} g(x) < 0$ for all $x$ in the\n",
    "neighborhood $0 < \\| x \\| \\leq \\delta$, then the system is convergent.\n",
    "\n",
    "The Lyapunov function can be considered to act as a sort of energy\n",
    "function that is minimized at equilibrium. Since energy is never added\n",
    "over time, the system will return to an equilibrium state. This often\n",
    "implies that to design a Lyapunov function it suffices to calculate the\n",
    "total energy of the system (kinetic + potential energy). In general, the\n",
    "design of a suitable Lyapunov function for a given dynamic system is\n",
    "somewhat of an art.\n",
    "\n",
    "#### Orbits and Poincaré maps\n",
    "\n",
    "In the case of systems with drift, in which drift cannot be cancelled\n",
    "out by choosing a suitable control, convergence to an equilibrium point\n",
    "is impossible. Examples of such systems include fixed-wing aircraft and\n",
    "bipedal walking. However, it may be possible to define a cyclic\n",
    "trajectory with favorable behavior, toward which all other trajectories\n",
    "converge (at least, when the starting point is sufficiently close to the\n",
    "trajectory).\n",
    "\n",
    "A trajectory that returns to a previously encountered state $x(0)$ will\n",
    "cycle forever. Such a trajectory is known as periodic. An example would\n",
    "be a bipedal walking gait in which the same configurations and\n",
    "velocities are reached repeatedly after every two steps. Another would\n",
    "be a holding pattern in which an aircraft can repeat an unlimited number\n",
    "of times (or at least, until fuel runs out).\n",
    "\n",
    "The image\n",
    "$\\{ x \\quad |\\quad x=x(t)\\text{ for some }t\\} \\in \\mathbb{R}^n$ of a\n",
    "periodic trajectory $x(t)$ is known as an *orbit*. A stable orbit is an\n",
    "orbit such that for all starting points in a neighborhood of the orbit,\n",
    "the resulting trajectory never deviates too far from the orbit. An orbit\n",
    "is convergent if for all starting states in a neighborhood of the orbit,\n",
    "the resulting trajectory converges toward the orbit. A convergent orbit\n",
    "in 2D is known as a *limit cycle*.\n",
    "\n",
    "A Poincaré map is method for studying the convergence of periodic\n",
    "trajectories, typically via simulation. The idea is to define an $n-1$-D\n",
    "region in state space (the Poincaré section) through which each\n",
    "trajectory passes through once per cycle. The points at which the\n",
    "trajectory passes through the section form a sequence of discrete states\n",
    "$x_1,x_2,x_3,...$. If those states converge to a fixed point, then the\n",
    "trajectory can be concluded to be stable. Often it is not possible to\n",
    "determine the sequence of states analytically, and hence this technique\n",
    "is most often applied with numerical simulation.\n",
    "\n",
    "Further study\n",
    "-------------\n",
    "\n",
    "This chapter has only briefly touched on the field of control theory.\n",
    "Most universities with engineering schools offer courses on control\n",
    "theory, which provide an in-depth study of linear control systems.\n",
    "Nonlinear control theory will discuss chaotic dynamic systems, Lyapunov\n",
    "functions, and Poincaré maps in much more depth, as well as the control\n",
    "design techniques of feedback linearization and sliding mode control.\n",
    "The most widely used software for developing and analyzing control\n",
    "systems is Matlab Control Toolbox (Mathworks, Inc.).\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Summary\n",
    "-------\n",
    "\n",
    "1.  Model-free control approaches do not require accurate knowledge of\n",
    "    the equations of motion, but require more manual effort to tune.\n",
    "    Model-based approaches can achieve higher performance, but are more\n",
    "    complex and may require more online computation.\n",
    "\n",
    "2.  PID control is the workhorse of 1D control. Gain tuning is required\n",
    "    to ensure stability and desired performance characteristics.\n",
    "\n",
    "3.  Applying PID directly to higher-dimensional systems can sometimes\n",
    "    work adequately (industrial robots), but performance degrades when\n",
    "    individual DOFs are coupled (vehicle control).\n",
    "\n",
    "4.  Simulation is widely used in the study of control stability and\n",
    "    performance in response to disturbances.\n",
    "\n",
    "5.  LTI systems admit many analytic tools for control design, such as\n",
    "    eigendecomposition and pole placement.\n",
    "\n",
    "6.  Analytical methods for control design in nonlinear systems include\n",
    "    linearization, Lyapunov functions, and Poincaré maps."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Exercises\n",
    "---------\n",
    "\n",
    "1.  Analyze the conditions necessary for convergence of a second-order\n",
    "    driftless LTI system $\\ddot{x} = ax + b\\dot{x} + cu$ under PI\n",
    "    control. What conditions must be satisfied for the controller's gain\n",
    "    constants to be overdamped, underdamped, and critically-damped?\n",
    "\n",
    "2.  Implement a PID controller for the pendulum swing-up system with\n",
    "    $m=1 kg$, $L=1m$, $g = 9.8m/s^2$ and $u_{max} = 20N$. Using\n",
    "    simulation, tune the gains to achieve low settling time, lower\n",
    "    overshoot, and rare saturation of the control. (Don't forget to use\n",
    "    the angular difference rather than simple distance!)\n",
    "\n",
    "3.  Why is there a relationship between the proportional gain and with\n",
    "    control frequency $1/\\Delta t$ in a PID controller? Analyze\n",
    "    stability for an LTI system and determine the maximum gain that\n",
    "    would not go unstable.\n",
    "\n",
    "4.  Using a simulation of the pendulum-swing up problem, empirically\n",
    "    test the stability of a PID controller problem when the pendulum is\n",
    "    near the vertical position $\\theta \\approx \\pi/2$. Investigate how\n",
    "    far the initial state can deviate from vertical, with the parameters\n",
    "    $m=1 kg$, $L=1 m$, $g=9.8 m/s^2$, $k_P=10$, $k_D=2$, and\n",
    "    $u_{max} = 5 N \\cdot m$.\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.7.3"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": true,
   "autoclose": false,
   "autocomplete": true,
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 1,
   "hotkeys": {
    "equation": "Ctrl-E",
    "itemize": "Ctrl-I"
   },
   "labels_anchors": false,
   "latex_user_defs": true,
   "report_style_numbering": false,
   "user_envs_cfg": false
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": true,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": true
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}