Controlability_observeability.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Running Python 3.8 with sympy version 1.6.1\n"
     ]
    }
   ],
   "source": [
    "from sympy import *\n",
    "import sympy as sy\n",
    "from IPython.display import Math\n",
    "# import control\n",
    "print(\"Running Python 3.8 with sympy version\", sy.__version__)\n",
    "# print(\"Running Python 3.8 with control version\", control.__version__)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Explaination\n",
    "\n",
    "_Controlability_ is the ability to change the system state from the initial state to any desired states by controlling the input of the system.\n",
    "\n",
    "_Observability_  is the ability to know the inital state of the system from the output, we only need the homogenous response to determine observability\n",
    "\n",
    "### we use the Kalman test to determine observability and controlability\n",
    "\n",
    "\n",
    "$$ Q_c =  \\left[\\begin{matrix}A & A B & A^{2} B & ... A^{n-1}B\\end{matrix}\\right]$$\n",
    " if determinant of Qc is not zero then system is controllable.\n",
    "\n",
    "\n",
    "\n",
    "$$ Q_0 = \\left[\\begin{matrix} C  \\\\  C A  \\\\ \\vdots \\\\ C A^{n-1} \\end{matrix} \\right] $$\n",
    "\n",
    "or another way of writing it is \n",
    "$$ Q_0 = \\left[\\begin{matrix} C^T & A^T C^T  & \\dots (A^T)^{n-1} C^T \\end{matrix} \\right] $$\n",
    "\n",
    "\n",
    " if determinant of Q0 is not zero then the system is observable. $ x^T $ means the transpose of x.\n",
    "\n",
    "\n",
    "\n",
    "The order of a matrix is m x n where m is the number of rows and n is the number of columns. We use the order of the A matrix when solving.\n",
    "\n",
    "\n",
    "If we dont have a full rank for Qc or Q0 then the system is partially controllable/ observable\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 346,
   "metadata": {},
   "outputs": [],
   "source": [
    "# defining functions we will use\n",
    "\n",
    "def get_qc(a,b):\n",
    "    # order of the matrix m x n\n",
    "    n = a.cols\n",
    "    # qc = (b, a*b, a^2 *b, a^3 *b, ...a^(n-1) * b)\n",
    "    return Matrix([[a**x * b for x in range(n)]])\n",
    "\n",
    "def get_q0_transpose(a,c):\n",
    "    '''It says transpose but this is the main way '''\n",
    "    return get_q0(a,c).T\n",
    "    \n",
    "def get_q0(a,c):\n",
    "    '''This gives you the second way of writing it'''\n",
    "    n = a.cols\n",
    "    return Matrix([[ a.T**x  * c.T for x in range(n)]])\n",
    "        \n",
    "def is_controlabale(A, B):\n",
    "    qc = get_qc(A,B)\n",
    "    d = qc.det()\n",
    "    if d !=0 :\n",
    "        return True\n",
    "    return False\n",
    "\n",
    "def is_observable(a,c):\n",
    "    q0 = get_q0(a,c)\n",
    "    d = q0.det()\n",
    "    if d!=0:\n",
    "        return True\n",
    "    return False\n",
    "\n",
    "def display_new_ABC(a,b,c, S):\n",
    "    s_n = get_inverse(S)\n",
    "    print(\"\\n new A = S^-1 A S = \" )\n",
    "    display(s_n *a * S)\n",
    "    display(\"new B = S^-1 B\", s_n *b )\n",
    "    display(\"new C = c S\",c*S )\n",
    "    \n",
    "def get_inverse(m):\n",
    "    '''\n",
    "    Takes in a matrix t and returns the inverse assuming it exists, the inverse will have it's poles\n",
    "    seperated by a gaussian dividtion by the determinent\n",
    "    Params:\n",
    "        m (list or sympy.Matrix)\n",
    "    Returns:\n",
    "        (sympy.Matrix) Inverted matrix or None if matrix doesnt exist\n",
    "        \n",
    "    '''\n",
    "    m = sy.Matrix(m)\n",
    "    if sy.det(m)!=0:\n",
    "        m = m.inv()\n",
    "        m = m.applyfunc(lambda e: sy.factor(e, gaussian=True))\n",
    "    else:\n",
    "        print(\"Not invertable, det=0\")\n",
    "        return None\n",
    "    return m\n",
    "\n",
    "def get_poles_from_matrix(a, show_steps = False):\n",
    "    '''Takes the A matrix of a system and returns the poles, format is {pole: multiplicity} can be used to determine stability'''\n",
    "    ss=symbols('s')\n",
    "    char_mtx = ss*eye(a.rows) - a\n",
    "    char_eq = char_mtx.det()\n",
    "    if show_steps:\n",
    "        display(char_mtx)\n",
    "        display(char_eq)\n",
    "    return(roots(char_eq))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Examples\n",
    "1. Determine controlability of the following:  $$\\dot{x} = \\left[\\begin{matrix}-1 & -1\\\\1 & 0\\end{matrix}\\right]x + \\left[\\begin{matrix}1\\\\0\\end{matrix}\\right] u $$ \n",
    "\n",
    "<br>\n",
    "\n",
    "2. Determine the observability of the following: $$ \\dot{x} = \\left[\\begin{matrix}-2 & -2\\\\1 & 0\\end{matrix}\\right] x + \\left[\\begin{matrix}1\\\\0\\end{matrix}\\right] u \\\\ y = \\left[\\begin{matrix}1 & 1\\end{matrix}\\right] $$\n",
    "\n",
    "\n",
    "<br>\n",
    "\n",
    "3. Determine the observability and controlability of the following:\n",
    "$$ A = \\left[\\begin{matrix}1 & 2 & -1\\\\0 & 1 & 0\\\\1 & -4 & 3\\end{matrix}\\right] , B = \\left[\\begin{matrix}0\\\\0\\\\1\\end{matrix}\\right], C = \\left[\\begin{matrix}1\\\\-1\\\\1\\end{matrix}\\right]\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'A'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}-1 & -1\\\\1 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-1, -1],\n",
       "[ 1,  0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'B'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1],\n",
       "[0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'is the system controllable?: True'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#Q1 is the following system controllale\n",
    "\n",
    "a = Matrix([[-1,-1],[1,0]])\n",
    "b = Matrix([[1],[0]])\n",
    "display(\"A\", a)\n",
    "display(\"B\", b)\n",
    "ans = is_controlabale(a,b)\n",
    "display(f'is the system controllable?: {ans}')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'is the system observable?: True'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Q2\n",
    "a = Matrix([[-2, -2], [1,0]])\n",
    "b = Matrix([[1],[0]])\n",
    "c = Matrix([[1,1]])\n",
    "# get_q0(a,c)\n",
    "ans = is_observable(a,c)\n",
    "display(f'is the system observable?: {ans}')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 120,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'is the system controllable?: False'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'is the system observable?: False'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#Q3\n",
    "a = Matrix([\n",
    "    [1,2,-1],\n",
    "    [0,1,0],\n",
    "    [1,-4, 3]\n",
    "])\n",
    "b = Matrix([[0], [0], [1]])\n",
    "c = Matrix([[1,-1,1]])\n",
    "display(f'is the system controllable?: {is_controlabale(a,b)}')\n",
    "display(f'is the system observable?: {is_observable(a,c)}')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Controllable and/or Observable component in a non-controllable and/or non-observable system\n",
    "\n",
    "### Finding the Controllable component\n",
    "\n",
    "lets start by a system that is not controllable\n",
    "If the system Qc matrix has a linearly dependant column then the system is not controllable, however, there is a controllable component in it. In the Qc matrix you will have linearly independent columns which are controllable.\n",
    "You can determine if a column is linearly independnt by takeing the reduced row echalon form of the Qc and every column that sticks to the reduced row echalon form then it is an indepeendnt column. You take the reduced row echalon form in sympy using the method `.rref()`\n",
    "\n",
    "The number of controllable components is equal to the rank of the Qc matrix.\n",
    "Now to get the controllable component replace the columns that are not linealy independent with an arbitrary column in the QC, and call the new modified Qc the S matrix, Then we use the Similarity Transormation  to get the new A matrix , and do this to get the rest of the matrices  $$A_{new} =  S^{-1} A S \\\\\n",
    "B_{new} = S^{-1} B \\\\\n",
    "C_{new} = CS$$\n",
    "\n",
    "We then put those new A,B,C matrices in this state space model\n",
    "\n",
    "$$ \\left[\\begin{matrix}\\dot{x_c} \\\\ \\overline{\\dot{x_{c}}} \\end{matrix}\\right]   = \\left[ \\begin{matrix} A_c && A_{c2} \\\\ 0 && \\overline{A_c} \\end{matrix} \\right]\n",
    "\\left[\\begin{matrix} x_c \\\\ \\overline{x_{c}} \\end{matrix}\\right]\n",
    " + \\left[ \\begin{matrix} B_c \\\\ 0 \\end{matrix} \\right] u\n",
    "$$\n",
    "$$\n",
    "Y = \\left[\\begin{matrix}C_c && \\overline{C_c} \\end{matrix}\\right]\n",
    "\\left[\\begin{matrix} x_c \\\\ \\overline{x_{c}} \\end{matrix}\\right]\n",
    "+ Du\n",
    "$$\n",
    "\n",
    "Where \n",
    "$x_c$ = the controllable state vector, the number of controllable vars is equal to the number of linearly independent vecotrs in the Qc\n",
    "\n",
    "$\\overline{x_c}$ the un-controllable state vector\n",
    "\n",
    "$A_c, B_c, C_c$ the parameters of the controllable component\n",
    "\n",
    "$\\overline{A_c}, \\overline{B_c}, \\overline{C_c}$ the parameters of the un-controllable component\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 169,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'is the system controllable?: False'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'This is Qc'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & -3\\\\1 & -3 & 7\\\\-3 & 7 & -15\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0,  1,  -3],\n",
       "[ 1, -3,   7],\n",
       "[-3,  7, -15]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'From the rref we see that the last column in Qc is not linearly independent'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & -2\\\\0 & 1 & -3\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, -2],\n",
       "[0, 1, -3],\n",
       "[0, 0,  0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " \n",
      " We get the S matrix by replacing the non-linearly dependent with an arbitary column [0,0,1]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "'S = '"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & 0\\\\1 & -3 & 0\\\\-3 & 7 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0,  1, 0],\n",
       "[ 1, -3, 0],\n",
       "[-3,  7, 1]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      " new A = S^-1 A S =  Matrix([[0, -2, 1], [1, -3, 0], [0, 0, -3]])\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "'new B = S^-1 B'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1],\n",
       "[0],\n",
       "[0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'new C = c S'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}-6 & 13 & 2\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([[-6, 13, 2]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      " Since the first two columns of Qc are linearly independent and only the 3rd col is dependent we take the first two cols and rows from the new A,B,C and as a controllable compoenent and we take the last col and row as uncontrollable\n",
      "\n",
      " \n",
      " The answer is neatly written below\n"
     ]
    }
   ],
   "source": [
    "#Example\n",
    "a = Matrix([\n",
    "    [0, 1, 0],\n",
    "    [0, 0, 1],\n",
    "    [-6, -11, -6]\n",
    "])\n",
    "\n",
    "b = Matrix([\n",
    "    [0],\n",
    "    [1],\n",
    "    [-3]\n",
    "])\n",
    "c = Matrix([[-1, 0, 2]])\n",
    "display(f'is the system controllable?: {is_controlabale(a,b)}')\n",
    "display(\"This is Qc\" , get_qc(a,b))\n",
    "display(\"From the rref we see that the last column in Qc is not linearly independent\",get_qc(a,b).rref()[0])\n",
    "\n",
    "print(\" \\n We get the S matrix by replacing the non-linearly dependent with an arbitary column [0,0,1]\")\n",
    "S = Matrix([[0, 1, 0], [1, -3, 0], [-3, 7, 1]])\n",
    "display(\"S = \", S)\n",
    "\n",
    "print(\"\\n new A = S^-1 A S = \" ,S**-1 *a * S )\n",
    "display(\"new B = S^-1 B\", S**-1 *b )\n",
    "display(\"new C = c S\",c*S )\n",
    "\n",
    "print(\"\\n Since the first two columns of Qc are linearly independent and only the 3rd col is dependent we take the first two cols and rows from the new A,B,C and as a controllable compoenent and we take the last col and row as uncontrollable\")\n",
    "\n",
    "# x1_dot, x2_dot, x3_dot = symbols(\"\\dot{x_1}, \\dot{x_2}, \\dot{x_3}\")\n",
    "# x1, x2, x3 = symbols(\"x_1, x_2, x_3\")\n",
    "\n",
    "print(\"\\n \\n The answer is neatly written below\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The controllable component is:\n",
    "$$\n",
    "\\left[\\begin{matrix}\\dot{x_1}\\\\ \\dot{x_2}\\end{matrix}\\right] = \\left[\\begin{matrix}0 & 2\\\\1 & 3\\end{matrix}\\right]\\left[\\begin{matrix} x_1 \\\\ x_2 \\end{matrix}\\right] + \\left[\\begin{matrix}1\\\\0\\end{matrix}\\right] u(t)\n",
    "$$\n",
    "\n",
    "$$\n",
    "y(t) = \\left[\\begin{matrix}-6 & 13\\end{matrix}\\right] \\left[\\begin{matrix} x_1 \\\\ x_2 \\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "\n",
    "The unconntrollable component is:\n",
    "$$ \\dot{x_3} = -3x_3 \\\\ y(t) = 2x_2 $$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 182,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'qc'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}4 & -8 & 16\\\\-1 & 2 & -4\\\\2 & -4 & 8\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 4, -8, 16],\n",
       "[-1,  2, -4],\n",
       "[ 2, -4,  8]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & -2 & 4\\\\0 & 0 & 0\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, -2, 4],\n",
       "[0,  0, 0],\n",
       "[0,  0, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'Replace the dependant cols with arbitrary cols, let S be'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}4 & 1 & 0\\\\-1 & 0 & 1\\\\2 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 4, 1, 0],\n",
       "[-1, 0, 1],\n",
       "[ 2, 0, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      " new A = S^-1 A S = \n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}-2 & -2 & -8\\\\0 & -3 & -4\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-2, -2, -8],\n",
       "[ 0, -3, -4],\n",
       "[ 0,  0,  0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'new B = S^-1 B'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1],\n",
       "[0],\n",
       "[0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'new C = c S'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & 2\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([[0, 1, 2]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "answer is neatly written below\n"
     ]
    }
   ],
   "source": [
    "# example 2 \n",
    "a = Matrix([\n",
    "    [-11, -36, 0],\n",
    "    [2,8,1],\n",
    "    [-4,-16, -2]\n",
    "])\n",
    "b = Matrix([\n",
    "    [4],[-1],[2]\n",
    "])\n",
    "c = Matrix([[1,2,-1]])\n",
    "d =1\n",
    "qc = get_qc(a,b)\n",
    "display(\"qc\", qc)\n",
    "display(qc.rref()[0])\n",
    "S = Matrix([\n",
    "    [4, 1, 0],\n",
    "    [-1, 0, 1],\n",
    "    [2,0 , 0]\n",
    "])\n",
    "display(\"Replace the dependant cols with arbitrary cols, let S be\", S)\n",
    "\n",
    "display_new_ABC(a,b,c,S)\n",
    "print(\"answer is neatly written below\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The controllable component is:\n",
    "$$ \\dot{x_1} = -2x_1 + u(t) \\\\ y(t) =  u(t) $$\n",
    "\n",
    "\n",
    "\n",
    "The unconntrollable component is:\n",
    "$$\n",
    "\\left[\\begin{matrix}\\dot{x_2}\\\\ \\dot{x_3}\\end{matrix}\\right] = \\left[\\begin{matrix}-3 & -4\\\\0 & 0\\end{matrix}\\right]\\left[\\begin{matrix} x_2 \\\\ x_3 \\end{matrix}\\right] + \\left[\\begin{matrix}0\\\\0\\end{matrix}\\right] u(t)\n",
    "$$\n",
    "$$\n",
    "y(t) = \\left[\\begin{matrix}1 & 2\\end{matrix}\\right] \\left[\\begin{matrix} x_2 \\\\ x_3 \\end{matrix}\\right] + u(t)\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Finding the Observable compoenent\n",
    "\n",
    "If a system is not observable we can find its obsercable component, this is similar to finding the controllable component above, with few diffrences\n",
    "To find the $S^{-1}$ matrix We take the Qoand replace the dependant rows by arbitrarly rows to make $S^{-1}$ a full rank matrix\n",
    "Then we find the new A,B,C the same way we do for controllability component above.\n",
    "\n",
    "$$ \\left[\\begin{matrix}\\dot{x_o} \\\\ \\overline{\\dot{x_{o}}} \\end{matrix}\\right]   = \\left[ \\begin{matrix} A_o && 0 \\\\ A_{o21} && \\overline{A_o} \\end{matrix} \\right]\n",
    "\\left[\\begin{matrix} x_o \\\\ \\overline{x_{o}} \\end{matrix}\\right]\n",
    " + \\left[ \\begin{matrix} B_o \\\\ \\overline{B_o} \\end{matrix} \\right] u\n",
    "$$\n",
    "$$\n",
    "Y = \\left[\\begin{matrix}C_o && 0 \\end{matrix}\\right]\n",
    "\\left[\\begin{matrix} x_o \\\\ \\overline{x_{o}} \\end{matrix}\\right]\n",
    "+ Du\n",
    "$$\n",
    "\n",
    "Where \n",
    "$x_o$ = the observable state vector, the number of observable vars is equal to the number of linearly independent vecotrs in the Qc\n",
    "\n",
    "$\\overline{x_o}$ the un-observable state vector\n",
    "\n",
    "$A_o, B_o, C_o$ the parameters of the observable component\n",
    "\n",
    "$\\overline{A_o}, \\overline{B_o}, \\overline{C_o}$ the parameters of the un-controllable component\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 247,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " is the system observable? False\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "'Q0 transpose (some books use transpose as the main q0)'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\\\0 & 0 & 21.6 & 0\\\\0 & 0 & 0 & 21.6\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 0,    1,    0],\n",
       "[0, 0,    0,    1],\n",
       "[0, 0, 21.6,    0],\n",
       "[0, 0,    0, 21.6]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " Since the rank is 2 we have 2 independat rows. We use the transpose to turn the rows into columns to simplify computatinos\n",
      "to find independatn rows using rref() we convert the rows to columns so we can find the linearly indepenadt columns of the transpose, which are indepndant rows if we reverse the transpose \n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "'From the Q0 transpose rref we see that the first two cols are independant, meaning that the first two rows of the Q0 are observable and we keep them, and we replace the last 2 rows with arbitrary values to get the the S^-1 to be a full rank marix '"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 21.6 & 0\\\\0 & 1 & 0 & 21.6\\\\0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, 21.6,    0],\n",
       "[0, 1,    0, 21.6],\n",
       "[0, 0,    0,    0],\n",
       "[0, 0,    0,    0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Now to find S^-1 we replace the last 2 rows of Q0 with the arbitrarly chosen rows [0 1 0 0] and [1 0 0 0]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "'S^-1 is'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\\\0 & 1 & 0 & 0\\\\1 & 0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 0, 1, 0],\n",
       "[0, 0, 0, 1],\n",
       "[0, 1, 0, 0],\n",
       "[1, 0, 0, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'so after taking the inverse we get S = '"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & 0 & 1\\\\0 & 0 & 1 & 0\\\\1 & 0 & 0 & 0\\\\0 & 1 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 0, 0, 1],\n",
       "[0, 0, 1, 0],\n",
       "[1, 0, 0, 0],\n",
       "[0, 1, 0, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      " new A = S^-1 A S = \n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & 0 & 0\\\\21.6 & 0 & 0 & 0\\\\-1 & 0 & 0 & 0\\\\0 & 0 & 1 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[   0, 1, 0, 0],\n",
       "[21.6, 0, 0, 0],\n",
       "[  -1, 0, 0, 0],\n",
       "[   0, 0, 1, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'new B = S^-1 B'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0\\\\-2\\\\1\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0],\n",
       "[-2],\n",
       "[ 1],\n",
       "[ 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'new C = c S'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([[1, 0, 0, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The first two rows and columns of the new A,B,C are the observable component and the last two are the non-observable. Answer is written below\n"
     ]
    }
   ],
   "source": [
    "# Example\n",
    "a = Matrix([\n",
    "    [0, 1, 0, 0],\n",
    "    [0, 0, -1, 0],\n",
    "    [0, 0, 0, 1],\n",
    "    [0,0,21.6, 0]\n",
    "])\n",
    "b = Matrix([\n",
    "    [0],\n",
    "    [1],\n",
    "    [0],\n",
    "    [-2]\n",
    "])\n",
    "c = Matrix([[0,0,1,0]])\n",
    "print(f' is the system observable? {is_observable(a,c)}')\n",
    "q0 = get_q0_transpose(a,c)\n",
    "display(\"Q0 transpose (some books use transpose as the main q0)\", q0)\n",
    "print(f' Since the rank is {q0.rank()} we have 2 independat rows. We use the transpose to turn the rows into columns to simplify computatinos')\n",
    "print(\"to find independatn rows using rref() we convert the rows to columns so we can find the linearly indepenadt columns of the transpose, which are indepndant rows if we reverse the transpose \")\n",
    "display(\"From the Q0 transpose rref we see that the first two cols are independant, meaning that the first two rows of the Q0 are observable and we keep them, and we replace the last 2 rows with arbitrary values to get the the S^-1 to be a full rank marix \",q0.T.rref()[0])\n",
    "\n",
    "\n",
    "print(\"Now to find S^-1 we replace the last 2 rows of Q0 with the arbitrarly chosen rows [0 1 0 0] and [1 0 0 0]\")\n",
    "s_neg = Matrix([\n",
    "        [0, 0, 1, 0],\n",
    "        [0, 0, 0, 1],\n",
    "        [0, 1, 0, 0],\n",
    "        [1, 0, 0, 0]\n",
    "])\n",
    "display(\"S^-1 is\", s_neg) \n",
    "S = get_inverse(s_neg)\n",
    "display(\"so after taking the inverse we get S = \", S)\n",
    "display_new_ABC(a,b,c, S)\n",
    "\n",
    "print(\"The first two rows and columns of the new A,B,C are the observable component and the last two are the non-observable. Answer is written below\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The observable compoenent\n",
    "$$ \\left[\\begin{matrix}\\dot{x_1} \\\\ {\\dot{x_{2}}} \\end{matrix}\\right]   = \\left[ \\begin{matrix} 0 && 1 \\\\ 21.6 && 0 \\end{matrix} \\right]\n",
    "\\left[\\begin{matrix} x_1 \\\\ x_2 \\end{matrix}\\right]\n",
    " + \\left[ \\begin{matrix} 0 \\\\ -2 \\end{matrix} \\right] u\n",
    "$$\n",
    "$$\n",
    "Y = \\left[\\begin{matrix}1 && 0 \\end{matrix}\\right]\n",
    "\\left[\\begin{matrix} x_1 \\\\ x_{2} \\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "The nonobservable component\n",
    "$$ \\left[\\begin{matrix}\\dot{x_3} \\\\ {\\dot{x_{4}}} \\end{matrix}\\right]   = \\left[ \\begin{matrix} 0 && 0 \\\\ 1 && 0 \\end{matrix} \\right]\n",
    "\\left[\\begin{matrix} x_3 \\\\ x_4 \\end{matrix}\\right]\n",
    " + \\left[ \\begin{matrix} 1 \\\\ 0 \\end{matrix} \\right] u\n",
    "$$\n",
    "$$\n",
    "Y = \\left[\\begin{matrix}1 && 0 \\end{matrix}\\right]\n",
    "\\left[\\begin{matrix} x_3 \\\\ x_{4} \\end{matrix}\\right]\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 257,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'q0'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & -3\\\\1 & -3 & 7\\\\-3 & 7 & -15\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0,  1,  -3],\n",
       "[ 1, -3,   7],\n",
       "[-3,  7, -15]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'finding the depandant rows'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & -2\\\\0 & 1 & -3\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, -2],\n",
       "[0, 1, -3],\n",
       "[0, 0,  0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Only the last row is depandent so replace it with the arbitrarly chosen [0 0 1] row\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "'S^-1'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & -3\\\\1 & -3 & 7\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0,  1, -3],\n",
       "[1, -3,  7],\n",
       "[0,  0,  1]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'Taking its inverse we get S = '"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}3 & 1 & 2\\\\1 & 0 & 3\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[3, 1, 2],\n",
       "[1, 0, 3],\n",
       "[0, 0, 1]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      " new A = S^-1 A S = \n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & 0\\\\-2 & -3 & 0\\\\1 & 0 & -3\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0,  1,  0],\n",
       "[-2, -3,  0],\n",
       "[ 1,  0, -3]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'new B = S^-1 B'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}-6\\\\13\\\\2\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-6],\n",
       "[13],\n",
       "[ 2]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "'new C = c S'"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([[1, 0, 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      " answer written below\n"
     ]
    }
   ],
   "source": [
    "## Example 2\n",
    "a = Matrix([\n",
    "    [0, 0, -6],\n",
    "    [1, 0, -11],\n",
    "    [0, 1, -6]\n",
    "])\n",
    "b = Matrix([\n",
    "    [-1],\n",
    "    [0],\n",
    "    [2]\n",
    "])\n",
    "c = Matrix([[0, 1, -3]])\n",
    "\n",
    "qo = get_q0_transpose(a,c)\n",
    "display(\"q0\", qo)\n",
    "display(\"finding the depandant rows\", qo.T.rref()[0])\n",
    "print(\"Only the last row is depandent so replace it with the arbitrarly chosen [0 0 1] row\")\n",
    "s_neg = Matrix([\n",
    "    qo.row(0),\n",
    "    qo.row(1),\n",
    "    [0,0,1]\n",
    "])\n",
    "s = get_inverse(s_neg)\n",
    "display(\"S^-1\", s_neg, \"Taking its inverse we get S = \", s)\n",
    "display_new_ABC(a,b,c,s)\n",
    "print(\"\\n answer written below\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Observable component\n",
    "$$ \\left[\\begin{matrix}\\dot{x_1} \\\\ {\\dot{x_{2}}} \\end{matrix}\\right]   = \\left[ \\begin{matrix} 0 && 1 \\\\ -2 && -3 \\end{matrix} \\right]\n",
    "\\left[\\begin{matrix} x_1 \\\\ x_2 \\end{matrix}\\right]\n",
    " + \\left[ \\begin{matrix} -6 \\\\ 13 \\end{matrix} \\right] u\n",
    "$$\n",
    "$$\n",
    "Y = \\left[\\begin{matrix}1 && 0 \\end{matrix}\\right]\n",
    "\\left[\\begin{matrix} x_1 \\\\ x_{2} \\end{matrix}\\right]\n",
    "$$\n",
    "\n",
    "non-observable component\n",
    "$$ \n",
    "\\dot{x_3} = -3x_3 + 2u \\\\\n",
    "y = 0\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}