Koopman.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "dfe1f9e3-6069-4928-b174-2cde47061623",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "339dc294-85aa-4c8a-b4f6-961eaaeda8dc",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = sympy.symbols('t')\n",
    "e_x = sympy.Function('e_x')(t)\n",
    "e_y = sympy.Function('e_y')(t)\n",
    "e_theta = sympy.Function('e_theta')(t)\n",
    "e = sympy.Matrix([e_x, e_y, e_theta]) # error in SE(2)\n",
    "v_x, v_y, omega = sympy.symbols('v_x, v_y, omega') # inputs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "b5e975bf-102b-4eab-bf6c-b7f537b69d2d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} & \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)}}{2}\\\\- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} & \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[e_theta(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))),                                       e_theta(t)/2],\n",
       "[                                     -e_theta(t)/2, e_theta(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))]])"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A = sympy.sin(e_theta)/e_theta\n",
    "B = (1 - sympy.cos(e_theta))/e_theta\n",
    "V = sympy.Matrix([\n",
    "    [A, -B],\n",
    "    [B, A]])\n",
    "V_inv = V.inv()\n",
    "V_inv.simplify()\n",
    "V_inv"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "9543b6f4-e9af-4776-9545-88d36814d6fa",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\\\operatorname{e_{\\theta}}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))],\n",
       "[-e_theta(t)*e_x(t)/2 + e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))],\n",
       "[                                                                      e_theta(t)]])"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g = sympy.Matrix.vstack(V_inv@e[:2, 0], sympy.Matrix([e_theta])) # measurements: log(e), error in Lie algebra\n",
    "g"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "a7756e59-77a2-4850-81e0-06779731e85b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{y}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{x}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - \\frac{2 \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin^{2}{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{\\left(2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}\\right)^{2}}\\\\- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{x}}{\\left(t \\right)}}{2} - \\frac{\\operatorname{e_{x}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{y}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - \\frac{2 \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin^{2}{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{\\left(2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}\\right)^{2}}\\\\\\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ e_theta(t)*Derivative(e_y(t), t)/2 + e_y(t)*Derivative(e_theta(t), t)/2 + e_theta(t)*e_x(t)*cos(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) + e_theta(t)*sin(e_theta(t))*Derivative(e_x(t), t)/(2 - 2*cos(e_theta(t))) + e_x(t)*sin(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) - 2*e_theta(t)*e_x(t)*sin(e_theta(t))**2*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t)))**2],\n",
       "[-e_theta(t)*Derivative(e_x(t), t)/2 - e_x(t)*Derivative(e_theta(t), t)/2 + e_theta(t)*e_y(t)*cos(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) + e_theta(t)*sin(e_theta(t))*Derivative(e_y(t), t)/(2 - 2*cos(e_theta(t))) + e_y(t)*sin(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) - 2*e_theta(t)*e_y(t)*sin(e_theta(t))**2*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t)))**2],\n",
       "[                                                                                                                                                                                                                                                                                                                                                                                         Derivative(e_theta(t), t)]])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g_dot = g.diff(t)\n",
    "g_dot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "8b5d760c-3346-48f0-9970-6b5aa3171d11",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{Derivative(e_x(t), t): omega*e_y(t) + v_x*(cos(e_theta(t)) - 1) - v_y*sin(e_theta(t)),\n",
       " Derivative(e_y(t), t): -omega*e_x(t) + v_x*sin(e_theta(t)) + v_y*(cos(e_theta(t)) - 1),\n",
       " Derivative(e_theta(t), t): 0}"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "eoms = {\n",
    "    e_x.diff(t): v_x*(sympy.cos(e_theta) - 1) - v_y*sympy.sin(e_theta) + omega*e_y,\n",
    "    e_y.diff(t): v_x*sympy.sin(e_theta) + v_y*(sympy.cos(e_theta) - 1) - omega*e_x,\n",
    "    e_theta.diff(t): 0}\n",
    "eoms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "a96ff02e-ba88-4cec-ba5e-566299a95484",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2} + \\frac{v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - \\frac{v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2} - \\frac{v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} - \\frac{v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin^{2}{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - \\frac{v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2} + \\frac{v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin^{2}{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2} + \\frac{v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - \\frac{v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-omega*e_theta(t)*e_x(t)/2 + omega*e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))) + v_x*e_theta(t)*sin(e_theta(t))/2 + v_x*e_theta(t)*sin(e_theta(t))*cos(e_theta(t))/(2 - 2*cos(e_theta(t))) - v_x*e_theta(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))) + v_y*e_theta(t)*cos(e_theta(t))/2 - v_y*e_theta(t)/2 - v_y*e_theta(t)*sin(e_theta(t))**2/(2 - 2*cos(e_theta(t)))],\n",
       "[-omega*e_theta(t)*e_y(t)/2 - omega*e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))) - v_x*e_theta(t)*cos(e_theta(t))/2 + v_x*e_theta(t)/2 + v_x*e_theta(t)*sin(e_theta(t))**2/(2 - 2*cos(e_theta(t))) + v_y*e_theta(t)*sin(e_theta(t))/2 + v_y*e_theta(t)*sin(e_theta(t))*cos(e_theta(t))/(2 - 2*cos(e_theta(t))) - v_y*e_theta(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))],\n",
       "[                                                                                                                                                                                                                                                                                                                                                                                  0]])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g_dot.subs(eoms).expand()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "149210e1-35c8-462f-8724-c766cbe7df07",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} & - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} & \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} - \\omega \\operatorname{e_{x}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + \\omega \\operatorname{e_{x}}{\\left(t \\right)} - \\omega \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2 v_{y} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + 2 v_{y}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)}\\\\\\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)} & - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} & \\frac{- \\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} + \\omega \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - \\omega \\operatorname{e_{y}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + \\omega \\operatorname{e_{y}}{\\left(t \\right)} + 2 v_{x} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2 v_{x}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)}\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                       -omega*e_theta(t)/2, -omega*e_theta(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2),  (omega*e_theta(t)*e_y(t) - omega*e_x(t)*cos(e_theta(t)) + omega*e_x(t) - omega*e_y(t)*sin(e_theta(t)) - 2*v_y*cos(e_theta(t)) + 2*v_y)/(2*(cos(e_theta(t)) - 1))],\n",
       "[omega*e_theta(t)*sin(e_theta(t))/(2*(cos(e_theta(t)) - 1)),                                       -omega*e_theta(t)/2, (-omega*e_theta(t)*e_x(t) + omega*e_x(t)*sin(e_theta(t)) - omega*e_y(t)*cos(e_theta(t)) + omega*e_y(t) + 2*v_x*cos(e_theta(t)) - 2*v_x)/(2*(cos(e_theta(t)) - 1))],\n",
       "[                                                         0,                                                         0,                                                                                                                                                                 0]])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A = g_dot.subs(eoms).jacobian(e)\n",
    "A.simplify()\n",
    "A\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "94b431a2-0b34-4492-9cce-274e7766e9b6",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & - \\omega & v_{y}\\\\\\omega & 0 & - v_{x}\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[    0, -omega,  v_y],\n",
       "[omega,      0, -v_x],\n",
       "[    0,      0,    0]])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "ad_nu = sympy.Matrix([\n",
    "    [0, -omega, v_y],\n",
    "    [omega, 0, -v_x],\n",
    "    [0, 0, 0]])\n",
    "ad_nu"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "452f8474-2ee9-4045-939f-ae3a413486dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "cleanup = {\n",
    "    -2*sympy.cos(e_theta)/(2*sympy.cos(e_theta)-2) + 2/(2*sympy.cos(e_theta) - 2): -1,\n",
    "    -sympy.cos(e_theta)/(2*sympy.cos(e_theta)-2) + 1/(2*sympy.cos(e_theta) - 2): -sympy.Rational(1, 2)\n",
    "}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "421f56bb-eba1-4672-a7ab-c6a1ddd21702",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-omega*e_theta(t)*e_x(t)/2 - omega*e_theta(t)*e_y(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2) - v_y*e_theta(t)],\n",
       "[-omega*e_theta(t)*e_y(t)/2 + omega*e_theta(t)*e_x(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2) + v_x*e_theta(t)],\n",
       "[                                                                                                            0]])"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g_dot_simp = g_dot.subs(eoms).applyfunc(lambda x: x.simplify().expand().collect(v_y*e_theta).subs(cleanup).collect(omega*e_theta*e_x).subs(cleanup).collect(omega*e_theta*e_y).collect(v_x*e_theta).subs(cleanup))\n",
    "g_dot_simp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "6925ee1f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\\\operatorname{e_{\\theta}}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))],\n",
       "[-e_theta(t)*e_x(t)/2 + e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))],\n",
       "[                                                                      e_theta(t)]])"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "156302d9-8e99-41aa-a5b4-c15da1cd903f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & \\omega & - v_{y}\\\\- \\omega & 0 & v_{x}\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[     0, omega, -v_y],\n",
       "[-omega,     0,  v_x],\n",
       "[     0,     0,    0]])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A = -ad_nu\n",
    "A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "3bfa14fb",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-omega*e_theta(t)*e_x(t)/2 + omega*e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))) - v_y*e_theta(t)],\n",
       "[-omega*e_theta(t)*e_y(t)/2 - omega*e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))) + v_x*e_theta(t)],\n",
       "[                                                                                                            0]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "(A@g).expand()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "6c78c2cb",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\\\\\operatorname{e_{\\theta}}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))],\n",
       "[-e_theta(t)*e_x(t)/2 + e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))],\n",
       "[                                                                      e_theta(t)]])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "id": "870b6813",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{y}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{x}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - \\frac{2 \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin^{2}{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{\\left(2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}\\right)^{2}}\\\\- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{x}}{\\left(t \\right)}}{2} - \\frac{\\operatorname{e_{x}}{\\left(t \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{y}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + \\frac{\\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - \\frac{2 \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin^{2}{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} \\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{\\left(2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}\\right)^{2}}\\\\\\frac{d}{d t} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ e_theta(t)*Derivative(e_y(t), t)/2 + e_y(t)*Derivative(e_theta(t), t)/2 + e_theta(t)*e_x(t)*cos(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) + e_theta(t)*sin(e_theta(t))*Derivative(e_x(t), t)/(2 - 2*cos(e_theta(t))) + e_x(t)*sin(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) - 2*e_theta(t)*e_x(t)*sin(e_theta(t))**2*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t)))**2],\n",
       "[-e_theta(t)*Derivative(e_x(t), t)/2 - e_x(t)*Derivative(e_theta(t), t)/2 + e_theta(t)*e_y(t)*cos(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) + e_theta(t)*sin(e_theta(t))*Derivative(e_y(t), t)/(2 - 2*cos(e_theta(t))) + e_y(t)*sin(e_theta(t))*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t))) - 2*e_theta(t)*e_y(t)*sin(e_theta(t))**2*Derivative(e_theta(t), t)/(2 - 2*cos(e_theta(t)))**2],\n",
       "[                                                                                                                                                                                                                                                                                                                                                                                         Derivative(e_theta(t), t)]])"
      ]
     },
     "execution_count": 56,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g_dot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "id": "ee21087c",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{2 v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{2 v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2}\\\\\\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{2 v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{2 v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-omega*e_theta(t)*e_x(t)*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) + omega*e_theta(t)*e_x(t)/(2*cos(e_theta(t)) - 2) - omega*e_theta(t)*e_y(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2) - 2*v_y*e_theta(t)*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) + 2*v_y*e_theta(t)/(2*cos(e_theta(t)) - 2)],\n",
       "[ omega*e_theta(t)*e_x(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2) - omega*e_theta(t)*e_y(t)*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) + omega*e_theta(t)*e_y(t)/(2*cos(e_theta(t)) - 2) + 2*v_x*e_theta(t)*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) - 2*v_x*e_theta(t)/(2*cos(e_theta(t)) - 2)],\n",
       "[                                                                                                                                                                                                                                                                                         0]])"
      ]
     },
     "execution_count": 55,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g_dot.subs(eoms).applyfunc(lambda x: x.simplify().expand())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + v_{y} \\left(- \\frac{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{2}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\\\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} + \\frac{2 v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{2 v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[           -omega*e_theta(t)*e_x(t)*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) + omega*e_theta(t)*e_x(t)/(2*cos(e_theta(t)) - 2) - omega*e_theta(t)*e_y(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2) + v_y*(-2*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) + 2/(2*cos(e_theta(t)) - 2))*e_theta(t)],\n",
       "[omega*e_theta(t)*e_x(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2) - omega*e_theta(t)*e_y(t)*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) + omega*e_theta(t)*e_y(t)/(2*cos(e_theta(t)) - 2) + 2*v_x*e_theta(t)*cos(e_theta(t))/(2*cos(e_theta(t)) - 2) - 2*v_x*e_theta(t)/(2*cos(e_theta(t)) - 2)],\n",
       "[                                                                                                                                                                                                                                                                                        0]])"
      ]
     },
     "execution_count": 53,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "g_dot.subs(eoms).applyfunc(lambda x: x.simplify().expand().collect(v_y*e_theta))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 168,
   "id": "875d8c75-00c9-4f50-8afb-b024b41d202d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} - v_{y} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\- \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}} + v_{x} \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-omega*e_theta(t)*e_x(t)/2 + omega*e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))) - v_y*e_theta(t)],\n",
       "[-omega*e_theta(t)*e_y(t)/2 - omega*e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))) + v_x*e_theta(t)],\n",
       "[                                                                                                            0]])"
      ]
     },
     "execution_count": 168,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "(A@g).expand()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 179,
   "id": "9169f0b7-152d-4b9c-873f-ad9a99b94288",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{v_{x}}{\\omega}\\\\\\frac{v_{y}}{\\omega}\\\\1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[v_x/omega],\n",
       "[v_y/omega],\n",
       "[        1]])"
      ]
     },
     "execution_count": 179,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.eigenvects()[0][2][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 181,
   "id": "20528a05-687f-4257-b1cd-f6c8c9e89ae8",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}i\\\\1\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[I],\n",
       "[1],\n",
       "[0]])"
      ]
     },
     "execution_count": 181,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.eigenvects()[1][2][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 182,
   "id": "f9212411-7c04-4c59-adb6-3b7ab31eed67",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- i\\\\1\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-I],\n",
       "[ 1],\n",
       "[ 0]])"
      ]
     },
     "execution_count": 182,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.eigenvects()[2][2][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "7573b484-1a46-46d1-9d5a-a1de99e472e3",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\left(- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2} - \\frac{\\omega \\left(\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{\\left(\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} - \\omega \\operatorname{e_{x}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + \\omega \\operatorname{e_{x}}{\\left(t \\right)} - \\omega \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2 v_{y} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + 2 v_{y}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)}\\\\- \\frac{\\omega \\left(- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{\\omega \\left(\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)} + \\frac{\\left(- \\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} + \\omega \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - \\omega \\operatorname{e_{y}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + \\omega \\operatorname{e_{y}}{\\left(t \\right)} + 2 v_{x} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2 v_{x}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[   -omega*(-e_theta(t)*e_x(t)/2 + e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))*e_theta(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2) - omega*(e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))*e_theta(t)/2 + (omega*e_theta(t)*e_y(t) - omega*e_x(t)*cos(e_theta(t)) + omega*e_x(t) - omega*e_y(t)*sin(e_theta(t)) - 2*v_y*cos(e_theta(t)) + 2*v_y)*e_theta(t)/(2*(cos(e_theta(t)) - 1))],\n",
       "[-omega*(-e_theta(t)*e_x(t)/2 + e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))*e_theta(t)/2 + omega*(e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))*e_theta(t)*sin(e_theta(t))/(2*(cos(e_theta(t)) - 1)) + (-omega*e_theta(t)*e_x(t) + omega*e_x(t)*sin(e_theta(t)) - omega*e_y(t)*cos(e_theta(t)) + omega*e_y(t) + 2*v_x*cos(e_theta(t)) - 2*v_x)*e_theta(t)/(2*(cos(e_theta(t)) - 1))],\n",
       "[                                                                                                                                                                                                                                                                                                                                                                                                                                   0]])"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A@g"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "id": "3a28e5bb",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1\\\\0 & 0\\\\1 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 1],\n",
       "[0, 0],\n",
       "[1, 0]])"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "B = sympy.Matrix([[0,1],[0,0],[1,0]])\n",
    "B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "id": "3cac5fb5",
   "metadata": {},
   "outputs": [],
   "source": [
    "K1, K2 = sympy.symbols('K1, K2')\n",
    "K = sympy.Matrix([[K1, 0, 0], [0, K1, K2]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "id": "20d6973d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\omega \\left(\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\left(- K_{1} - \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2}\\right) \\left(- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) + \\left(- K_{2} + \\frac{\\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} - \\omega \\operatorname{e_{x}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + \\omega \\operatorname{e_{x}}{\\left(t \\right)} - \\omega \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2 v_{y} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + 2 v_{y}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}\\\\- \\frac{\\omega \\left(- \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2} + \\frac{\\omega \\left(\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)} + \\frac{\\left(- \\omega \\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} + \\omega \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - \\omega \\operatorname{e_{y}}{\\left(t \\right)} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} + \\omega \\operatorname{e_{y}}{\\left(t \\right)} + 2 v_{x} \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 2 v_{x}\\right) \\operatorname{e_{\\theta}}{\\left(t \\right)}}{2 \\left(\\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)} - 1\\right)}\\\\- K_{1} \\left(\\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{y}}{\\left(t \\right)}}{2} + \\frac{\\operatorname{e_{\\theta}}{\\left(t \\right)} \\operatorname{e_{x}}{\\left(t \\right)} \\sin{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}{2 - 2 \\cos{\\left(\\operatorname{e_{\\theta}}{\\left(t \\right)} \\right)}}\\right)\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-omega*(e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))*e_theta(t)/2 + (-K1 - omega*e_theta(t)*sin(e_theta(t))/(2*cos(e_theta(t)) - 2))*(-e_theta(t)*e_x(t)/2 + e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t)))) + (-K2 + (omega*e_theta(t)*e_y(t) - omega*e_x(t)*cos(e_theta(t)) + omega*e_x(t) - omega*e_y(t)*sin(e_theta(t)) - 2*v_y*cos(e_theta(t)) + 2*v_y)/(2*(cos(e_theta(t)) - 1)))*e_theta(t)],\n",
       "[             -omega*(-e_theta(t)*e_x(t)/2 + e_theta(t)*e_y(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))*e_theta(t)/2 + omega*(e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))*e_theta(t)*sin(e_theta(t))/(2*(cos(e_theta(t)) - 1)) + (-omega*e_theta(t)*e_x(t) + omega*e_x(t)*sin(e_theta(t)) - omega*e_y(t)*cos(e_theta(t)) + omega*e_y(t) + 2*v_x*cos(e_theta(t)) - 2*v_x)*e_theta(t)/(2*(cos(e_theta(t)) - 1))],\n",
       "[                                                                                                                                                                                                                                                                                                                                                            -K1*(e_theta(t)*e_y(t)/2 + e_theta(t)*e_x(t)*sin(e_theta(t))/(2 - 2*cos(e_theta(t))))]])"
      ]
     },
     "execution_count": 35,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "(A-B@K)@g"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "id": "2a82e69b",
   "metadata": {},
   "outputs": [],
   "source": [
    "zeta1, zeta2, zeta3 = sympy.symbols('zeta_1, zeta_2, zeta_3')\n",
    "zeta = sympy.Matrix([zeta1, zeta2, zeta3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "id": "e6ff739a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\zeta_{2} \\left(K_{1} + \\omega\\right) + \\zeta_{3} \\left(K_{2} - v_{y}\\right)\\\\- \\omega \\zeta_{1} + v_{x} \\zeta_{3}\\\\K_{1} \\zeta_{1}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[zeta_2*(K1 + omega) + zeta_3*(K2 - v_y)],\n",
       "[             -omega*zeta_1 + v_x*zeta_3],\n",
       "[                              K1*zeta_1]])"
      ]
     },
     "execution_count": 42,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "(A+B@K)@zeta"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c777770",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.12 ('base')",
   "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.12"
  },
  "vscode": {
   "interpreter": {
    "hash": "40d3a090f54c6569ab1632332b64b2c03c39dcf918b08424e98f38b5ae0af88f"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}