{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Ho-Kalman Identification Example" ] }, { "cell_type": "code", "execution_count": 1, "metadata": { "init_cell": true }, "outputs": [], "source": [ "%load_ext autoreload\n", "%autoreload 2\n", "%matplotlib inline\n", "\n", "import control as ctrl\n", "import matplotlib.pyplot as plt\n", "import scipy as sp\n", "import scipy.linalg as la\n", "import vibrationtesting as vt\n", "import scipy.signal as sig" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Considers a system \n", "\n", "$$\\ddot{x} + 2 \\dot{x} + 100 x = f(t)$$\n", "with a displacement sensor. \n", "\n", "The state space representation is \n", "\n", "$$\\dot{\\mathbf{z}} = A \\mathbf{z} + B u$$\n", "\n", "$$y = C \\mathbf{z} + D u$$ \n", "\n", "where \n", "\n", "$$A = \n", "\\begin{bmatrix}\n", "0&1\\\\\n", "-100&-2\n", "\\end{bmatrix}\n", ",\\quad\n", "B = \n", "\\begin{bmatrix}0\\\\1\\end{bmatrix}\n", ",\\quad\n", "C = \\begin{bmatrix}1&0\\end{bmatrix}\n", ", \\text{ and }\n", "D = [0]$$" ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "sample_freq = 1e3\n", "A = sp.array([[0, 1],\\\n", " [-100, 2]])\n", "B = sp.array([[0], [1]])\n", "C = sp.array([[1, 0]])\n", "D = sp.array([[0]])\n", "sys = ctrl.ss(A, B, C, D)\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "For this example, we will generate an impulse response for only 4 discrete times." ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([ 0. , 0.16666667, 0.33333333, 0.5 , 0.66666667,\n", " 0.83333333])" ] }, "execution_count": 3, "metadata": {}, "output_type": "execute_result" } ], "source": [ "t = sp.linspace(0, 1, num = 6, endpoint = False)\n", "dt = t[1]\n", "t" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The impulse response for a second order underdamped system is known to be\n", "\n", "$$h(t) = \\frac{1}{\\omega_d}e^{-\\zeta \\omega_n t}\\sin\\left(\\omega_d t\\right)$$" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "omega_n = sp.sqrt(100)\n", "zeta = 2/(2*omega_n)\n", "omega_d = omega_n * sp.sqrt(1 - zeta**2)" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([ 0. , 0.08474903, -0.01254052, -0.05886968, 0.01769677,\n", " 0.03956333])" ] }, "execution_count": 5, "metadata": {}, "output_type": "execute_result" } ], "source": [ "h = 1/omega_d * sp.exp(-zeta*omega_n*t)*sp.sin(omega_d*t)\n", "h" ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [ { "data": { "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYEAAAD/CAYAAAAXBmohAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n0GEUAQDoMIoAAHQYRQAAOowiAAAd9v+NmCh/BAsPnwAAAABJRU5ErkJggg==\n", "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "plt.plot(t,h,'.')\n", "plt.axis([0,1,-0.1,0.1])\n", "plt.grid()" ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([[ 0. , 0.08474903],\n", " [ 0.08474903, -0.01254052],\n", " [-0.01254052, -0.05886968],\n", " [-0.05886968, 0.01769677]])" ] }, "execution_count": 6, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Hankel_0 = sp.vstack((h[0:-2],h[1:-1]))\n", "Hankel_0 = Hankel_0.T\n", "Hankel_0" ] }, { "cell_type": "code", "execution_count": 7, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([ 0.08474903, -0.01254052, -0.05886968, 0.01769677])" ] }, "execution_count": 7, "metadata": {}, "output_type": "execute_result" } ], "source": [ "h[1:-1]" ] }, { "cell_type": "code", "execution_count": 8, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([-0.01254052, -0.05886968, 0.01769677, 0.03956333])" ] }, "execution_count": 8, "metadata": {}, "output_type": "execute_result" } ], "source": [ "h[2:]" ] }, { "cell_type": "code", "execution_count": 9, "metadata": { "scrolled": true }, "outputs": [ { "data": { "text/plain": [ "array([[ 0.08474903, -0.01254052],\n", " [-0.01254052, -0.05886968],\n", " [-0.05886968, 0.01769677],\n", " [ 0.01769677, 0.03956333]])" ] }, "execution_count": 9, "metadata": {}, "output_type": "execute_result" } ], "source": [ "Hankel_1 = sp.vstack((h[1:-1],h[2:]))\n", "Hankel_1 = Hankel_1.T\n", "Hankel_1" ] }, { "cell_type": "code", "execution_count": 10, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[[ 0.56941225 0.57615451]\n", " [-0.59214005 0.56070011]\n", " [-0.32038129 -0.49580091]\n", " [ 0.47169449 -0.32839431]]\n", "[[-0.66563569 0.74627684]\n", " [ 0.74627684 0.66563569]]\n" ] } ], "source": [ "U, sig, Vt = la.svd(Hankel_0)\n", "V = Vt.T\n", "U = U[:,:2]\n", "print(U)\n", "V = V[:,:2]\n", "print(V)" ] }, { "cell_type": "code", "execution_count": 11, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[[ 0.11107284 0. ]\n", " [ 0. 0.0979112 ]]\n" ] } ], "source": [ "sig = sp.diag(sig)\n", "print(sig)" ] }, { "cell_type": "code", "execution_count": 12, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[[-0.22322281 0.85303151]\n", " [-0.85967388 0.07525037]]\n" ] } ], "source": [ "A_d = la.inv(sp.sqrt(sig))@U.T@Hankel_1@V@la.inv(sp.sqrt(sig))\n", "print(A_d)" ] }, { "cell_type": "code", "execution_count": 13, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[-0.07398622+0.84324217j -0.07398622-0.84324217j]\n", "[[ 0.12298924-0.69493489j 0.12298924+0.69493489j]\n", " [ 0.70847664+0.j 0.70847664-0.j ]]\n" ] } ], "source": [ "lam_d, vec = la.eig(A_d)\n", "print(lam_d)\n", "print(vec)" ] }, { "cell_type": "code", "execution_count": 14, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[[-0.22322281 0.85303151]\n", " [-0.85967388 0.07525037]]\n", "[[-0.22322281 +0.00000000e+00j 0.85303151 +2.77555756e-17j]\n", " [-0.85967388 -6.93889390e-18j 0.07525037 +0.00000000e+00j]]\n" ] } ], "source": [ "# This should be the same as A_d\n", "print(A_d)\n", "print(vec@sp.diag(lam_d)@la.inv(vec))" ] }, { "cell_type": "code", "execution_count": 15, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "array([-1.+9.94987437j, -1.-9.94987437j])" ] }, "execution_count": 15, "metadata": {}, "output_type": "execute_result" } ], "source": [ "lam = sp.log(lam_d)/dt\n", "lam" ] }, { "cell_type": "code", "execution_count": 16, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "The undamped natural frequency is 10.0 rad/sec.\n", "The damping ratio is 0.09999999999999998.\n" ] } ], "source": [ "# These are the continuous time eigenvalues\n", "print('The undamped natural frequency is {} rad/sec.'.format(abs(lam[0])))\n", "print('The damping ratio is {}.'.format(-sp.real(lam[0])/abs(lam[0])))\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The identified state matrix is" ] }, { "cell_type": "code", "execution_count": 17, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[[ -2.76092394 10.06538424]\n", " [-10.1437611 0.76092394]]\n" ] } ], "source": [ "A = la.logm(A_d)/dt\n", "print(A)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "which is the system the result in a balanced realization form. " ] }, { "cell_type": "code", "execution_count": 18, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[[-0.22184035]\n", " [ 0.23351573]]\n" ] } ], "source": [ "# The discrete input matrix is\n", "B_d = sp.sqrt(sig)@V.T[:,0].T.reshape((2,1))\n", "print(B_d)" ] }, { "cell_type": "code", "execution_count": 19, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[[-2.58036274]\n", " [-0.22677789]]\n" ] } ], "source": [ "# The continuous input matrix is\n", "B = la.solve((A_d - sp.eye(2)), A) @ B_d\n", "print(B)" ] }, { "cell_type": "code", "execution_count": 20, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "[ 0.18977139 0.18028315]\n" ] } ], "source": [ "C = (U @ sp.sqrt(sig))[0,:]\n", "print(C)" ] }, { "cell_type": "code", "execution_count": 21, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "0.0" ] }, "execution_count": 21, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# Of course, D is\n", "D = h[0]\n", "D" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "hide_input": false, "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.6.3" }, "latex_envs": { "bibliofile": "biblio.bib", "cite_by": "apalike", "current_citInitial": 1, "eqLabelWithNumbers": true, "eqNumInitial": 0 }, "toc": { "nav_menu": {}, "number_sections": true, "sideBar": true, "skip_h1_title": false, "toc_cell": false, "toc_position": {}, "toc_section_display": "block", "toc_window_display": false }, "varInspector": { "cols": { "lenName": 16, "lenType": 16, "lenVar": 40 }, "kernels_config": { "python": { "delete_cmd_postfix": "", "delete_cmd_prefix": "del ", "library": "var_list.py", "varRefreshCmd": "print(var_dic_list())" }, "r": { "delete_cmd_postfix": ") ", "delete_cmd_prefix": "rm(", "library": "var_list.r", "varRefreshCmd": "cat(var_dic_list()) " } }, "types_to_exclude": [ "module", "function", "builtin_function_or_method", "instance", "_Feature" ], "window_display": false } }, "nbformat": 4, "nbformat_minor": 1 }