Calculation of PID parameters with sympy (nonlinear system of equations)

PID_parameters.ipynb

{
 "metadata": {
  "name": "",
  "signature": "sha256:0cb0d9e1bb5e585e4972400cc388a8b735d08e40b33465f13063ea47a36b3185"
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "%load_ext displaytools\n",
      "# optional but usefull\n",
      "# see https://github.com/cknoll/displaytools"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 1
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "import sympy\n",
      "\n",
      "s, tau_c, tau_1, tau_2, phi, K = sympy.symbols('s, tau_c, tau_1, tau_2, phi, K')\n",
      "\n",
      "target = (s**2*tau_1*tau_2 + s*tau_1 + s*tau_2 + 1)/(K*s*(-phi + tau_c))\n",
      "\n",
      "#K_C, tau_I, tau_D = cparams = sympy.symbols('K_C, tau_I, tau_D', real=True)\n",
      "\n",
      "# Make the unknown parameters better vissible, and allow subs\n",
      "K_C, tau_I, tau_D = XX1, XX2, XX3  = XX = sympy.Matrix(sympy.symbols('XX1:4', real=True))\n",
      "\n",
      "PID = K_C*(1 + 1/(tau_I*s) + tau_D*s)\n",
      "\n",
      "#eq = (target - PID).together()\n",
      "#eq *= sympy.denom(eq).simplify()\n",
      "\n",
      "eq = (target - PID).as_numer_denom()[0]\n",
      "eq = sympy.poly(eq, s)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 2
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "C = sympy.Matrix(eq.coeffs()) ##:"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "display_data",
       "text": [
        "C := Matrix([\n",
        "[K*XX1*XX2*XX3*phi - K*XX1*XX2*XX3*tau_c + XX2*tau_1*tau_2],\n",
        "[  K*XX1*XX2*phi - K*XX1*XX2*tau_c + XX2*tau_1 + XX2*tau_2],\n",
        "[                            K*XX1*phi - K*XX1*tau_c + XX2]])"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "---\n"
       ]
      }
     ],
     "prompt_number": 3
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Obviosly, the first two equations are nonlinear while the last is linear."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# directly try to solve all eqations -> no success\n",
      "sympy.solve(eq.coeffs(), XX)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 4,
       "text": [
        "[]"
       ]
      }
     ],
     "prompt_number": 4
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# solve linear equation w.r.t. XX1 ...\n",
      "sol1 = sympy.solve(C[-1], XX1, dict=True)[0] ##:"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "display_data",
       "text": [
        "sol1 := {XX1: -XX2/(K*(phi - tau_c))}"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "---\n"
       ]
      }
     ],
     "prompt_number": 5
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# ... and substitue it into the equations\n",
      "C2 = sympy.simplify(C.subs(sol1)) ##:"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "display_data",
       "text": [
        "C2 := Matrix([\n",
        "[XX2*(-XX2*XX3 + tau_1*tau_2)],\n",
        "[  XX2*(-XX2 + tau_1 + tau_2)],\n",
        "[                           0]])"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "---\n"
       ]
      }
     ],
     "prompt_number": 6
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# now solve the remaining bilinear equations"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 7
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "sol2a, sol2b = sympy.solve(C2, XX[1:], dict=True) ##"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "display_data",
       "text": [
        "{XX3: tau_1*tau_2/(tau_1 + tau_2), XX2: 0}"
       ]
      },
      {
       "metadata": {},
       "output_type": "display_data",
       "text": [
        "{XX3: tau_1*tau_2/(tau_1 + tau_2), XX2: tau_1 + tau_2}"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "---\n"
       ]
      }
     ],
     "prompt_number": 9
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# apply both partial solutions\n",
      "\n",
      "XX_sol = XX.subs(sol1).subs(sol2b)##:"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "display_data",
       "text": [
        "XX_sol := Matrix([\n",
        "[-(tau_1 + tau_2)/(K*(phi - tau_c))],\n",
        "[                     tau_1 + tau_2],\n",
        "[       tau_1*tau_2/(tau_1 + tau_2)]])"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "---\n"
       ]
      }
     ],
     "prompt_number": 10
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "### Proof"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "booksolution = {K_C: 1/K*(tau_1 + tau_2)/(tau_c - phi), tau_I: tau_1 + tau_2,  tau_D: tau_1*tau_2/(tau_1 + tau_2)} ##:"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "display_data",
       "text": [
        "booksolution := {XX3: tau_1*tau_2/(tau_1 + tau_2),\n",
        " XX2: tau_1 + tau_2,\n",
        " XX1: (tau_1 + tau_2)/(K*(-phi + tau_c))}"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "---\n"
       ]
      }
     ],
     "prompt_number": 11
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "sympy.simplify(XX_sol - XX.subs(booksolution))"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 12,
       "text": [
        "Matrix([\n",
        "[0],\n",
        "[0],\n",
        "[0]])"
       ]
      }
     ],
     "prompt_number": 12
    }
   ],
   "metadata": {}
  }
 ]
}