Root_Locus_Plot.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Stability Analysis with Root Locus Plot\n",
    "\n",
    "The purpose of controller stability analysis is to determine the range of controller gains between lower $K_{cL}$ and upper $K_{cU}$ limits that lead to a stable controller.\n",
    "\n",
    "$K_{cL} \\le K_c \\le K_{cU}$\n",
    "\n",
    "The principles of stability analysis presented here are general for any linear time-invariant system whether it is for controller design or for analysis of system dynamics. Several characteristics of a system in the Laplace domain can be deduced without transforming a system signal or transfer function back into the time domain. Some of the analysis relies on the roots of the transfer function denominator, also known as poles. The roots of the numerator, also known as zeros, do not affect the stability directly but can potentially cancel an unstable pole to create an overall stable system.\n",
    "\n",
    "#### Converge or Diverge\n",
    "\n",
    "A first point of analysis is whether the system converges or diverges. This is determined by analyzing the roots of the denominator of the transfer function. If any of the real parts of the roots of the denominator are positive then the system is unstable. A simple rule to determine whether there are positive real roots is to examine the signs of the polynomial. If there are mixed signs (+ or -) then the system will be unstable because there is at least one positive real root.\n",
    "\n",
    "See [Stability Analysis](https://apmonitor.com/pdc/index.php/Main/StabilityAnalysis) for additional information on Routh Arrays, Root Locus Plots, and Bode Plots."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy.optimize import fsolve\n",
    "import sympy as sym\n",
    "from IPython import display\n",
    "\n",
    "class RLPlot:\n",
    "    def dev(u,σ=3,n=10,early=False):\n",
    "        '''\n",
    "        dev(u,σ=3,n=10)\n",
    "        \n",
    "        returns 1 when the i-th value of u is more than σ standard deviations\n",
    "          from the mean of the last n values of u\n",
    "        \n",
    "        ut = np.array([t[i] for i in np.where(dev(u) > 0)[0]])\n",
    "        '''\n",
    "        if early:\n",
    "            du = np.array([abs(np.mean(list(u[max(0,i-n):i+1])+\\\n",
    "                [u[0]]*(max(0,i-n)-(i-n)))-u[i])/max(1e-20,\\\n",
    "                np.std(list(u[max(0,i-n):i+1])+[u[0]]*\\\n",
    "                (max(0,i-n)-(i-n)))) > σ for i in range(len(u))])\n",
    "        else:\n",
    "            du = np.array([np.logical_and(abs(np.mean(u[max(0,i-n):i+1])\\\n",
    "                -u[i])/max(1e-20,np.std(u[max(0,i-n):i+1])) > σ, i>n ) \\\n",
    "                for i in range(len(u))])\n",
    "        return du\n",
    "    \n",
    "    def Routh(func,K=sym.Symbol('K'),r=100,o=0,res=1000,it=100,func1=lambda x:1,show=False,Ks=[0],dev=dev):\n",
    "        '''\n",
    "        Routh(func,K=sym.Symbol('K'),r=100,o=0,res=200,it=100,func1=lambda x:1,show=False)\n",
    "        \n",
    "        determines the range of convergence for a gain function in the s domain\n",
    "        \n",
    "        gain function denominator = func = lambda x: f(x,K)\n",
    "        K = sym.Symbol('K')\n",
    "        r = radius to check around origin o in res+1 steps\n",
    "        it = maxiumum interations\n",
    "        gain function numerator = func1 = lambda x: f(x,K) only used to plot the signal\n",
    "        show = plot the signal\n",
    "        \n",
    "        returns the range of convergence\n",
    "        '''\n",
    "        x = sym.Symbol('x')\n",
    "        f = func(x)\n",
    "        i=0\n",
    "        c=[]\n",
    "        while f!=0 and i<it:\n",
    "            c = [f.subs(x,0)]+c\n",
    "            f = sym.diff(f/(i+1),x)\n",
    "            i+=1\n",
    "        if show:\n",
    "            from scipy import signal\n",
    "            f1 = func1(x)\n",
    "            i=0\n",
    "            c0=[]\n",
    "            while f1!=0 and i<it:\n",
    "                if type(f1) == int or type(f1) == float:\n",
    "                    c0 = [f1]+c0\n",
    "                else:\n",
    "                    c0 = [f1.subs(x,0)]+c0\n",
    "                f1 = sym.diff(f1/(i+1),x)\n",
    "                i+=1\n",
    "            if sym.diff(func(x),K)==0:\n",
    "                if sym.diff(func1(x),K)==0:\n",
    "                    plt.plot(*signal.step(signal.TransferFunction(np.array(c0).astype(float),\\\n",
    "                        np.array(c).astype(float))),label=f'{K} = {i}')\n",
    "                else:\n",
    "                    for i in Ks:\n",
    "                        plt.plot(*signal.step(signal.TransferFunction(np.array([j.subs(K,i) \\\n",
    "                            for j in c0]).astype(float),np.array(c).astype(float))),label=f'{K} = {i}')\n",
    "            elif sym.diff(func1(x),K)==0:\n",
    "                for i in Ks:\n",
    "                    plt.plot(*signal.step(signal.TransferFunction(np.array(c0).astype(float),\\\n",
    "                        np.array([j.subs(K,i) for j in c]).astype(float))),label=f'{K} = {i}')\n",
    "            else:\n",
    "                for i in Ks:\n",
    "                    plt.plot(*signal.step(signal.TransferFunction(np.array([j.subs(K,i) \\\n",
    "                        for j in c0]).astype(float),np.array([j.subs(K,i) for j in c]).astype(float))),\\\n",
    "                             label=f'{K} = {i}')\n",
    "            plt.xlabel('Time')\n",
    "            plt.legend()\n",
    "            plt.show()\n",
    "        c = np.array(c+[0]*(len(c)%2))\n",
    "        c = c.reshape((int(len(c)/2),2)).T\n",
    "        i=0\n",
    "        while c[-1][0]!=0 and i<2*it:\n",
    "            n = c[-2,1:]-c[-2,0]*c[-1,1:]/c[-1,0]\n",
    "            c = np.array(list(c)+[list(n)+[0]])\n",
    "        c = c.T[0]/c[0,0]\n",
    "        C = np.copy(c)\n",
    "        k = np.linspace(o-r,o+r,res+1)\n",
    "        c = np.array([[i.subs(K,j) for i in c] for j in k])\n",
    "        c = np.array([all(i==abs(i)) for i in c])\n",
    "        if all(c):\n",
    "            k = np.array([k[0],k[-1]])\n",
    "        else:\n",
    "            k = np.array([[fsolve(lambda k_: [i.subs(K,k) for k in k_],j)[0] \\\n",
    "                           for i in C] for j in k[np.where(dev(c))[0]]])\n",
    "            if len(k) == 0:\n",
    "                k = np.array([np.nan]*2)\n",
    "            elif len(k) == 1:\n",
    "                k = k[0][np.where(np.array([min([abs(i.subs(K,j)) for i in C[:-1]]) \\\n",
    "                        for j in k[0]])==min(np.array([min([abs(i.subs(K,j)) \\\n",
    "                                for i in C[:-1]]) for j in k[0]])))[0][0]]\n",
    "                k = np.array([-np.inf]*int(c[0])+[k]+[np.inf]*int(c[-1]))\n",
    "            else:\n",
    "                k = k[0,np.where(np.isclose(*k))[0]]\n",
    "        return np.sort(k)\n",
    "    \n",
    "    def Locus(func,K=sym.Symbol('K'),r=10,o=0,res=1000,it=100,show=False,Routh=Routh):\n",
    "        '''\n",
    "        Locus(func,K=sym.Symbol('K'),r=10,o=0,res=1000,it=100,show=False)\n",
    "        \n",
    "        determines the range of smooth vs oscillatory behavior\n",
    "           for a gain function in the s domain\n",
    "        \n",
    "        gain function denominator = func = lambda x: f(x,K)\n",
    "        K = sym.Symbol('K')\n",
    "        r = radius to check around origin o in res+1 steps\n",
    "        it = maxiumum interations\n",
    "        show = show the Root Locus Plot (with slider)\n",
    "        \n",
    "        returns the range of smooth convergence\n",
    "        '''\n",
    "        x = sym.Symbol('x')\n",
    "        f = func(x)\n",
    "        if sym.diff(func(x),K) == 0:\n",
    "            i=0\n",
    "            c=[]\n",
    "            while f!=0 and i<it:\n",
    "                c = [f.subs(x,0)]+c\n",
    "                f = sym.diff(f/(i+1),x)\n",
    "                i+=1\n",
    "            if all(np.isclose(np.roots(c).imag**2,0)):\n",
    "                out = np.array([-np.inf,np.inf])\n",
    "                check = np.array([o-r,o+r])\n",
    "            else:\n",
    "                out = np.array([np.nan]*2)\n",
    "                check = np.array([o,o])\n",
    "        else:\n",
    "            check = Routh(func,K,r,o,res,it)\n",
    "        bounds = np.copy(check)\n",
    "        \n",
    "        if not all(np.isfinite(check)):\n",
    "            out = np.copy(check)\n",
    "            bounds = np.array([o-r,o+r])\n",
    "        \n",
    "        if show or (all(np.isfinite(check)) and sym.diff(func(x),K)!=0):\n",
    "            if sym.diff(func(x),K) != 0:\n",
    "                i=0\n",
    "                c=[]\n",
    "                while f!=0 and i<it:\n",
    "                    c = [f.subs(x,0)]+c\n",
    "                    f = sym.diff(f/(i+1),x)\n",
    "                    i+=1\n",
    "            if sym.diff(func(x),K) == 0:\n",
    "                k = np.linspace(*bounds,1+1)\n",
    "            else:\n",
    "                k = np.linspace(*bounds,res+1)\n",
    "            R = np.array([np.roots(np.array([i.subs(K,j) for i in c])) for j in k])\n",
    "        \n",
    "        if all(np.isfinite(check)) and sym.diff(func(x),K)!=0:\n",
    "            R_ = np.array([abs(np.subtract(*np.meshgrid(a,a))) for a in R])\n",
    "            R_ = [[a[i,i+1:] for i,_ in enumerate(a)] for a in R_]\n",
    "            R_ = np.array([min(np.array(np.concatenate(a).flat)) for a in R_])\n",
    "            if all(R_==R_[0]):\n",
    "                check = [np.nan]*2\n",
    "            else:\n",
    "                check = [k[R_==min(R_)],k[R_==min(R_[R_!=min(R_)])]]\n",
    "\n",
    "            def zero(k):\n",
    "                R = np.roots(np.array([i.subs(K,k) for i in c]))\n",
    "                R = abs(np.subtract(*np.meshgrid(R,R)))\n",
    "                R = [R[i,i+1:] for i,_ in enumerate(R)]\n",
    "                return min(np.array(np.concatenate(R).flat))\n",
    "            out = np.array([fsolve(lambda k: [zero(i) for i in k],p)[0] for p in check])\n",
    "            if np.isclose(*out):\n",
    "                out = [np.nan]*2\n",
    "\n",
    "        if show:\n",
    "            real = []\n",
    "            imag = []\n",
    "            for i in R:\n",
    "                for j in i:\n",
    "                    real += [j.real]\n",
    "                    imag += [j.imag]\n",
    "            def update(k):\n",
    "                roots = np.roots(np.array([i.subs(K,k) for i in c]))\n",
    "                plt.figure(figsize=(12,8))\n",
    "                ax = plt.gca()\n",
    "                ax.clear()\n",
    "                plt.plot(real,imag,'ro',label='Roots')\n",
    "                plt.plot([0,0],[min(imag),max(imag)],'b--',label='Stability Limit')\n",
    "                plt.plot(roots.real,roots.imag,'ks',label='Roots (K='+str(k)+')')\n",
    "                plt.legend(); plt.xlabel('Re'); plt.ylabel('Im'); plt.grid()\n",
    "                #display.clear_output(wait=True)\n",
    "                plt.show()\n",
    "            import ipywidgets as wg\n",
    "            k_slide = wg.FloatSlider(value=o,min=min(bounds),max=max(bounds),\\\n",
    "                                     step=10*(max(bounds)-min(bounds))/res)\n",
    "            wg.interact(update, k=k_slide)\n",
    "            print('Locus Root Plot')\n",
    "\n",
    "        return np.array(out)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import warnings\n",
    "with warnings.catch_warnings(record=True):\n",
    "    K = sym.Symbol('K')  # controller gain\n",
    "    # denominator of closed-loop transfer function\n",
    "    RLPlot.Locus(lambda x:(x**2+x+1)*(x+1)**2-2*K,K,10,show=True)"
   ]
  }
 ],
 "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}