{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 9章 最適レギュレータ"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from control.matlab import *\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "#plt.rcParams['font.family'] ='sans-serif' #使用するフォント\n",
    "plt.rcParams['font.family'] = 'Times New Roman' # font familyの設定\n",
    "plt.rcParams['mathtext.fontset'] = 'cm' # math fontの設定\n",
    "plt.rcParams['xtick.direction'] = 'in' #x軸の目盛線が内向き('in')か外向き('out')か双方向か('inout')\n",
    "plt.rcParams['ytick.direction'] = 'in' #y軸の目盛線が内向き('in')か外向き('out')か双方向か('inout')\n",
    "plt.rcParams['xtick.major.width'] = 1.0 #x軸主目盛り線の線幅\n",
    "plt.rcParams['ytick.major.width'] = 1.0 #y軸主目盛り線の線幅\n",
    "plt.rcParams['font.size'] = 11 #フォントの大きさ\n",
    "plt.rcParams['axes.linewidth'] = 0.5 # 軸の線幅edge linewidth。囲みの太さ\n",
    "plt.rcParams['mathtext.default'] = 'it'#'regular'\n",
    "plt.rcParams['axes.xmargin'] = '0'\n",
    "plt.rcParams['axes.ymargin'] = '0.05'\n",
    "plt.rcParams['savefig.facecolor'] = 'None'\n",
    "plt.rcParams['savefig.edgecolor'] = 'None'\n",
    "\n",
    "plt.rcParams[\"legend.fancybox\"] = True     # 丸角\n",
    "# plt.rcParams[\"legend.framealpha\"] = 1    # 透明度の指定、0で塗りつぶしなし\n",
    "# plt.rcParams[\"legend.edgecolor\"] = 'gray' # edgeの色を変更\n",
    "plt.rcParams[\"legend.handlelength\"] = 1.8  # 凡例の線の長さを調節\n",
    "plt.rcParams[\"legend.labelspacing\"] = 0.4  # 垂直方向（縦）の距離の各凡例の距離\n",
    "plt.rcParams[\"legend.handletextpad\"] = 0.7 # 凡例の線と文字の距離の長さ\n",
    "plt.rcParams[\"legend.markerscale\"] = 1.0   # 点がある場合のmarker scale"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def linestyle_generator():\n",
    "    linestyle = ['-', '--', '-.', ':']\n",
    "    lineID = 0\n",
    "    while True:\n",
    "        yield linestyle[lineID]\n",
    "        lineID = (lineID + 1) % len(linestyle)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_set(fig_ax, *args):\n",
    "    fig_ax.set_xlabel(args[0])\n",
    "    fig_ax.set_ylabel(args[1])\n",
    "    fig_ax.grid(ls=':', lw=0.5)\n",
    "    if len(args)==3:\n",
    "        fig_ax.legend(loc=args[2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def bodeplot_set(fig_ax, *args):\n",
    "    fig_ax[0].grid(which=\"both\", ls=':', lw=0.5)\n",
    "    fig_ax[0].set_ylabel('Gain [dB]')\n",
    "\n",
    "    fig_ax[1].grid(which=\"both\", ls=':', lw=0.5)\n",
    "    fig_ax[1].set_xlabel('$\\omega$ [rad/s]')\n",
    "    fig_ax[1].set_ylabel('Phase [deg]')\n",
    "    \n",
    "    if len(args) > 0:\n",
    "        fig_ax[1].legend(loc=args[0])\n",
    "    if len(args) > 1:\n",
    "        fig_ax[0].legend(loc=args[1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 最適レギュレータ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 制御対象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "A = [[ 0.  1.]\n",
      "     [ 0. -1.]]\n",
      "\n",
      "B = [[0.]\n",
      "     [1.]]\n",
      "\n",
      "C = [[1. 0.]\n",
      "     [0. 1.]]\n",
      "\n",
      "D = [[0.]\n",
      "     [0.]]\n",
      "\n"
     ]
    }
   ],
   "source": [
    "A = '0 1; 0 -1'\n",
    "B = '0; 1'\n",
    "C = '1 0 ; 0 1'\n",
    "D = '0; 0'\n",
    "P = ss(A, B, C, D)\n",
    "print(P)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 例9.3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- フィードバックゲイン ---\n",
      "[[-1. -1.]]\n",
      "[[-1. -1.]]\n",
      "--- 閉ループ極 ---\n",
      "[-1.+8.820093e-09j -1.-8.820093e-09j]\n",
      "[-1.+1.49011612e-08j -1.-1.49011612e-08j]\n"
     ]
    }
   ],
   "source": [
    "Q = np.array([ [1, 0], [0, 1]])\n",
    "R = 1\n",
    "\n",
    "F, X, E = lqr(P.A, P.B, Q, R)\n",
    "F = -F\n",
    "\n",
    "print('--- フィードバックゲイン ---')\n",
    "print(F)\n",
    "print(-(1/R)*P.B.T*X)\n",
    "print('--- 閉ループ極 ---')\n",
    "print(E)\n",
    "print(np.linalg.eigvals(P.A+P.B*F))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[2., 1.],\n",
       "        [1., 1.]])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 216x165.6 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "Acl = P.A + P.B*F\n",
    "Pfb = ss(Acl, P.B, P.C, P.D)\n",
    "\n",
    "tdata = np.arange(0, 5, 0.01)\n",
    "xini, tini = initial(Pfb, tdata, [-0.3, 0.4]) #ゼロ入力応答\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(3, 2.3))\n",
    "\n",
    "ax.plot(tini, xini[:,0], label = '$x_1$', c='k', lw=1)\n",
    "ax.plot(tini, xini[:,1], ls = '-.', label = '$x_2$', c='k', lw=1)\n",
    "\n",
    "plot_set(ax, '$t$', '$x$', 'best')\n",
    "\n",
    "# fig.savefig(\"sf_lqr.pdf\", transparent=True, bbox_inches=\"tight\", pad_inches=0.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- フィードバックゲイン ---\n",
      "[[-10.          -3.69041576]]\n",
      "[[-10.          -3.69041576]]\n",
      "--- 閉ループ極 ---\n",
      "[-2.345208+2.1213202j -2.345208-2.1213202j]\n",
      "[-2.34520788+2.12132034j -2.34520788-2.12132034j]\n"
     ]
    }
   ],
   "source": [
    "Q = np.array([ [100, 0], [0, 1]])\n",
    "R = 1\n",
    "\n",
    "F, X, E = lqr(P.A, P.B, Q, R)\n",
    "F = -F\n",
    "\n",
    "print('--- フィードバックゲイン ---')\n",
    "print(F)\n",
    "print(-(1/R)*P.B.T*X)\n",
    "print('--- 閉ループ極 ---')\n",
    "print(E)\n",
    "print(np.linalg.eigvals(P.A+P.B*F))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[46.9041576 , 10.        ],\n",
       "        [10.        ,  3.69041576]])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 216x165.6 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "Acl = P.A + P.B*F\n",
    "Pfb = ss(Acl, P.B, P.C, P.D)\n",
    "\n",
    "tdata = np.arange(0, 5, 0.01)\n",
    "xini, tini = initial(Pfb, tdata, [-0.3, 0.4]) #ゼロ入力応答\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(3, 2.3))\n",
    "\n",
    "ax.plot(tini, xini[:,0], label = '$x_1$', c='k', lw=1)\n",
    "ax.plot(tini, xini[:,1], ls = '-.', label = '$x_2$', c='k', lw=1)\n",
    "\n",
    "plot_set(ax, '$t$', '$x$', 'best')\n",
    "\n",
    "# fig.savefig(\"sf_lqr.pdf\", transparent=True, bbox_inches=\"tight\", pad_inches=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### ハミルトン行列"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 2.34520788+2.12132034j  2.34520788-2.12132034j -2.34520788+2.12132034j\n",
      " -2.34520788-2.12132034j]\n",
      "--- ハミルトン行列の安定固有値 ---\n",
      "[(-2.3452078799117135+2.1213203435596406j), (-2.3452078799117135-2.1213203435596406j)]\n",
      "--- フィードバックゲイン ---\n",
      "[[-10.          -3.69041576]]\n"
     ]
    }
   ],
   "source": [
    "H = np.block([[P.A, -P.B*(1/R)*P.B.T], [-Q, -P.A.T]])\n",
    "eigH = np.linalg.eigvals(H)\n",
    "print(eigH)\n",
    "\n",
    "print('--- ハミルトン行列の安定固有値 ---')\n",
    "eigH_stable = [ i for i in eigH if i < 0]\n",
    "print(eigH_stable)\n",
    "\n",
    "F = -acker(P.A, P.B, eigH_stable)\n",
    "print('--- フィードバックゲイン ---')\n",
    "print(F)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 円条件"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "    3.69 s + 10\n",
      "-------------------\n",
      "s^2 + s + 2.776e-17\n",
      "\n",
      "(inf, 70.74566579604311, nan, 4.2583738912001925)\n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAM4AAADOCAYAAAB2Hz3EAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjMuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8vihELAAAACXBIWXMAAAsTAAALEwEAmpwYAAAgZ0lEQVR4nO2deXRUVda3n1OZExIgEAaRJjK0CIpAArLUFmgFVHp1Ay0NgtovdlQMKgEcOwiBAIICDoCKLZ/gwKDwylIR2jRKq60oRMJiUCAgg5iGJEBCJVUVKjnfH6HyhpCQ1HDvqVzOs1YtqlJV9/e7VbW559y7z95CSolGo/EOm2oDGk1jRAeORuMDOnA0Gh/QgaPR+IAOHI3GB3TgaDQ+EKraQE1SUlLklVdeqdqGRsOMGTOWSSlTansu6ALnyiuvJCMjw69tFBQU0LJly8AYaoT62kNg9GfMmPFLXc/poZpG4wOWDJzw8PDLWl97MF5fB44F9bUH4/UtGTh2u/2y1tcejNe3ZOBoNEZjycCx8hBBewgOfUsGTkRExGWtrz0Yr2/JwLHy2Fp7CA59SwaO6sV5qvW1B+P1LRk4YWFhl7W+9mC8viUDx8pja+0hOPQtGTilpaWXtb72YLy+6YEjhIgTQqwTQvwkhPhICBETaI2KiopAb7JR6WsPxuurOOLcAtwPXAOUAOMCLRAaqjbpW7W+9mC8vul7JqX8xHNfCLEV+LX681lZWcyYMaPq8RNPPMGMGTNwu924XC4AYmJiqKiowOFwABAdHQ3836HZZrPhcDgoKSkBKse6oaGhVY/Dw8MJDw+vOl0ZHh5OREQEdrsdKSVhYWFERERQWlpKRUUFoaGhREZGVj222WxER0fjdDpxu91Vj10uF+fOnePcuXNERUXhcrkoKysDoEmTJpSVlVU9jomJ8WqfoqKisNlsDd4nKSUulytg+ySEoEmTJl7tU0hICCUlJQHbJ2+/J7fbjdPp9Guf6kRKqewGvAiEVv/b9OnTpb8UFhb6vY3GrK89BEYfyJB1/HaVnRwQQgwHXpRSugO9bSuPrbWH4NBXEjhCiMHALinlUSFEu0Bv32ZTe7JQtb72YLy+6XMcIUQaMAnIF0LYgO+B8YHU8IylVaFaX3swXl/FyYGXgJeM1HA6nURGRhopEdT62oPx+uqP5wbgdgd82tSo9LUH4/UtGThWHltrD8Ghr/7TNQArj621h+DQt2TgeC7AXa762oPx+pYMnHPnzl3W+tqD8fqWDBwhxGWtrz0Yr2/JwGnSpMllra89GK9vycCx8thaewgOfUsGTr2ZrRbX1x6M17dk4Gg0RmPJwLHy2Fp7CA59SwaOlYcI2kNw6OvAsaC+9mC8viUDR6MxGksGTkxMwAvnNCp97cF4fUsGjpXT2bWH4NC3ZOBY+cKb9hAc+pYMHI3GaCwZOFYeW2sPwaFvycCxclki7SE49C0ZOJ7KkZervvZgvL4lA0ejMRpLBo6V17prD8Ghb8nA0WiMxpKBY+WGRtpDcOirqh3dVQjxsRCivwp9jcZflHT+kVL+JIQ4CxhSTSEqKsqIzdZJcXExR44cweFw4HA4cLlcREdHExUVRZMmTejYsaOhjVyllOTl5RETE8PevXtJSEigtLSU3Nxcrr32Wux2O23atDFMvy7M/h7M1FfZMqvWnO9ANJYSQhjWsKikpITc3Fx27dpFSUkJISEhREZG0rZtW6KioggPD8dms1FaWkphYSEOh4NPPvkEu91OaGgoHTp04Oqrr6Zbt25UVFR43VjKbrdTWlrKyZMnycrKYvDgwWzdupWmTZsycOBATp8+TWRkJG63m1OnTlFQUMDmzZspKysjOTmZr7/+mv79+9O6dWtatGhheGMpQFljqXPnzhESEmJIYykhFfWiF0IsB5ZLKbdU/3tGRobMyMjwa9sFBQW0bNnSr21Ux+VysXbtWnbu3El8fDy9evUiKSmpTo269KWUHDp0iO3bt7N3715cLhd/+tOf6NevX72ljI4ePUpYWBiLFi2iT58+DBgwgJiYGMLDw73y4HA4cDqdfPrpp/z444889thjCCFISEhowCfhHYH+HszWF0LMkFJm1Pac+kaRQcyxY8d45513KCkp4c9//jNjx471a3tCCDp16kSnTp2Ayuzdjz76iCeffJJrrrmG0aNHX3AKVUrJ/v37OXz4MPv27WPcuHHMmTPHLw9RUVFERUVV7UthYSHLli2jX79+tGvXjo4dOyqvh9YYsGTg+NvfPicnh5UrV9KuXTvGjx9PfHy8IfqhoaGMGDGCESNGsHfvXubOnUtoaCgTJkwgNDSU+fPn079/f4YMGcKQIUMM8dCiRQuefPJJANavX8/q1auZOHEiMTExfgeQv9+DvxiqX1ePQyNvQAfgGyAdiKr+XCB6gJaWlvr0vrKyMjlv3jy5ePFi6Xa7TdeXUsq9e/fKXr16yaVLl/q8DX89bNu2TaalpcnTp08r8xAI/NUn2HqASimPSClvlFLOllIGPKHIM7n0hl27djFp0iSGDRvGhAkTqia2ZulXVFSwdOlSmjRpwtatW0lISOCZZ56hqKjINA8ekpOTmTlzJvn5+bz//vs+b8cfD4HASH1LDtW8we1288orrxAaGsrLL7/sV8D4Sn5+Pvv27SMpKYn27dsDMHz4cG6++WZmzpzJ7bffzqBBg0z1FBsbS2xsLIcPH+aHH37gqquuonnz5qZ6CGYsmTlQ15mmmhQVFZGWlsbtt9/OY489FrCgaag+wOeff87ixYu58cYbSU5OvuC5hIQEFixYwOnTp3nhhRc8w9yAe7gUgwYNonv37syaNYucnByv3hsoD75iqH5dYzhVt0DMcRwOR72vyc/Pl6mpqbKwsNBvPV/0pZRyy5YtMi8vr0Gv/frrr+X06dNleXl5QD00lPLycnno0CG5bdu2Br8n0B68xV99gm2OYzSeC2Z1kZ+fz/Tp05k7d67XZ8wCoQ/w2WefceDAgQZf0b/pppv44x//yLPPPtugI09DPHiDzWYjMTGRf/3rX3z//fcNek+gPXiLkfqWDJxLUVxcTEZGBvPmzSM2NlaJh82bN5OUlERKSopX7+vduzcjRoxg1qxZXg3bAoUQgqeeeoqEhAR++OEH0/WDCUsGTl1jW6fTyd///ndmzpxpaF3hS42tc3Nzyc7OpkWLFj5tOykpid/97ne88sorPnvwByEEiYmJvP/++5w4cUKJh4ZipL4lA6euC1/z58/n8ccf9/lH669+cXExJ06c4IknnvBr+wMGDKBZs2Z8/fXXXnsIBEII5syZw/79+y/ZLtDKF0AtGTi1jW23bdtG69atSUxMVKIP8MILL9ClS5eApLTcd999fPDBB3Wuqzd6fmGz2UhISGDRokV1vkbPcRoZNcf/LpeLd955h7/97W9K9AEOHDjAlClTaNWqVUA0hBA8/vjjLFiwoMEeAk3Xrl25++67+eWXX5R5uBRG6lsycGqufXnxxRdJS0vDZjNnd2vq2+12XnvtNZo2bRpQnfbt29O2bVu+++67ej0YRUJCAnPnzq213KxZHurCSH1LBk71se2OHTto2rQpHTt2VKIPlRnITz75pCFZx/fffz8rV67E6XRe0oNRhIaGkpaWxunTpy96Ts9xGhnV15q/8847PPTQQ8r0d+/ezebNmw1bgSmEIC0tjddff71OD0bTuXNnli9fftFZNl1zoJHhqeC4c+dOevfubdoQraY+wKFDh7j77rsN1bvqqqvIy8u7QNfsKpr33nsvO3fuvOBvupJnIyM0tDJ3de3atdx1113K9M+ePYvNZjNl7f2QIUP45z//eZEHs2jTpg2lpaWUl5cr81ATI/UtGTiRkZEUFxcTHh5OZGSkEn2AVatWcfXVV5uiOXDgQL744ouLPJhJs2bN2LRpk1IP1TFS35KBU1paysqVKxkzZowyfSklAwcOpEuXLqZoCiFo3749R48erfJgNv3797/gPwo9x2lklJeXc+jQoaq1/WZTUVHBt99+y+7du03VHTt2LO+++26VB7MRQrBp0yYOHz6szEN19BzHSw4fPkyvXr2U6XtKHv3hD38wVTc+Pr7qarnZJ0Q8jB07liNHjij14MFIfUsGzu7du+nXr58y/ejoaAoLC5VcAExISKCwsFBZwfPmzZtTWFgI6KLrjY4TJ06YkpNWFyUlJezbt0+JdlJSEj/88MNFF0TNxLNSVKUHo/UtGTg2m01pbTCHw8GECROUaPfs2ZMdO3Yo7fg8fvx4ysvLddfpxoTb7VZeUO+LL77wuTqNv8TFxWG325XOL/bv38+WLVv0HKcx8dNPP/Gb3/xGqYeKigpTc+NqQ+X8olevXpw8eVLPcWpDCKG+5Vct5OTk0LVrV6Ue2rdvr/SoFxcXVzVBV4EnqdZTfF0VRuo3OHCEEN2FEE8JIaYJIaYByw1z5QenT582dFn0pXjvvfdITExk4sSJJCYm8t577ynx0bx5c2VDRQ8bN2685OpQMzBS35tknv93/uY5VeFzeXshxAQq23xcAbwspTzj67Zq4nQ6lax1f++993jwwQcpLS2lW7duHDlyhAcffBDA72Lt3hIZGVlvmwqjufPOO5XPNY3U92ao9k8p5VIp5Qop5Qpgti+CQojewA1Syn8Am4HpvmynLpxOpyEln+ojPT29KsXj+PHjQGXKR3p6uuleVq5cydKlS03Xrc7OnTuVHfk9GKnvzRFnqxBiAeAZA3QHRvmgOQzYc/5+DvAOMMnzpL+NpZo0acKZM2cMbVhUWxOmoqIirrrqKmJiYqrKJzVr1ozY2FgKCgrqbcLU0MZSDdmn3bt3U1JSgsvl8muf/GksVVRURFFRETExMcoaSzkcDhISEgxpLOVN4DwPvAp4Kln72rGnDbDt/H0nlcO1KgYNGsR//vOfWt9Ysw5aTEzMBY+jo6MpLy+nvLy8qg9MdWo+rpk9W3PFYM3nL/W4adOm/PzzzwBce+21VT+cDh06VDU3qi1btyH7dKl9qG2fnE4n0dHRRERE+LVPtT2ub3tQuU9jxozBZrMRExMTkH3yxoPncUFBAZGRkQHZp5p4M1R7X0r5arWh2iwv3ludE4Dnk4sFTvq4nVqJiopSMr6fPXv2RT+I6OhoZs/2aUTrFw6HQ1mxRQ9vvPGGUn2j8eaI07rGUK0bMNoHzf8FHjt//zrgIx+2USctW7bk7Nmzgdxkg/CcAEhPT+fYsWN06NCB2bNnm35iwOVycfbsWeXXkTp37mzpOU69RxwhhGem7QT2AkfO3wp8EZRS7gD2CiFSgNuAqb5spy569+7Nnj176n+hAYwdO5bDhw+zevVqDh48aHrQAPz3v/8F4IorrqjnlcZx6tQpunXrpvzMnpH6DRmqzRNC3AfsB84B8vzN5+V1UsoXpJRvSimnSSkvLo/iB506deLXX38N5Ca9pqioSFmSZ25uLqA2cLZt20ZpaamlA6chQ7Xr63hdjwB7CQiq86MAbrjhBuLi4pRof/vttwB069ZNiT5UFipMSEhQvgLUSBryKxsjpRxX8wbcY7Q5XykrK7ugaITZtG7dmiVLlijR/vLLL4mPj1e6rGL16tVER0dfdDbNbIzUrzdwpJS5dfz9x8DbCQydOnXiwIEDyvTdbreyiXFOTg59+vRRmtLvuWKvlxU0Mrp161bntSAzcLlcDBkypM6C6EZx/Phx8vPzGTx4sLIEy/z8fO69917A2CTLhhAUSZ6NiXbt2imbnHuQUrJmzRpTNdetWwfA73//e1N1q/PWW28pr6dmBpYMnJiYGK6//nqvm70GUr979+707NnTVN233nqLFi1a0KNHD2Xzi/79+5OQUJn/e1nPcRojFRUV3HXXXaxdu1aZPsCxY8dMa/lnt9vZu3cvw4cPx2azKSnN9PHHH1/wY9XloRoZDoejKk+ruLhYiT7A4MGDa23BYQSzZ8+mrKyMkSNHXuDBTPbt20f37t2rHqvwUB0j9S0ZOB7Gjh2rbDEZVCYP9uvXjzNnzhiutX79euLj4xk4cKDhWrVx7Ngx/vKXvyhfg2MWlgwcT7Jlx44dOXjwoOlliqonezZv3pxly5YZqvfFF1+Qm5vL6NGjq2q5mb3e//XXX6dZs2YX/E3XHGjEPPLII7z44ovK9BMTExk5ciQnTwY0CbyKsrIypk2bhtvtNr0PkIfCwkJSUlKUZUuowJKBUz3VIzExkfj4eLKzs5XoA7Rt25bMzExDshlefvlljh8/Tr9+/ejR4/+yoMxKdyktLSUzM7PWTAXVKTe66LqfPPDAAyxfvlxZ0mFYWBipqamcOnUqoNvdtWsXR48e5eeff66qb2A2x44d4/HHH79s5jYeLBk4NVcQ2mw2Jk6cyEsvvaREH+Caa65hzZo1VdnL/uJ2u3njjTfYv38/rVq1YvToC5dGmdHM6ptvvmHPnj1ceeWVtT5vhodLYaS+JQOntgzpzp07ExMTY8qQra4M7QcffDBg+i+99BIDBgzgs88+Iy0trdb/LIzmp59+Yvjw4XU+rzpTXVfy9BJPsYeaPPzww1WLzFToh4eHM2zYMKZNm+bXxblVq1bRvn171q1bR2xsLA8//HCDPQQCl8tFZmYm48aNu+QQzUgPDcFIfUsGTl3YbDbmzJnDyy+/TF5enhIPERERjBgxguzsbKSUXr9/w4YN2O12+vTpw5o1a3jooYcuOg1sNFlZWdx3332X3bymOpYMnEv1tw8LC2Pu3LnMnj2bX375xXR9qOwo0KpVKzIzM70Kng0bNpCbm8sDDzxAZmYmYWFhTJo0qdbX1ufBF86dO8fTTz/NwIED6dChQ72vN8KDNxipb8nAqS87Nzo6mvnz5/PCCy8YMmxrSHZwhw4duPPOOzlw4ECDTlN/8MEHHD9+nIkTJ/Ljjz/y9ttv88gjj9S5RDrQGcplZWXk5OSQkpLS4ORJ1VnSuuu0lzRkbBsZGcn8+fNZtmzZBW3OzdIHSE5OxmazMXnyZAoKaq994na7WbBgAWVlZVWnnKdNm0Z0dDRPP/203x4awuHDh5k0aRKdOnWic+fODX6fnuNYlLCwMObMmUNxcTHp6elKykp17tyZzMxMiouLL8rm3rNnD2lpaQwdOrSqYk52djZr165lypQpVYUOjUJKybvvvktISAgLFy5UUlo4WLHkiiNvi66PHDmSW265henTpzN06FBuvfVWU/Xj4uKIi4vjwIEDbNmyheTkZN58802g8rRz9SHH1KlTiY+PZ/LkyQH1UJOysjI+/PBDunTpQvv27X3ahori92bpW/KI48sH1rp1axYsWMDJkyd59tln/WqT4esXNmTIEFq2bMmAAQOIjIwkLS3tgqD5/PPP2bRpE08//XS9eWH+/GjeffddFi9ezKhRo7jhhht83o6VA8eSRxy73d6g+r81EUJw9913k5eXx6JFi3A6nYwaNYrrrrvOUP3y8nI2bNjAV199RZcuXfj22285deoUK1eu5MyZM6SkpBASEkJaWhqJiYk8+uijAfdQVFTEm2++SY8ePRg4cCDt2rVr8HsD5SHQGKlvycDxl7Zt2zJ16lScTidr1qxhxYoV9O3bl+HDhwe0BXtBQQFvv/02J06cYOjQoTz//PNV10Zat27NmDFjOHToEHl5eaSmprJr1y4++OCDgP4YsrOzWbduHRMnTmTMmDG0bds2YNu2MqYGjhCiFfAkUCilfM4onUAdoiMjI/nrX/8KwPfff8/06dOx2Wy0adOG5ORkrr/++lrzoerSz8/PJzs7mx07dmC324mNjeW+++67ZNXNjh07UlRUxLZt2+jTpw+nTp0iIyODtLQ0CgsL6dixY60XImvzUF5ezoEDB2jWrBmvvvoqvXr14uqrr2bGjBkB/Q/hUh7MxDJDNSnlSSHEfmq09gg0Rlz46tu3L3379gUq6zNnZ2ezcOFCnE4nUkpiYmKqWouEh4fjcDhwOBxVpWCllCQkJJCcnMxjjz3mVSGJWbNmUVBQwMaNG0lKSsLtdlNaWspXX31FTk4OkZGRbNu2jfvvv5+NGzcSERGB2+0mLi6OLl268OGHHzJ06FBycnKIjY3lrrvuIiMjw/BcMitfABW+pH34JSjE/wCJUsqM2p6/6aab5DfffFP12NvGUlDZla1FixamNpYKCQmhuLiYkpIS7HY7bdq0wWazYbPZCA0N9bmx1O7du7n55psZPXo0S5curXOfQkNDCQ8PJy8vj7KyMlwu1wVHMk8NBrMaS0HlmbmmTZsqayzl+R583aeEhIQZdf1OkVIG/AZkAO/WvJ1/7n+AjLreO336dOkv+fn5fm8jWPSHDRsmY2Ji5K+//qrMg6+o9uCv/qV+p4YM1WRdUWoSRozXVehv2rSJ9evXM2fOHK8n7ao/g2DwYKS+Ja/jWGFs7XQ6eeSRR/jtb39b78VOozz4i2oPlknyFEI0A/oA1wkhWhulY4W17vPmzePgwYMsWbLEpx+A6s8gGDwYqW/2WbUzwASjdRp7BcmDBw/y3HPPMWrUKG677TYlHgKBag+6kqeXNOZ0dikljz76KOHh4SxcuFCJh0Ch2oOR+uo/XQNQmebhr/769evZuHEjCxcu9KsdoerPIBg8GKlvySNOYx1b2+12Jk6cSI8ePRqUj2aEh0Ci2oNl5jhm0VjH1pmZmRw7doxVq1b5PcxQ/RkEgwc9x/GSxliWaO/evSxcuJBx48Zx0003KfEQaFR70OWhvKSxFfuWUpKamkpsbCzz5s1T4sEIVHvQRde9xOzuBP7qr1y5kn//+98899xzVd3MzPZgBKo9GKlvycBpTN2Oi4qKmDJlCn379iUlJUWJB6NQ7cFIfUueHGhMY+upU6eSn5/Phg0bCAkJUeLBKFR70HMcL2ksY+vt27ezZMkSUlNTSUpKUuLBSFR70HMcLzGyv32g9MvLyxk/fjytW7dm1qxZSjwYjWoPRupbcqh27ty5oNd/7bXXyM7OZtWqVTRt2lSJB6NR7cFIfUsecVQXA69PPy8vj/T0dAYNGsSoUaOUeDAD1R6M1Ldk4DRp0iSo9SdPnozL5WLJkiWGfbmqP4Ng8GCkviUDJ5jH1llZWaxevZpnnnmGLl26KPFgFqo9GKlvycBR1euzPn2n00lqaiqdO3fmqaeeUuLBTFR7MFLfkicHgpV58+aRm5vLZ599pjzlXuMfljziBOPY+sCBAzz33HOMHj2aQYMGKfFgNqo96DmOlwTbEEFKyYQJE4iIiPBrVac/HlSg2oMeqnlJsH1h77//PllZWSxatMi02syqP4Ng8GCkviWPOMFEUVERaWlpJCUl1dodWtM4seQRx5u6zEbrP/vss5w4cYKPP/44oEmc3nhQhWoPRupb8ogTLOns2dnZLFmyhAkTJpCcnKzEg0pUezBS35KBEwwX3jxJnK1atTIkibMhHlSj2oNlkjyFEM8DgwAJjJVS/mimvpksXbqU7du3G5bEqVGLaUccIcT1wNtSyl7AP4BMo7RUj61LS0tJT0/n1ltvNSyJsz5UfwbB4MFIfdOOOFLKndUefgNcY5SW6rJE06ZNw263s2jRImUZwqo/g2DwYKS+IYEjhMgAOtf8u5TynvN3BwG1XgnMyspixowZVY99aSzlcDgMbVh0qSZMW7duZcWKFUyaNIl27dpRUFAA1N+Eqb598rYJU0lJCaGhoQHZJ18bS3n+VdVY6uzZs7Rt29avfaqTuhrnGHUDrgcG1PV8Y24s5Xa7ZVJSkmzTpo0sLi5W4sGD6qZOweCh0TWWqgshxLVAUynlFiFEHFAhpbQHWkfVWvdly5aRnZ3NW2+9RWxsrBIPHlSv9w8GD0bqmxY4QohbgLeBgvPjfgn0NUvfaAoLC3nmmWfo378/I0eOVG1HYzBmnhz4Ekg0Q6u0tNT0/+3S09MpKipi8eLFOBwO5WeUVHwGwebBSH1LXgA1m+zsbN544w0effRRrr32WtV2NCZgycCJiooyTauiooIJEybQqlUrMjIyTNevC+3BWH1LJnmaWUFy+fLlfPfdd6xYsaIqQ0B1BUvtwXh99Z+uAXiuAxjN6dOneeqpp7jxxhu55557qv5ulv6l0B6M1bdk4JjFtGnTOHXqFEuWLFH+v6vGXCz5bRvZ397D7t27efXVV3nooYfo2bOn6fr1oT0Yq2/JwDG627GUksmTJxMXF8fMmTNN128I2oOx+pYMHKPH1p988glZWVlkZGTQsmVL0/Ubgvag5zhe8/zzzxu27bKyMqZMmULXrl1JTU01Xb+haA8G69eVxKbqFogkz8rdMob58+dLQH766adK9BuK9uC/PpdI8rTkEcco8vPzmTlzJnfccQd33HGHajsahYjKwAoehBBvAr/4uZkBwBa/zTRefe0hMPpXSilrbcwadIGj0TQG9FBNo/EBHTgajQ/owNFofMCSgSOEeF4IsUMI8YMQwrBqOvV4aCWEmC+EeEaB9gQhxANCiOlCiGYK9LsKIT4WQvQ3W/u8fpwQYp0Q4ichxEdCiICvKrRc4JhZv+1SSClPAvsBUxO2hBC9gRuklP8ANgPTzdQHkFL+BJwFVHXPvQW4n8oSZCXAuEALWC5wpJQ7pZS7zz/8BvhVoR0VfS6GAXvO3885/1gFynp8SCk/kVIWnb+IuRUDfgPqMwH9wJ/6bSZ6MJs2wLbz953AFYp8BAuJwJJAb7RRB46UMqOu584P2bZLKQ+r8qCIE4CnQkUscFKhF6UIIYYDL0opA962wHJDNbi4fpsQQn1DTPP4X6Dn+fvXAR+ps6IOIcRgYJeU8qgQol2gt9+ojzi1ESz1286fzeoDtBFCtJZSnjBDV0q5QwixVwiRAvwGmGqGbnWEEB2A3wI3CSG+k1I6TNZPAyYB+UIIG/A9MD6gGjrlRqPxHksO1TQao9GBo9H4gA4cjcYHdOBoND6gA0ej8QEdOBqND+jAaeQIIZKEEMeEEIuFEFOEEF+qykq+nLDcBdDLDSllthDiILD2fKaEDXgJ6KXWmbXRgWM9rgF2AgghbgPigT8AX0gp31JpzErooZp1uFUI8S2QC6QIIeKpXJNSBnyJPgIFFH3EsQ7fAQepDJZ5VC51OCelXA8ghAhRZ8166COOhZBSLgd+pnLV68/An86vCAW4S5UvK6KPOI0cIURPoCMwWAjxJZAKbAfCgReBjedPHgQ0O/hyR2dHazQ+oIdqGo0P6MDRaHxAB45G4wM6cDQaH9CBo9H4gA4cjcYHdOBoND6gA0ej8YH/D/cTPYT6nTPfAAAAAElFTkSuQmCC\n",
      "text/plain": [
       "<Figure size 216x216 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "L = ss(P.A, P.B, -F, 0)\n",
    "print(tf(L))\n",
    "\n",
    "import matplotlib.patches as patches\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(3, 3))\n",
    "x, y, w = nyquist(L, logspace(-2,3,1000), plot=False)\n",
    "ax.plot(x, y, c='k')\n",
    "# ax.plot(x, -y, ls='--')\n",
    "ax.scatter(-1, 0, color='k')\n",
    "\n",
    "ax.grid(ls=':', lw=0.5)\n",
    "ax.set_xlabel('Re')\n",
    "ax.set_ylabel('Im')\n",
    "ax.set_xlim(-2.5, 2.5)\n",
    "ax.set_ylim(-2.5, 2.5)\n",
    "ax.set_xticks([-2, -1, 0, 1, 2])\n",
    "\n",
    "\n",
    "c = patches.Circle(xy=(-1, 0), radius=1, fill=False, ec='k', lw=0.5)\n",
    "ax.add_patch(c)\n",
    "\n",
    "c = patches.Circle(xy=(0, 0), radius=1, fill=False, ec='k', ls='--', lw=0.5)\n",
    "ax.add_patch(c)\n",
    "\n",
    "print(margin(L))\n",
    "\n",
    "fig.tight_layout()\n",
    "\n",
    "#fig.savefig(\"lqr_nyquist.pdf\", transparent=True, bbox_inches=\"tight\", pad_inches=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 例9.4: 折り返し法"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 0. -1.]\n"
     ]
    }
   ],
   "source": [
    "v = 2\n",
    "print(P.pole())\n",
    "X, _, _ = care(P.A+v*np.eye(2), P.B, np.zeros([2,2]), R)\n",
    "#print(X)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-3. -4.]\n",
      "[ 0. -1.]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<matplotlib.legend.Legend at 0x7fcfb1d2dd00>"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 216x165.6 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "FF = - (1/R)*(P.B.T)*X\n",
    "Acl = P.A + P.B*FF\n",
    "Pfb = ss(Acl, P.B, P.C, P.D)\n",
    "print(Pfb.pole())\n",
    "print(P.pole())\n",
    "\n",
    "tdata = np.arange(0, 5, 0.01)\n",
    "xini, tini = initial(Pfb, tdata, [-0.3, 0.4]) #ゼロ入力応答\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(3, 2.3))\n",
    "\n",
    "ax.plot(tini, xini[:,0], label = '$x_1$', c='k')\n",
    "ax.plot(tini, xini[:,1], ls = '-.', label = '$x_2$', c='k')\n",
    "\n",
    "ax.set_xlabel('$t$')\n",
    "ax.set_ylabel('$x$')\n",
    "ax.grid(ls=':')\n",
    "ax.legend()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "6 s + 12\n",
      "--------\n",
      "s^2 + s\n",
      "\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 216x216 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "L = ss(P.A, P.B, -FF, 0)\n",
    "print(tf(L))\n",
    "\n",
    "import matplotlib.patches as patches\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(3, 3))\n",
    "x, y, w = nyquist(L, logspace(-2,3,1000), plot=False)\n",
    "ax.plot(x, y, c='k')\n",
    "# ax.plot(x, -y, ls='--')\n",
    "ax.scatter(-1, 0, color='k')\n",
    "\n",
    "ax.grid(ls=':', lw=0.5)\n",
    "ax.set_xlabel('Re')\n",
    "ax.set_ylabel('Im')\n",
    "ax.set_xlim(-2.5, 2.5)\n",
    "ax.set_ylim(-2.5, 2.5)\n",
    "ax.set_xticks([-2, -1, 0, 1, 2])\n",
    "\n",
    "\n",
    "c = patches.Circle(xy=(-1, 0), radius=1, fill=False, ec='k')\n",
    "ax.add_patch(c)\n",
    "\n",
    "c = patches.Circle(xy=(0, 0), radius=1, fill=False, ec='k', ls='--')\n",
    "ax.add_patch(c)\n",
    "\n",
    "fig.tight_layout()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-2.345208+2.1213202j -2.345208-2.1213202j]\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 216x216 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "fig, ax = plt.subplots(figsize=(3, 3))\n",
    "\n",
    "P1pole = pole(P)\n",
    "P2pole = pole(Pfb)\n",
    "\n",
    "Q = [ [100, 0], [0, 1]]\n",
    "R = 1\n",
    "_, _, P3pole = lqr(P.A, P.B, Q, R)\n",
    "\n",
    "print(P3pole)\n",
    "\n",
    "ax.scatter(P1pole.real, P1pole.imag, s=50, marker='o',label='$A$', color='k')\n",
    "ax.scatter(P2pole.real, P2pole.imag, s=50, marker='*',label='$A+BF$', color='k')\n",
    "#ax.scatter(P3pole.real, P3pole.imag, s=50, marker='x',label='$A+BF$', color='k')\n",
    "\n",
    "ax.vlines(-v, -5, 5, color='k', lw=1)\n",
    "\n",
    "#ydata = np.arange(-5,5,0.1)\n",
    "#xdata = -np.abs(ydata)\n",
    "#ax.plot(xdata, ydata)\n",
    "\n",
    "ax.set_xlim(-7.2,1.2)\n",
    "ax.set_ylim(-4.2,4.2)\n",
    "plot_set(ax, 'Re', 'Im', 2)\n",
    "\n",
    "#fig.savefig(\"lqr_pole_assignment.pdf\", transparent=True, bbox_inches=\"tight\", pad_inches=0.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 章末問題"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- フィードバックゲイン ---\n",
      "[[-3. -4.]]\n",
      "[[-3. -4.]]\n",
      "--- 閉ループ極 ---\n",
      "[-2.+0.j -1.+0.j]\n",
      "[-1. -2.]\n"
     ]
    }
   ],
   "source": [
    "#章末問題４\n",
    "A = '0 1; 1 1'\n",
    "B = '0; 1'\n",
    "C = '1 0 ; 0 1'\n",
    "D = '0; 0'\n",
    "P = ss(A, B, C, D)\n",
    "\n",
    "Q = np.array([ [3, 0], [0, 2]])\n",
    "R = 1\n",
    "\n",
    "F, X, E = lqr(P.A, P.B, Q, R)\n",
    "F = -F\n",
    "\n",
    "print('--- フィードバックゲイン ---')\n",
    "print(F)\n",
    "print(-(1/R)*P.B.T*X)\n",
    "print('--- 閉ループ極 ---')\n",
    "print(E)\n",
    "print(np.linalg.eigvals(P.A+P.B*F))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[5., 3.],\n",
       "        [3., 4.]])"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- フィードバックゲイン ---\n",
      "[[-1.         -3.31662479]]\n",
      "[[-1.         -3.31662479]]\n",
      "--- 閉ループ極 ---\n",
      "[-2.981188  +0.j -0.33543673+0.j]\n",
      "[-0.33543674 -2.98118805]\n"
     ]
    }
   ],
   "source": [
    "#章末問題８\n",
    "A = '0 1; 0 0'\n",
    "B = '0; 1'\n",
    "C = '1 0 ; 0 3'\n",
    "D = '0; 0'\n",
    "P = ss(A, B, C, D)\n",
    "\n",
    "Q = P.C.T * P.C\n",
    "R = 1\n",
    "\n",
    "F, X, E = lqr(P.A, P.B, Q, R)\n",
    "F = -F\n",
    "\n",
    "print('--- フィードバックゲイン ---')\n",
    "print(F)\n",
    "print(-(1/R)*P.B.T*X)\n",
    "print('--- 閉ループ極 ---')\n",
    "print(E)\n",
    "print(np.linalg.eigvals(P.A+P.B*F))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[3.31662479, 1.        ],\n",
       "        [1.        , 3.31662479]])"
      ]
     },
     "execution_count": 47,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "3.3166247903554"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.sqrt(11)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-0.33543673964540455"
      ]
     },
     "execution_count": 49,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "(-np.sqrt(11)+np.sqrt(7))/2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "-2.9811880507099953"
      ]
     },
     "execution_count": 50,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "(-np.sqrt(11)-np.sqrt(7))/2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[288.,  48.],\n",
       "        [ 48.,   8.]])"
      ]
     },
     "execution_count": 42,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#章末問題９\n",
    "A = '1 1; 0 -5'\n",
    "B = '0; 1'\n",
    "C = '1 0 ; 0 1'\n",
    "D = '0; 0'\n",
    "P = ss(A, B, C, D)\n",
    "\n",
    "v = 3\n",
    "R = 1\n",
    "\n",
    "X, _, _ = care(P.A+v*np.eye(2), P.B, np.zeros([2,2]), R)\n",
    "X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-48.  -8.]]\n",
      "[-5. -7.]\n",
      "[ 1. -5.]\n"
     ]
    }
   ],
   "source": [
    "FF = - (1/R)*(P.B.T)*X\n",
    "print(FF)\n",
    "\n",
    "Acl = P.A + P.B*FF\n",
    "Pfb = ss(Acl, P.B, P.C, P.D)\n",
    "print(Pfb.pole())\n",
    "print(P.pole())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 付録：最適極"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 95,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 216x216 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "Q = [ [100, 0], [0, 1]]\n",
    "R = 0.001\n",
    "\n",
    "_, _, E = lqr(P.A, P.B, Q, R)\n",
    "\n",
    "fig, ax = plt.subplots(figsize=(3, 3))\n",
    "ax.scatter(E.real, E.imag, s=50, marker='o',label='$R=$'+str(R), color='k')\n",
    "\n",
    "for i in np.arange(1,10,0.1):\n",
    "    _, _, E = lqr(P.A, P.B, Q, R*i)\n",
    "    ax.scatter(E.real, E.imag, s=5, marker='.', color='gray')\n",
    "for i in np.arange(10,100,1):\n",
    "    _, _, E = lqr(P.A, P.B, Q, R*i)\n",
    "    ax.scatter(E.real, E.imag, s=5, marker='.', color='gray')\n",
    "for i in np.arange(100,1000,10):\n",
    "    _, _, E = lqr(P.A, P.B, Q, R*i)\n",
    "    ax.scatter(E.real, E.imag, s=5, marker='.', color='gray')\n",
    "for i in np.arange(1000,100000,100):\n",
    "    _, _, E = lqr(P.A, P.B, Q, R*i)\n",
    "    ax.scatter(E.real, E.imag, s=5, marker='.', color='gray')\n",
    "\n",
    "_, _, E = lqr(P.A, P.B, Q, R*10000000)\n",
    "ax.scatter(E.real, E.imag, s=50, marker='*',label='$R=$'+str(R*10000000), color='k') \n",
    "ax.set_xlim(-31.2,1.2)\n",
    "ax.set_ylim(-16.2,16.2)\n",
    "plot_set(ax, 'Re', 'Im', 'best')\n",
    "\n",
    "#fig.savefig(\"lqr_opt_pole_locus.pdf\", transparent=True, bbox_inches=\"tight\", pad_inches=0.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 181,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-10.60926 +0.j -29.806772+0.j]\n"
     ]
    }
   ],
   "source": [
    "_, _, E = lqr(P.A, P.B, Q, R)\n",
    "print(E)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 617,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(2.7755575615628168e-18, 70.74566579604317, 0.0, 4.258373891200274)"
      ]
     },
     "execution_count": 617,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "margin(L)"
   ]
  }
 ],
 "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": 4
}