倒立振子のフィードバックゲインの計算です

detect_feedback_gain.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from control import matlab"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# パラメータ\n",
    "M = 5;\n",
    "m = 0.5;\n",
    "l = 3.0;\n",
    "J = m*l*l/3.0;\n",
    "B_dump = 5.0;\n",
    "C_dump = 0.2;\n",
    "g = 9.8;\n",
    "\n",
    "#行列形式に\n",
    "T = np.array([M+m,m*l,m*l,J+m*l*l]).reshape(2,2)\n",
    "\n",
    "A=np.array([\n",
    "        0,np.linalg.det(T),0,0,\n",
    "        0,-B_dump*(J+m*l*l),-m*m*l*l*g,C_dump*m*l,\n",
    "        0,0,0,np.linalg.det(T),\n",
    "        0,B_dump*l*m,m*g*l*(M+m),-C_dump*(M+m)]).reshape(4,4)\n",
    "A=A/np.linalg.det(T)\n",
    "\n",
    "B=np.array([0,J+m*l*l,0,-m*l]).reshape(4,1)\n",
    "B/=np.linalg.det(T)\n",
    "\n",
    "C=np.array([[1,0,0,0],[0,0,1,0]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "('K =', array([[   0.25795368,    7.08877443,  111.92378883,   69.38212657]]))\n",
      "('e =', array([-1.71270059, -0.6       , -0.4       , -0.3       ]))\n"
     ]
    }
   ],
   "source": [
    "# 欲しい固有値．実部が全部負であれば安定となる．\n",
    "# 小さいほど収束は早くなるが，入力の値が大きくなる\n",
    "poles =  [-0.5, -0.3,-0.4,-0.6]\n",
    "\n",
    "K = -matlab.place(A, B, poles)#マイナスは式との数式上のつじつまを合わせるため\n",
    "\n",
    "e = np.linalg.eigvals(A+B.dot(K))\n",
    "\n",
    "print(\"K =\", K)\n",
    "print(\"e =\", e)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "('G =', array([[  9.7398596 ,   0.30567864],\n",
      "       [ 19.2229346 ,   0.30081657],\n",
      "       [  0.30567864,   8.96145888],\n",
      "       [  6.87245941,  15.09833317]]))\n",
      "('e =', array([-7.        , -6.        , -5.        , -1.71270059]))\n"
     ]
    }
   ],
   "source": [
    "# 欲しい固有値．実部が全部負であれば安定となる．\n",
    "# 小さいほど収束は早くなるが，（たぶん）測定誤差が出てくると思う．\n",
    "poles = [-5, -6,-7,-8]\n",
    "\n",
    "G = matlab.place(A.T, C.T, poles).T\n",
    "\n",
    "e = np.linalg.eigvals(A-G.dot(C))\n",
    "\n",
    "print(\"G =\", G)\n",
    "print(\"e =\", e)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}