DwellLyap-M2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dwell Time Sequential Control Design - Mode 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import packages\n",
    "import numpy as np\n",
    "import scipy as sp\n",
    "import cvxpy as cp\n",
    "import numpy.linalg as npla\n",
    "import scipy.linalg as spla\n",
    "import matplotlib.pyplot as plt\n",
    "import IPython.display as ipdi\n",
    "import lib as lb"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# System state-space definition\n",
    "A_list = [np.array([[+1, +1, -1], [0.0, +1, +1], [0.0, 0.0, +1]])]\n",
    "A_list += [np.array([[+1, +1, 0.0], [0.0, +1, -1], [0.0, 0.0, +1]])]\n",
    "A_list += [np.array([[+1, -1, 0.0], [0.0, +1, 0.0], [0.0, 0.0, +1]])]\n",
    "B_list = [np.array([[+1, 0.0], [+1, +1], [0.0, +1]])]\n",
    "B_list += [np.array([[0.0, 0.0], [+1, 0.0], [0.0, +1]])]\n",
    "B_list += [np.array([[0.0, +1], [0.0, 0.0], [0.0, +1]])]\n",
    "\n",
    "# Dwell time lower bounds for subsystems\n",
    "D1 = 1\n",
    "D2 = 12\n",
    "D3 = 5\n",
    "\n",
    "# Weighting matrices\n",
    "Q = np.identity(3)\n",
    "R = np.identity(2)\n",
    "\n",
    "# System dimensions\n",
    "nxn = np.shape(A_list[0])\n",
    "nxm = np.shape(B_list[0])\n",
    "mxn = nxm[::-1]\n",
    "\n",
    "# Switching sequence for simulation with known sequence\n",
    "#sw = [np.random.randint(3) for i in range(50)]\n",
    "#sw = [np.random.choice(np.arange(3), p=[0.8, 0.1, 0.1]) for i in range(50)]\n",
    "sw = np.tile([0, 1, 2, 2, 0], 10)\n",
    "\n",
    "# Set of possible switching sequences\n",
    "Is = [(0, 1), (1, 2), (2, 0)]\n",
    "\n",
    "# Initial state\n",
    "x_0 = np.array([[25], [-50], [100]])\n",
    "\n",
    "# Matplotlib settings\n",
    "ipdi.set_matplotlib_formats('svg')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Design control mode 2 - $\\Delta_{2}=10$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Optimization variables\n",
    "X2 = cp.Variable(shape=nxn, PSD=True)\n",
    "K2 = cp.Variable(shape=mxn, PSD=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Constraints\n",
    "A2 = A_list[1]\n",
    "B2 = B_list[1]\n",
    "Constraints = [cp.bmat([[X2, (A2*X2 + B2*K2).T, (spla.sqrtm(Q)*X2).T, (spla.sqrtm(R)*K2).T],\n",
    "                        [A2*X2 + B2*K2, X2, np.zeros(nxn), np.zeros(nxm)],\n",
    "                        [spla.sqrtm(Q)*X2, np.zeros(nxn), np.identity(nxn[0]), np.zeros(nxm)],\n",
    "                        [spla.sqrtm(R)*K2, np.zeros(mxn), np.zeros(mxn), np.identity(nxm[1])]]) >> 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Objective function\n",
    "Objective = cp.Minimize(-cp.trace(X2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "     pcost       dcost       gap    pres   dres   k/t\n",
      " 0:  0.0000e+00 -6.6395e+00  2e+01  2e+00  4e+00  1e+00\n",
      " 1: -1.6595e+00 -2.7233e+00  4e+00  3e-01  8e-01  4e-01\n",
      " 2: -1.5386e+00 -1.6980e+00  5e-01  4e-02  1e-01  3e-02\n",
      " 3: -1.4629e+00 -1.4722e+00  3e-02  2e-03  6e-03  1e-03\n",
      " 4: -1.4618e+00 -1.4626e+00  2e-03  2e-04  5e-04  1e-04\n",
      " 5: -1.4614e+00 -1.4615e+00  3e-04  3e-05  6e-05  2e-05\n",
      " 6: -1.4614e+00 -1.4614e+00  6e-06  5e-07  1e-06  3e-07\n",
      " 7: -1.4614e+00 -1.4614e+00  3e-07  2e-08  6e-08  2e-08\n",
      "Optimal solution found.\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "-1.4614000195857753"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Problem formulation\n",
    "Problem = cp.Problem(Objective, Constraints)\n",
    "Problem.solve(solver=cp.CVXOPT, eps=1e-12, verbose=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The Lyapunov P2 is equal to: \n",
      " [[ 2.92714145  2.35822028 -0.3883925 ]\n",
      " [ 2.35822028  4.5954421  -1.26168039]\n",
      " [-0.3883925  -1.26168039  2.64265093]]\n"
     ]
    }
   ],
   "source": [
    "# Compute Lyapunov function mode 2\n",
    "P2 = spla.inv(X2.value)\n",
    "print(\"The Lyapunov P2 is equal to: \\n {}\".format(P2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The eigenvalues of P2 are equal to: \n",
      " [6.65769767+0.j 1.12080644+0.j 2.38673037+0.j]\n"
     ]
    }
   ],
   "source": [
    "# Compute the eigenvalues of P2\n",
    "(eig_2, vec_2) = spla.eig(P2)\n",
    "print(\"The eigenvalues of P2 are equal to: \\n {}\".format(eig_2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The feedback gain F2 is equal to: \n",
      " [[-0.43110884 -1.23724189  0.87328944]\n",
      " [-0.04273072  0.02446158 -0.76930923]]\n"
     ]
    }
   ],
   "source": [
    "# Compute state feedback gain mode 2\n",
    "F2 = K2.value @ P2\n",
    "print(\"The feedback gain F2 is equal to: \\n {}\".format(F2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The closed-loop dynamics in mode 2 is: \n",
      " [[ 1.          1.          0.        ]\n",
      " [-0.43110884 -0.23724189 -0.12671056]\n",
      " [-0.04273072  0.02446158  0.23069077]]\n"
     ]
    }
   ],
   "source": [
    "# Closed-loop system dynamics in mode 2\n",
    "Acl2 = A2 + B2@F2\n",
    "print(\"The closed-loop dynamics in mode 2 is: \\n {}\".format(Acl2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The eigenvalues of Acl2 are equal to: \n",
      " [0.30568617+0.21422577j 0.30568617-0.21422577j 0.38207653+0.j        ]\n"
     ]
    }
   ],
   "source": [
    "# Compute the eigenvalues of closed-loop subsystem 2\n",
    "(eig_cl2, vec_cl2) = spla.eig(Acl2)\n",
    "print(\"The eigenvalues of Acl2 are equal to: \\n {}\".format(eig_cl2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Design control mode 3 - $\\Delta_{3}=2$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Optimization variables\n",
    "X3 = cp.Variable(shape=nxn, PSD=True)\n",
    "K3 = cp.Variable(shape=mxn, PSD=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Constraints\n",
    "A3 = A_list[2]\n",
    "B3 = B_list[2]\n",
    "Constraints = [cp.bmat([[X3, (A3*X3 + B3*K3).T, (spla.sqrtm(Q)*X3).T, (spla.sqrtm(R)*K3).T],\n",
    "                        [A3*X3 + B3*K3, X3, np.zeros(nxn), np.zeros(nxm)],\n",
    "                        [spla.sqrtm(Q)*X3, np.zeros(nxn), np.identity(nxn[0]), np.zeros(nxm)],\n",
    "                        [spla.sqrtm(R)*K3, np.zeros(mxn), np.zeros(mxn), np.identity(nxm[1])]]) >> 0]\n",
    "Constraints += [cp.bmat([[P2, npla.matrix_power(Acl2, D2).T],\n",
    "                         [npla.matrix_power(Acl2, D2), X3]]) >> 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Objective function\n",
    "Objective = cp.Minimize(-cp.trace(X3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "----------------------------------------------------------------------------\n",
      "\tSCS v2.1.1 - Splitting Conic Solver\n",
      "\t(c) Brendan O'Donoghue, Stanford University, 2012\n",
      "----------------------------------------------------------------------------\n",
      "Lin-sys: sparse-direct, nnz in A = 57\n",
      "eps = 7.50e-05, alpha = 1.50, max_iters = 5000, normalize = 1, scale = 1.00\n",
      "acceleration_lookback = 10, rho_x = 1.00e-03\n",
      "Variables n = 12, constraints m = 93\n",
      "Cones:\tsd vars: 93, sd blks: 3\n",
      "Setup time: 3.20e-03s\n",
      "----------------------------------------------------------------------------\n",
      " Iter | pri res | dua res | rel gap | pri obj | dua obj | kap/tau | time (s)\n",
      "----------------------------------------------------------------------------\n",
      "     0| 3.99e+19  5.86e+17  9.97e-01 -1.81e+19 -2.37e+16  6.55e+20  2.57e-03 \n",
      "   100| 2.73e-03  1.16e-02  2.16e-02 -9.52e-01 -8.91e-01  1.40e-15  1.27e-02 \n",
      "   200| 1.03e-03  3.65e-03  5.66e-03 -8.24e-01 -8.39e-01  4.11e-16  3.74e-02 \n",
      "   300| 1.28e-03  4.35e-03  5.44e-05 -8.25e-01 -8.25e-01  2.15e-15  4.75e-02 \n",
      "   400| 1.10e-03  2.46e-03  1.11e-03 -8.15e-01 -8.17e-01  1.23e-15  5.68e-02 \n",
      "   500| 9.58e-04  2.86e-03  3.23e-04 -8.14e-01 -8.13e-01  2.80e-17  6.48e-02 \n",
      "   600| 1.37e-03  6.15e-03  1.80e-03 -8.13e-01 -8.08e-01  4.02e-16  7.27e-02 \n",
      "   700| 1.57e-03  5.58e-03  2.27e-03 -8.07e-01 -8.01e-01  1.95e-15  8.10e-02 \n",
      "   800| 3.95e-04  1.52e-03  2.96e-04 -7.92e-01 -7.92e-01  3.49e-16  8.91e-02 \n",
      "   900| 5.83e-04  2.44e-03  1.66e-03 -7.95e-01 -7.90e-01  2.55e-16  9.69e-02 \n",
      "  1000| 7.17e-04  2.96e-03  1.00e-03 -7.92e-01 -7.89e-01  4.56e-16  1.07e-01 \n",
      "  1100| 3.95e-04  1.46e-03  9.92e-05 -7.90e-01 -7.90e-01  3.93e-18  1.15e-01 \n",
      "  1200| 2.07e-04  7.84e-04  6.53e-04 -7.90e-01 -7.88e-01  4.28e-16  1.24e-01 \n",
      "  1300| 5.32e-04  1.12e-03  1.41e-03 -7.93e-01 -7.89e-01  8.51e-16  1.34e-01 \n",
      "  1400| 2.11e-04  9.77e-04  4.94e-04 -7.86e-01 -7.87e-01  6.12e-16  1.45e-01 \n",
      "  1500| 1.57e-04  7.03e-04  2.66e-04 -7.87e-01 -7.87e-01  6.72e-16  1.51e-01 \n",
      "  1600| 6.71e-04  2.24e-03  1.94e-03 -7.92e-01 -7.87e-01  1.38e-15  1.60e-01 \n",
      "  1700| 4.25e-04  1.79e-03  6.63e-04 -7.88e-01 -7.87e-01  6.17e-16  1.67e-01 \n",
      "  1800| 4.59e-04  1.64e-03  5.78e-04 -7.84e-01 -7.85e-01  4.99e-16  1.75e-01 \n",
      "  1900| 2.37e-04  7.22e-04  1.45e-03 -7.89e-01 -7.85e-01  6.70e-16  1.82e-01 \n",
      "  2000| 1.92e-04  5.65e-04  8.55e-04 -7.81e-01 -7.83e-01  7.72e-16  1.90e-01 \n",
      "  2100| 2.98e-04  1.26e-03  4.18e-04 -7.81e-01 -7.82e-01  9.96e-16  2.09e-01 \n",
      "  2200| 3.17e-04  2.04e-03  4.98e-05 -7.82e-01 -7.82e-01  1.05e-15  2.22e-01 \n",
      "  2300| 1.02e-04  3.75e-04  2.00e-04 -7.82e-01 -7.82e-01  1.06e-15  2.34e-01 \n",
      "  2400| 1.96e-04  6.16e-04  1.40e-04 -7.81e-01 -7.82e-01  4.26e-16  2.42e-01 \n",
      "  2500| 1.25e-04  3.40e-04  1.96e-04 -7.81e-01 -7.81e-01  1.23e-15  2.60e-01 \n",
      "  2600| 1.84e-04  6.99e-04  1.63e-04 -7.82e-01 -7.81e-01  1.51e-15  2.78e-01 \n",
      "  2700| 1.40e-04  5.26e-04  2.43e-04 -7.81e-01 -7.81e-01  1.67e-15  2.87e-01 \n",
      "  2800| 1.74e-04  7.92e-04  1.96e-04 -7.81e-01 -7.81e-01  2.13e-17  2.96e-01 \n",
      "  2900| 1.92e-04  7.38e-04  2.26e-04 -7.80e-01 -7.81e-01  1.59e-15  3.04e-01 \n",
      "  3000| 1.24e-04  2.95e-04  3.57e-05 -7.81e-01 -7.81e-01  1.76e-15  3.13e-01 \n",
      "  3100| 4.83e-04  1.88e-03  2.07e-03 -7.75e-01 -7.80e-01  3.86e-16  3.23e-01 \n",
      "  3200| 1.37e-04  6.16e-04  6.39e-04 -7.81e-01 -7.80e-01  3.53e-16  3.31e-01 \n",
      "  3300| 5.75e-05  1.91e-04  1.57e-04 -7.79e-01 -7.80e-01  4.23e-17  3.41e-01 \n",
      "  3400| 1.57e-04  3.91e-04  1.75e-04 -7.80e-01 -7.80e-01  5.45e-16  3.48e-01 \n",
      "  3500| 4.02e-05  8.22e-05  2.63e-04 -7.80e-01 -7.80e-01  6.81e-16  3.57e-01 \n",
      "  3600| 6.16e-05  1.98e-04  1.75e-04 -7.80e-01 -7.80e-01  2.76e-16  3.66e-01 \n",
      "  3700| 6.12e-05  1.95e-04  1.10e-04 -7.79e-01 -7.80e-01  7.69e-16  3.76e-01 \n",
      "  3800| 2.09e-04  8.94e-04  4.00e-04 -7.80e-01 -7.79e-01  2.62e-18  3.90e-01 \n",
      "  3900| 2.58e-04  1.22e-03  5.38e-04 -7.77e-01 -7.79e-01  6.04e-16  4.00e-01 \n",
      "  4000| 5.74e-05  2.99e-04  1.83e-04 -7.80e-01 -7.80e-01  2.41e-17  4.11e-01 \n",
      "  4100| 1.04e-04  4.72e-04  1.45e-04 -7.80e-01 -7.79e-01  6.43e-16  4.26e-01 \n",
      "  4200| 3.44e-02  8.13e-02  9.68e-04 -7.78e-01 -7.80e-01  3.69e-16  4.35e-01 \n",
      "  4300| 1.09e-04  4.68e-04  2.34e-04 -7.79e-01 -7.80e-01  3.08e-17  4.44e-01 \n",
      "  4400| 3.58e-04  2.35e-03  9.62e-04 -7.77e-01 -7.80e-01  1.24e-15  4.53e-01 \n",
      "  4500| 1.04e-04  4.82e-04  6.52e-05 -7.80e-01 -7.79e-01  1.07e-16  4.62e-01 \n",
      "  4600| 5.52e-05  4.13e-04  4.15e-05 -7.79e-01 -7.79e-01  2.50e-16  4.71e-01 \n",
      "  4700| 2.21e-02  5.26e-02  1.97e-04 -7.79e-01 -7.79e-01  1.15e-15  4.80e-01 \n",
      "  4800| 2.50e-03  5.63e-03  1.87e-04 -7.79e-01 -7.80e-01  3.00e-16  4.89e-01 \n",
      "  4900| 5.04e-05  2.26e-04  5.84e-06 -7.79e-01 -7.79e-01  4.81e-16  4.97e-01 \n",
      "  4940| 2.87e-05  7.00e-05  5.75e-05 -7.79e-01 -7.79e-01  1.24e-15  5.01e-01 \n",
      "----------------------------------------------------------------------------\n",
      "Status: Solved\n",
      "Timing: Solve time: 5.01e-01s\n",
      "\tLin-sys: nnz in L factor: 169, avg solve time: 1.02e-06s\n",
      "\tCones: avg projection time: 8.00e-05s\n",
      "\tAcceleration: avg step time: 1.37e-05s\n",
      "----------------------------------------------------------------------------\n",
      "Error metrics:\n",
      "dist(s, K) = 5.5471e-07, dist(y, K*) = 2.0361e-09, s'y/|s||y| = -5.5994e-08\n",
      "primal res: |Ax + s - b|_2 / (1 + |b|_2) = 2.8704e-05\n",
      "dual res:   |A'y + c|_2 / (1 + |c|_2) = 7.0033e-05\n",
      "rel gap:    |c'x + b'y| / (1 + |c'x| + |b'y|) = 5.7467e-05\n",
      "----------------------------------------------------------------------------\n",
      "c'x = -0.7791, -b'y = -0.7793\n",
      "============================================================================\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "-0.7791264313754219"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Problem formulation\n",
    "Problem = cp.Problem(Objective, Constraints)\n",
    "Problem.solve(solver=cp.SCS, eps=7.5e-5, verbose=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The Lyapunov P3 is equal to: \n",
      " [[ 3.84380943e+01 -1.05115336e+03 -3.70716781e+01]\n",
      " [-1.05115336e+03  4.07325210e+04  1.05068704e+03]\n",
      " [-3.70716781e+01  1.05068704e+03  3.84373398e+01]]\n"
     ]
    }
   ],
   "source": [
    "# Compute Lyapunov function mode 3\n",
    "P3 = spla.inv(X3.value)\n",
    "print(\"The Lyapunov P3 is equal to: \\n {}\".format(P3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The eigenvalues of P3 are equal to: \n",
      " [4.07867779e+04+0.j 1.36602940e+00+0.j 2.12525190e+01+0.j]\n"
     ]
    }
   ],
   "source": [
    "# Compute the eigenvalues of P3\n",
    "(eig_3, vec_3) = spla.eig(P3)\n",
    "print(\"The eigenvalues of P3 are equal to: \\n {}\".format(eig_3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The feedback gain F2 is equal to: \n",
      " [[-0.03981736  1.48848784  0.03973852]\n",
      " [-0.39853455  1.73005077 -0.3335413 ]]\n"
     ]
    }
   ],
   "source": [
    "# Compute state feedback gain mode 3\n",
    "F3 = K3.value @ P3\n",
    "print(\"The feedback gain F2 is equal to: \\n {}\".format(F3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The closed-loop dynamics in mode 3 is: \n",
      " [[ 0.60146545  0.73005077 -0.3335413 ]\n",
      " [ 0.          1.          0.        ]\n",
      " [-0.39853455  1.73005077  0.6664587 ]]\n"
     ]
    }
   ],
   "source": [
    "# Closed-loop system dynamics in mode 3\n",
    "Acl3 = A3 + B3@F3\n",
    "print(\"The closed-loop dynamics in mode 3 is: \\n {}\".format(Acl3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The eigenvalues of Acl3 are equal to: \n",
      " [0.26792416+0.j 1.        +0.j 1.        +0.j]\n"
     ]
    }
   ],
   "source": [
    "# Compute the eigenvalues of closed-loop subsystem 3\n",
    "(eig_cl3, vec_cl3) = spla.eig(Acl3)\n",
    "print(\"The eigenvalues of Acl3 are equal to: \\n {}\".format(eig_cl3))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Design control mode 1 - $\\Delta_{1}=5$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Optimization variables\n",
    "X1 = cp.Variable(shape=nxn, PSD=True)\n",
    "K1 = cp.Variable(shape=mxn, PSD=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Constraints\n",
    "A1 = A_list[0]\n",
    "B1 = B_list[0]\n",
    "Constraints = [cp.bmat([[X1, (A1*X1 + B1*K1).T, (spla.sqrtm(Q)*X1).T, (spla.sqrtm(R)*K1).T],\n",
    "                        [A1*X1 + B1*K1, X1, np.zeros(nxn), np.zeros(nxm)],\n",
    "                        [spla.sqrtm(Q)*X1, np.zeros(nxn), np.identity(nxn[0]), np.zeros(nxm)],\n",
    "                        [spla.sqrtm(R)*K1, np.zeros(mxn), np.zeros(mxn), np.identity(nxm[1])]]) >> 0]\n",
    "Constraints += [cp.bmat([[P2, npla.matrix_power(Acl2, D2).T],\n",
    "                         [npla.matrix_power(Acl2, D2), X1]]) >> 0]\n",
    "Constraints += [cp.bmat([[P3, npla.matrix_power(Acl3, D3).T],\n",
    "                         [npla.matrix_power(Acl3, D3), X1]]) >> 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Objective function\n",
    "Objective = cp.Minimize(-cp.trace(X1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "     pcost       dcost       gap    pres   dres   k/t\n",
      " 0:  0.0000e+00 -5.0899e+04  5e+04  1e-04  6e+00  1e+00\n",
      " 1: -5.0206e+00 -4.9159e+04  5e+04  1e-04  6e+00  4e+00\n",
      " 2: -1.5527e+00 -4.8545e+04  5e+04  1e-04  6e+00  1e+01\n",
      " 3: -4.7383e+00 -4.7851e+04  5e+04  1e-04  6e+00  3e+01\n",
      " 4: -3.4520e+00 -3.8653e+04  4e+04  1e-04  5e+00  9e+01\n",
      " 5: -1.5692e+00 -7.6778e+02  8e+02  2e-06  9e-02  2e+00\n",
      " 6: -1.5161e+00 -2.8928e+01  3e+01  7e-08  3e-03  6e-02\n",
      " 7: -1.5534e+00 -4.6733e+00  3e+00  8e-09  4e-04  8e-03\n",
      " 8: -1.6632e+00 -4.0346e+00  2e+00  6e-09  3e-04  2e-02\n",
      " 9: -1.7045e+00 -2.5001e+00  8e-01  2e-09  1e-04  8e-03\n",
      "10: -1.7256e+00 -1.9859e+00  3e-01  7e-10  3e-05  4e-03\n",
      "11: -1.7345e+00 -1.7729e+00  4e-02  1e-10  5e-06  1e-03\n",
      "12: -1.7384e+00 -1.7442e+00  6e-03  2e-11  7e-07  2e-04\n",
      "13: -1.7387e+00 -1.7404e+00  2e-03  5e-12  2e-07  7e-05\n",
      "14: -1.7388e+00 -1.7392e+00  4e-04  1e-12  5e-08  2e-05\n",
      "15: -1.7388e+00 -1.7390e+00  1e-04  3e-13  2e-08  5e-06\n",
      "16: -1.7389e+00 -1.7389e+00  3e-05  8e-14  4e-09  1e-06\n",
      "17: -1.7389e+00 -1.7389e+00  6e-06  2e-14  8e-10  2e-07\n",
      "18: -1.7389e+00 -1.7389e+00  1e-06  4e-15  2e-10  5e-08\n",
      "Optimal solution found.\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "-1.7388537890085198"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Problem formulation\n",
    "Problem = cp.Problem(Objective, Constraints)\n",
    "Problem.solve(solver=cp.CVXOPT, eps=1e-12, verbose=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The Lyapunov P1 is equal to: \n",
      " [[ 1.70863815  0.42957555 -0.88682483]\n",
      " [ 0.42957555  1.80719839 -0.39168736]\n",
      " [-0.88682483 -0.39168736  2.85249333]]\n"
     ]
    }
   ],
   "source": [
    "# Compute Lyapunov function mode 1\n",
    "P1 = spla.inv(X1.value)\n",
    "print(\"The Lyapunov P1 is equal to: \\n {}\".format(P1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The eigenvalues of P1 are equal to: \n",
      " [3.51433779+0.j 1.15519843+0.j 1.69879365+0.j]\n"
     ]
    }
   ],
   "source": [
    "# Compute the eigenvalues of P1\n",
    "(eig_1, vec_1) = spla.eig(P1)\n",
    "print(\"The eigenvalues of P1 are equal to: \\n {}\".format(eig_1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The feedback gain F1 is equal to: \n",
      " [[-0.42957275 -0.80719767  0.39168416]\n",
      " [ 0.17818387 -0.03788955 -0.96566465]]\n"
     ]
    }
   ],
   "source": [
    "# Compute state feedback gain mode 1\n",
    "F1 = K1.value @ P1\n",
    "print(\"The feedback gain F1 is equal to: \\n {}\".format(F1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The closed-loop dynamics in mode 1 is: \n",
      " [[ 0.57042725  0.19280233 -0.60831584]\n",
      " [-0.25138888  0.15491278  0.42601951]\n",
      " [ 0.17818387 -0.03788955  0.03433535]]\n"
     ]
    }
   ],
   "source": [
    "# Closed-loop system dynamics in mode 1\n",
    "Acl1 = A1 + B1@F1\n",
    "print(\"The closed-loop dynamics in mode 1 is: \\n {}\".format(Acl1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The eigenvalues of Acl1 are equal to: \n",
      " [0.25576906+0.30647009j 0.25576906-0.30647009j 0.24813725+0.j        ]\n"
     ]
    }
   ],
   "source": [
    "# Compute the eigenvalues of closed-loop subsystem 1\n",
    "(eig_cl1, vec_cl1) = spla.eig(Acl1)\n",
    "print(\"The eigenvalues of Acl1 are equal to: \\n {}\".format(eig_cl1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Dwell time switch computation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The minimum dwell time from mode 1 to 2 is: 1\n"
     ]
    }
   ],
   "source": [
    "# Switch from subsystem 1 to subsystem 2\n",
    "flag = True\n",
    "D12 = 0\n",
    "while flag:\n",
    "    D12 += 1\n",
    "    if np.all(npla.eigvals(P1 - npla.matrix_power(Acl1, D12).T@P2@npla.matrix_power(Acl1, D12)) > 0):\n",
    "        flag = False\n",
    "print(\"The minimum dwell time from mode 1 to 2 is: {}\".format(D12))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The minimum dwell time from mode 2 to 1 is: 1\n"
     ]
    }
   ],
   "source": [
    "# Switch from subsystem 2 to subsystem 1\n",
    "flag = True\n",
    "D21 = 0\n",
    "while flag:\n",
    "    D21 += 1\n",
    "    if np.all(npla.eigvals(P2 - npla.matrix_power(Acl2, D21).T@P1@npla.matrix_power(Acl2, D21)) > 0):\n",
    "        flag = False\n",
    "print(\"The minimum dwell time from mode 2 to 1 is: {}\".format(D21))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The minimum dwell time from mode 2 to 3 is: 7\n"
     ]
    }
   ],
   "source": [
    "# Switch from subsystem 2 to subsystem 3\n",
    "flag = True\n",
    "D23 = 0\n",
    "while flag:\n",
    "    D23 += 1\n",
    "    if np.all(npla.eigvals(P2 - npla.matrix_power(Acl2, D23).T@P3@npla.matrix_power(Acl2, D23)) > 0):\n",
    "        flag = False\n",
    "print(\"The minimum dwell time from mode 2 to 3 is: {}\".format(D23))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The minimum dwell time from mode 3 to 1 is: 1\n"
     ]
    }
   ],
   "source": [
    "# Switch from subsystem 3 to subsystem 1\n",
    "flag = True\n",
    "D31 = 0\n",
    "while flag:\n",
    "    D31 += 1\n",
    "    if np.all(npla.eigvals(P3 - npla.matrix_power(Acl3, D31).T@P1@npla.matrix_power(Acl3, D31)) > 0):\n",
    "        flag = False\n",
    "print(\"The minimum dwell time from mode 3 to 1 is: {}\".format(D31))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Simulation with switching system dynamics in $\\mathcal{I}_{s}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [],
   "source": [
    "inv_a = spla.lapack.dpotri(4*np.identity(3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([[ 0.0625,  0.    , -0.    ],\n",
       "        [ 0.    ,  0.0625, -0.    ],\n",
       "        [ 0.    ,  0.    ,  0.0625]]), 0)"
      ]
     },
     "execution_count": 35,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "inv_a"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 1.70863815,  0.42957555, -0.88682483],\n",
       "       [ 0.42957555,  1.80719839, -0.39168736],\n",
       "       [-0.88682483, -0.39168736,  2.85249333]])"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "spla.inv(X1.value)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.25,  0.  , -0.  ],\n",
       "       [ 0.  ,  0.25, -0.  ],\n",
       "       [ 0.  ,  0.  ,  0.25]])"
      ]
     },
     "execution_count": 37,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "inv_a[0]*4*np.identity(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "['CVXOPT', 'ECOS', 'ECOS_BB', 'GLPK', 'GLPK_MI', 'OSQP', 'SCS']"
      ]
     },
     "execution_count": 38,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cp.installed_solvers()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "F1 is equal to: \n",
      " [[-0.42957275 -0.80719767  0.39168416]\n",
      " [ 0.17818387 -0.03788955 -0.96566465]] \n",
      "\n",
      "F2 is equal to: \n",
      " [[-0.43110884 -1.23724189  0.87328944]\n",
      " [-0.04273072  0.02446158 -0.76930923]] \n",
      "\n",
      "F3 is equal to: \n",
      " [[-0.03981736  1.48848784  0.03973852]\n",
      " [-0.39853455  1.73005077 -0.3335413 ]] \n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(\"F1 is equal to: \\n {} \\n\".format(F1))\n",
    "print(\"F2 is equal to: \\n {} \\n\".format(F2))\n",
    "print(\"F3 is equal to: \\n {} \\n\".format(F3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 1.70863815  0.42957555 -0.88682483]\n",
      " [ 0.42957555  1.80719839 -0.39168736]\n",
      " [-0.88682483 -0.39168736  2.85249333]]\n"
     ]
    }
   ],
   "source": [
    "print(P1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 2.92714145  2.35822028 -0.3883925 ]\n",
      " [ 2.35822028  4.5954421  -1.26168039]\n",
      " [-0.3883925  -1.26168039  2.64265093]]\n"
     ]
    }
   ],
   "source": [
    "print(P2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 3.84380943e+01 -1.05115336e+03 -3.70716781e+01]\n",
      " [-1.05115336e+03  4.07325210e+04  1.05068704e+03]\n",
      " [-3.70716781e+01  1.05068704e+03  3.84373398e+01]]\n"
     ]
    }
   ],
   "source": [
    "print(P3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "cvxenv",
   "language": "python",
   "name": "cvxenv"
  },
  "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.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}