Destabilizing LQR.ipynb

{
 "metadata": {
  "name": "",
  "signature": "sha256:8f67fe418f8989fe4cb9cd61f043bc49c93d2ad81b4f3f10a3eaa3eb435d9e25"
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "%pylab inline\n",
      "import control\n",
      "import scipy.linalg"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "Populating the interactive namespace from numpy and matplotlib\n"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stderr",
       "text": [
        "WARNING: pylab import has clobbered these variables: ['sys', 'gamma']\n",
        "`%matplotlib` prevents importing * from pylab and numpy\n"
       ]
      }
     ],
     "prompt_number": 155
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "eps = 0.000001\n",
      "A = array([[0, 1], [-1, -1]])\n",
      "B = array([[0],[1]])\n",
      "C = eye(2)\n",
      "D = array([[0],[0]])\n",
      "sys = control.ss(A, B, C, D)\n",
      "sys.damp()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 156,
       "text": [
        "(array([ 1.,  1.]),\n",
        " array([ 0.5,  0.5]),\n",
        " array([-0.5+0.8660254j, -0.5-0.8660254j]))"
       ]
      }
     ],
     "prompt_number": 156
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "K = control.care(sys.A, sys.B, eye(2), eye(1))[2]\n",
      "sys_closed = control.feedback(sys, control.ss(0,[0,0],0,K))\n",
      "sys_closed.damp()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 157,
       "text": [
        "(array([ 1.18920712,  1.18920712]),\n",
        " array([ 0.70710678,  0.70710678]),\n",
        " array([-0.84089642+0.84089642j, -0.84089642-0.84089642j]))"
       ]
      }
     ],
     "prompt_number": 157
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "Q = diag([1, -1])\n",
      "R = 0.01*eye(1)\n",
      "P = scipy.linalg.solve_continuous_are(sys.A, sys.B, Q, R)\n",
      "K_destab = -linalg.inv(R).dot(B.T).dot(P)\n",
      "sys_destab = control.feedback(\n",
      "    sys_closed, control.ss(0,[0,0],0,K_destab))\n",
      "t, y, x = control.forced_response(\n",
      "    sys_destab,\n",
      "    T=linspace(0,10, 1000), X0=[0.001,0])\n",
      "plot(t, x.T)\n",
      "u = K_destab.dot(x)\n",
      "plot(t, u.T, '--')\n",
      "sys_destab.damp()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 176,
       "text": [
        "(array([ 1.96850431,  0.71328852]),\n",
        " array([ 1.,  1.]),\n",
        " array([-1.96850431, -0.71328852]))"
       ]
      },
      {
       "metadata": {},
       "output_type": "display_data",
       "png": "iVBORw0KGgoAAAANSUhEUgAAAZAAAAEACAYAAACd2SCPAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n",
       "text": [
        "<matplotlib.figure.Figure at 0x50ff350>"
       ]
      }
     ],
     "prompt_number": 176
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "Ac = sys_closed.A\n",
      "Bc = sys_closed.B\n",
      "Ac.T.dot(P) + P.dot(Ac) - P.dot(Bc).dot(linalg.inv(R)).dot(Bc.T).dot(P) + Q"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 5,
       "text": [
        "matrix([[ -1.44328993e-15,  -1.11022302e-16],\n",
        "        [  2.88657986e-15,   8.88178420e-16]])"
       ]
      }
     ],
     "prompt_number": 5
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "P = scipy.linalg.solve_continuous_are(Ar, Br, Q, R)\n",
      "P"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 10,
       "text": [
        "array([[  5.99999200e-09,  -3.99999800e-03],\n",
        "       [  1.10000025e-14,  -4.00001000e-09]])"
       ]
      }
     ],
     "prompt_number": 10
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "Ar.T.dot(P) + P.dot(Ar) - P.dot(Br).dot(linalg.inv(R)).dot(Br.T).dot(P) + Q"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 11,
       "text": [
        "array([[ -5.39801537e-12,   1.28056854e-09],\n",
        "       [  3.00001109e-12,   2.22044605e-15]])"
       ]
      }
     ],
     "prompt_number": 11
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "A = array([[-1]])\n",
      "B = array([[1]])\n",
      "C = array([[1]])\n",
      "D = array([[1]])\n",
      "Q=array([[-1]])\n",
      "R=array([[1]])\n",
      "sys = control.ss(A, B, C, D)\n",
      "P = scipy.linalg.solve_continuous_are(A, B, Q, R)\n",
      "A.T.dot(P) + P.dot(A) - P.dot(B).dot(linalg.inv(R)).dot(B.T).dot(P) + Q"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 153,
       "text": [
        "array([[-4.]])"
       ]
      }
     ],
     "prompt_number": 153
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "linalg.inv(array([[1]]))"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 154,
       "text": [
        "array([[ 1.]])"
       ]
      }
     ],
     "prompt_number": 154
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "%pylab inline\n",
      "import picos as pic\n",
      "import cvxopt as cvx\n",
      "import numpy as np\n",
      "import scipy.linalg\n",
      "import scipy.optimize\n",
      "import scipy.integrate\n",
      "def solve_bounded_disturbance(A, B, C, D):\n",
      "    \n",
      "    def solve_lmi(A_data, B_data, C_data, D_data, alpha, verbose=False):\n",
      "        sdp = pic.Problem()\n",
      "\n",
      "        # shape\n",
      "        n_x = A_data.shape[0]\n",
      "        n_u = B_data.shape[1]\n",
      "        \n",
      "        # variables\n",
      "        P = sdp.add_variable('P', (n_x, n_x), vtype='symmetric')\n",
      "        alpha = pic.new_param('alpha', alpha)\n",
      "        mu_1 = sdp.add_variable('mu_1')\n",
      "        mu_2 = sdp.add_variable('mu_2')\n",
      "\n",
      "        # parameters\n",
      "        A = pic.new_param('A', cvx.matrix(A_data))\n",
      "        B = pic.new_param('B', cvx.matrix(B_data))\n",
      "        C = pic.new_param('C', cvx.matrix(C_data))\n",
      "        D = pic.new_param('D', cvx.matrix(D_data))\n",
      "        I_n_u = pic.new_param('I', cvx.sparse(cvx.matrix(np.eye(n_u))))\n",
      "        I_n_x = pic.new_param('I', cvx.sparse(cvx.matrix(np.eye(n_x))))\n",
      "\n",
      "        eps = 1e-10\n",
      "        sdp.add_constraint(\n",
      "            (P*A + A.T*P + 2*alpha*P & P*B) //\n",
      "            (B.T*P &  -2*alpha*mu_1*I_n_u)  << -eps)\n",
      "        sdp.add_constraint(\n",
      "            (C.T*C - P & C.T*D) //\n",
      "            (D.T*C & D.T*D - mu_2*I_n_u)  << -eps)\n",
      "        sdp.add_constraint(P >> eps*I_n_x)\n",
      "        sdp.add_constraint(mu_1 << 0.01)\n",
      "        sdp.add_constraint(mu_2 << 0.01)\n",
      "        sdp.set_objective('max', mu_1 + mu_2)\n",
      "        try:\n",
      "            sdp.solve(verbose=verbose)\n",
      "            mu_1 =  sdp.variables['mu_1'].value[0,0]\n",
      "            mu_2 =  sdp.variables['mu_2'].value[0,0]\n",
      "            gamma = np.sqrt(mu_1 + mu_2)\n",
      "        except Exception as e:\n",
      "            print e\n",
      "            gamma = -1\n",
      "            \n",
      "        return gamma, sdp\n",
      "    \n",
      "    # we use fmin to solve a line search problem in alpha for minimum gamma\n",
      "    print('line search')\n",
      "    alpha_opt  = scipy.optimize.fmin(lambda alpha: -solve_lmi(A, B, C, D, alpha)[0], x0=-1)\n",
      "    \n",
      "    gamma, sdp = solve_lmi(A, B, C, D, alpha_opt)\n",
      "    print sdp\n",
      "    \n",
      "    if sdp.status == 'optimal':\n",
      "        P = sdp.variables['P'].value\n",
      "        mu_1 =  sdp.variables['mu_1'].value[0,0]\n",
      "        mu_2 =  sdp.variables['mu_2'].value[0,0]\n",
      "        print 'optimal alpha: ', alpha_opt\n",
      "        print 'gamma: ', gamma\n",
      "        print 'mu_1: ', mu_1\n",
      "        print 'mu_2: ', mu_2\n",
      "        print 'P: ',  P\n",
      "    else:\n",
      "        sdp = solve_lmi(A, B, C, D, alpha_opt, verbose=True)\n",
      "        raise RuntimeError('Optimization failed')\n",
      "        \n",
      "    return sdp, P, alpha_opt, gamma"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "Populating the interactive namespace from numpy and matplotlib\n"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stderr",
       "text": [
        "WARNING: pylab import has clobbered these variables: ['gamma']\n",
        "`%matplotlib` prevents importing * from pylab and numpy\n"
       ]
      }
     ],
     "prompt_number": 151
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "A = array([[0,1],[-1,-1]])\n",
      "B = array([[0],[1]])\n",
      "C = eye(2)\n",
      "D = array([[0], [0]])\n",
      "sdp, P, alpha, gamma = solve_bounded_disturbance(A, B, C, D)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "line search\n",
        "Warning: Maximum number of function evaluations has been exceeded."
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "---------------------\n",
        "optimization problem  (SDP):\n",
        "5 variables, 0 affine constraints, 17 vars in 5 SD cones\n",
        "\n",
        "P \t: (2, 2), symmetric\n",
        "mu_2 \t: (1, 1), continuous\n",
        "mu_1 \t: (1, 1), continuous\n",
        "\n",
        "\tmaximize mu_1 + mu_2\n",
        "such that\n",
        "  [P*A + A.T*P + 2.0*alpha*P,P*B;B.T*P,( ( ( -2.0 )*alpha )*mu_1 )*I] \u227c |-1e-10|\n",
        "  [C.T*C -P,C.T*D;D.T*C,D.T*D -mu_2*I] \u227c |-1e-10|\n",
        "  P \u227d ( 1e-10 )*I\n",
        "  mu_1 \u227c 0.01\n",
        "  mu_2 \u227c 0.01\n",
        "---------------------\n",
        "optimal alpha:  [-1.]\n",
        "gamma:  nan\n",
        "mu_1:  -0.119521546231\n",
        "mu_2:  0.0099999994918\n",
        "P:  [ 5.74e+00 -1.43e+00]\n",
        "[-1.43e+00  1.43e+00]\n",
        "\n"
       ]
      }
     ],
     "prompt_number": 152
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [],
     "language": "python",
     "metadata": {},
     "outputs": []
    }
   ],
   "metadata": {}
  }
 ]
}