Omelyan_Simplification.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "26c2c256",
   "metadata": {},
   "outputs": [],
   "source": [
    "%display latex"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "0150479a",
   "metadata": {},
   "outputs": [],
   "source": [
    "wx, wy, wz, h, qw, qx, qy, qz, w = var('omega_x omega_y omega_z h q_w q_x q_y q_z omega')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "71e225b2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "0 & -\\frac{1}{2} \\, \\omega_{x} & -\\frac{1}{2} \\, \\omega_{y} & -\\frac{1}{2} \\, \\omega_{z} \\\\\n",
       "\\frac{1}{2} \\, \\omega_{x} & 0 & \\frac{1}{2} \\, \\omega_{z} & \\frac{1}{2} \\, \\omega_{y} \\\\\n",
       "\\frac{1}{2} \\, \\omega_{y} & -\\frac{1}{2} \\, \\omega_{z} & 0 & \\frac{1}{2} \\, \\omega_{x} \\\\\n",
       "\\frac{1}{2} \\, \\omega_{z} & \\frac{1}{2} \\, \\omega_{y} & -\\frac{1}{2} \\, \\omega_{x} & 0\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "0 & -\\frac{1}{2} \\, \\omega_{x} & -\\frac{1}{2} \\, \\omega_{y} & -\\frac{1}{2} \\, \\omega_{z} \\\\\n",
       "\\frac{1}{2} \\, \\omega_{x} & 0 & \\frac{1}{2} \\, \\omega_{z} & \\frac{1}{2} \\, \\omega_{y} \\\\\n",
       "\\frac{1}{2} \\, \\omega_{y} & -\\frac{1}{2} \\, \\omega_{z} & 0 & \\frac{1}{2} \\, \\omega_{x} \\\\\n",
       "\\frac{1}{2} \\, \\omega_{z} & \\frac{1}{2} \\, \\omega_{y} & -\\frac{1}{2} \\, \\omega_{x} & 0\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[           0 -1/2*omega_x -1/2*omega_y -1/2*omega_z]\n",
       "[ 1/2*omega_x            0  1/2*omega_z  1/2*omega_y]\n",
       "[ 1/2*omega_y -1/2*omega_z            0  1/2*omega_x]\n",
       "[ 1/2*omega_z  1/2*omega_y -1/2*omega_x            0]"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Q = (1/2)*Matrix([[0,-wx,-wy,-wz],[wx,0,wz,wy],[wy,-wz,0,wx],[wz,wy,-wx,0]])\n",
    "Q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "2a00bf53",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "1 & 0 & 0 & 0 \\\\\n",
       "0 & 1 & 0 & 0 \\\\\n",
       "0 & 0 & 1 & 0 \\\\\n",
       "0 & 0 & 0 & 1\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "1 & 0 & 0 & 0 \\\\\n",
       "0 & 1 & 0 & 0 \\\\\n",
       "0 & 0 & 1 & 0 \\\\\n",
       "0 & 0 & 0 & 1\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[1 0 0 0]\n",
       "[0 1 0 0]\n",
       "[0 0 1 0]\n",
       "[0 0 0 1]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "I = Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])\n",
    "I"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "446914c3",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "-\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} + 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} + {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} + 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} - 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "-\\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} + h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} - 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "-\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} + 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} + {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} + 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} - 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "-\\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} + h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} - 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[-(h^4*omega_x^4 - h^4*omega_y^4 + 2*h^4*omega_x^2*omega_z^2 + h^4*omega_z^4 + 16*h^3*omega_x*omega_y*omega_z + 32*h^2*omega_y^2 - 256)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                                                                                                                                                      -8*h*omega_x/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                             8*(h^3*omega_y^3 - h^3*omega_y*omega_z^2 + (h^3*omega_x^2 - 16*h)*omega_y)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                            -8*(h^3*omega_z^3 + 8*h^2*omega_x*omega_y + (h^3*omega_x^2 - h^3*omega_y^2 + 16*h)*omega_z)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[                              8*(h^3*omega_x^3 - h^3*omega_x*omega_y^2 + h^3*omega_x*omega_z^2 + 8*h^2*omega_y*omega_z + 16*h*omega_x)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                                                                                                             -(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 - 16)/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                             8*(h^3*omega_z^3 - 8*h^2*omega_x*omega_y + (h^3*omega_x^2 - h^3*omega_y^2 + 16*h)*omega_z)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                             8*(h^3*omega_y^3 - h^3*omega_y*omega_z^2 - (h^3*omega_x^2 - 16*h)*omega_y)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[                                                           -8*(h^3*omega_y^3 + h^3*omega_y*omega_z^2 - (h^3*omega_x^2 + 16*h)*omega_y)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                                                                                                                                                      -8*h*omega_z/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16) -(h^4*omega_x^4 - h^4*omega_y^4 + 2*h^4*omega_x^2*omega_z^2 + h^4*omega_z^4 - 16*h^3*omega_x*omega_y*omega_z + 32*h^2*omega_y^2 - 256)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                               8*(h^3*omega_x^3 - h^3*omega_x*omega_y^2 + h^3*omega_x*omega_z^2 - 8*h^2*omega_y*omega_z + 16*h*omega_x)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[                                                                                                                                                                                      8*h*omega_z/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                                                                                                                                                       8*h*omega_y/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                                                                                                                                                      -8*h*omega_x/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                                                                                                             -(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 - 16)/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)]"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A1 = (I - (h/2)*Q)^-1*(I + (h/2)*Q)\n",
    "A1 = A1.simplify_full()\n",
    "A1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "7a0c390f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}1</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}1$$"
      ],
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A1.det().full_simplify()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "84150d37",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "-\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} + 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} + {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} + 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} - 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "-\\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} + h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} - 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "-\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} + 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} + {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} + 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, {\\left(h^{3} \\omega_{z}^{3} - 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "-\\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} + h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} & \\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} - 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16} & -\\frac{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[-(h^4*omega_x^4 - h^4*omega_y^4 + 2*h^4*omega_x^2*omega_z^2 + h^4*omega_z^4 + 16*h^3*omega_x*omega_y*omega_z + 32*h^2*omega_y^2 - 256)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                                                                                                                                                      -8*h*omega_x/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                             8*(h^3*omega_y^3 - h^3*omega_y*omega_z^2 + (h^3*omega_x^2 - 16*h)*omega_y)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                            -8*(h^3*omega_z^3 + 8*h^2*omega_x*omega_y + (h^3*omega_x^2 - h^3*omega_y^2 + 16*h)*omega_z)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[                              8*(h^3*omega_x^3 - h^3*omega_x*omega_y^2 + h^3*omega_x*omega_z^2 + 8*h^2*omega_y*omega_z + 16*h*omega_x)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                                                                                                             -(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 - 16)/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                             8*(h^3*omega_z^3 - 8*h^2*omega_x*omega_y + (h^3*omega_x^2 - h^3*omega_y^2 + 16*h)*omega_z)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                             8*(h^3*omega_y^3 - h^3*omega_y*omega_z^2 - (h^3*omega_x^2 - 16*h)*omega_y)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[                                                           -8*(h^3*omega_y^3 + h^3*omega_y*omega_z^2 - (h^3*omega_x^2 + 16*h)*omega_y)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                                                                                                                                                                                      -8*h*omega_z/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16) -(h^4*omega_x^4 - h^4*omega_y^4 + 2*h^4*omega_x^2*omega_z^2 + h^4*omega_z^4 - 16*h^3*omega_x*omega_y*omega_z + 32*h^2*omega_y^2 - 256)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)                               8*(h^3*omega_x^3 - h^3*omega_x*omega_y^2 + h^3*omega_x*omega_z^2 - 8*h^2*omega_y*omega_z + 16*h*omega_x)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[                                                                                                                                                                                      8*h*omega_z/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                                                                                                                                                       8*h*omega_y/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                                                                                                                                                      -8*h*omega_x/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)                                                                                                                                             -(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 - 16)/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)]"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "B = A1.subs({h^2*wx^2 + h^2*wy^2 + h^2*wz^2 - 16: h^2*w^2 - 16, h^2*wx^2 + h^2*wy^2 + h^2*wz^2 + 16: h^2*w^2 + 16})\n",
    "B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "11bafe66",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\mathrm{False}</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\mathrm{False}$$"
      ],
      "text/plain": [
       "False"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "((w^2*h^2 + 16)/(h*16))^-1*(-I*(w^2*h^2 - 16)/(h*16) + Q) == B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "4e899894",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{r}\n",
       "q_{w} \\\\\n",
       "q_{x} \\\\\n",
       "q_{y} \\\\\n",
       "q_{z}\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{r}\n",
       "q_{w} \\\\\n",
       "q_{x} \\\\\n",
       "q_{y} \\\\\n",
       "q_{z}\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[q_w]\n",
       "[q_x]\n",
       "[q_y]\n",
       "[q_z]"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "S = Matrix([[qw,qx,qy,qz]]).T\n",
    "S"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "a650870f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{r}\n",
       "-\\frac{{\\left(h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} + 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256\\right)} q_{w} + 8 \\, {\\left(h^{3} \\omega_{x}^{3} + h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 16 \\, h \\omega_{x}\\right)} q_{x} - 8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} + {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)} q_{y} + 8 \\, {\\left(h^{3} \\omega_{z}^{3} + 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)} q_{z}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)} q_{w} - {\\left(h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 32 \\, h^{2} \\omega_{y}^{2} - 256\\right)} q_{x} + 8 \\, {\\left(h^{3} \\omega_{z}^{3} - 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)} q_{y} + 8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)} q_{z}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "-\\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} + h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)} q_{w} + 8 \\, {\\left(h^{3} \\omega_{z}^{3} + {\\left(h^{3} \\omega_{x}^{2} + h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)} q_{x} + {\\left(h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256\\right)} q_{y} - 8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} - 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)} q_{z}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, h \\omega_{z} q_{w} + 8 \\, h \\omega_{y} q_{x} - 8 \\, h \\omega_{x} q_{y} - {\\left(h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16\\right)} q_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{r}\n",
       "-\\frac{{\\left(h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} + 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256\\right)} q_{w} + 8 \\, {\\left(h^{3} \\omega_{x}^{3} + h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 16 \\, h \\omega_{x}\\right)} q_{x} - 8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} + {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)} q_{y} + 8 \\, {\\left(h^{3} \\omega_{z}^{3} + 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)} q_{z}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} + 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)} q_{w} - {\\left(h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 32 \\, h^{2} \\omega_{y}^{2} - 256\\right)} q_{x} + 8 \\, {\\left(h^{3} \\omega_{z}^{3} - 8 \\, h^{2} \\omega_{x} \\omega_{y} + {\\left(h^{3} \\omega_{x}^{2} - h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)} q_{y} + 8 \\, {\\left(h^{3} \\omega_{y}^{3} - h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} - 16 \\, h\\right)} \\omega_{y}\\right)} q_{z}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "-\\frac{8 \\, {\\left(h^{3} \\omega_{y}^{3} + h^{3} \\omega_{y} \\omega_{z}^{2} - {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)} q_{w} + 8 \\, {\\left(h^{3} \\omega_{z}^{3} + {\\left(h^{3} \\omega_{x}^{2} + h^{3} \\omega_{y}^{2} + 16 \\, h\\right)} \\omega_{z}\\right)} q_{x} + {\\left(h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + 2 \\, h^{4} \\omega_{x}^{2} \\omega_{z}^{2} + h^{4} \\omega_{z}^{4} - 16 \\, h^{3} \\omega_{x} \\omega_{y} \\omega_{z} + 32 \\, h^{2} \\omega_{y}^{2} - 256\\right)} q_{y} - 8 \\, {\\left(h^{3} \\omega_{x}^{3} - h^{3} \\omega_{x} \\omega_{y}^{2} + h^{3} \\omega_{x} \\omega_{z}^{2} - 8 \\, h^{2} \\omega_{y} \\omega_{z} + 16 \\, h \\omega_{x}\\right)} q_{z}}{h^{4} \\omega_{x}^{4} - h^{4} \\omega_{y}^{4} + h^{4} \\omega_{z}^{4} + 32 \\, h^{2} \\omega_{x}^{2} + 2 \\, {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{z}^{2} + 256} \\\\\n",
       "\\frac{8 \\, h \\omega_{z} q_{w} + 8 \\, h \\omega_{y} q_{x} - 8 \\, h \\omega_{x} q_{y} - {\\left(h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} - 16\\right)} q_{z}}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[-((h^4*omega_x^4 - h^4*omega_y^4 + 2*h^4*omega_x^2*omega_z^2 + h^4*omega_z^4 + 16*h^3*omega_x*omega_y*omega_z + 32*h^2*omega_y^2 - 256)*q_w + 8*(h^3*omega_x^3 + h^3*omega_x*omega_y^2 + h^3*omega_x*omega_z^2 + 16*h*omega_x)*q_x - 8*(h^3*omega_y^3 - h^3*omega_y*omega_z^2 + (h^3*omega_x^2 - 16*h)*omega_y)*q_y + 8*(h^3*omega_z^3 + 8*h^2*omega_x*omega_y + (h^3*omega_x^2 - h^3*omega_y^2 + 16*h)*omega_z)*q_z)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[          (8*(h^3*omega_x^3 - h^3*omega_x*omega_y^2 + h^3*omega_x*omega_z^2 + 8*h^2*omega_y*omega_z + 16*h*omega_x)*q_w - (h^4*omega_x^4 - h^4*omega_y^4 + 2*h^4*omega_x^2*omega_z^2 + h^4*omega_z^4 - 32*h^2*omega_y^2 - 256)*q_x + 8*(h^3*omega_z^3 - 8*h^2*omega_x*omega_y + (h^3*omega_x^2 - h^3*omega_y^2 + 16*h)*omega_z)*q_y + 8*(h^3*omega_y^3 - h^3*omega_y*omega_z^2 - (h^3*omega_x^2 - 16*h)*omega_y)*q_z)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[-(8*(h^3*omega_y^3 + h^3*omega_y*omega_z^2 - (h^3*omega_x^2 + 16*h)*omega_y)*q_w + 8*(h^3*omega_z^3 + (h^3*omega_x^2 + h^3*omega_y^2 + 16*h)*omega_z)*q_x + (h^4*omega_x^4 - h^4*omega_y^4 + 2*h^4*omega_x^2*omega_z^2 + h^4*omega_z^4 - 16*h^3*omega_x*omega_y*omega_z + 32*h^2*omega_y^2 - 256)*q_y - 8*(h^3*omega_x^3 - h^3*omega_x*omega_y^2 + h^3*omega_x*omega_z^2 - 8*h^2*omega_y*omega_z + 16*h*omega_x)*q_z)/(h^4*omega_x^4 - h^4*omega_y^4 + h^4*omega_z^4 + 32*h^2*omega_x^2 + 2*(h^4*omega_x^2 + 16*h^2)*omega_z^2 + 256)]\n",
       "[                                                                                                                                                                                                                                                                                                                                                                (8*h*omega_z*q_w + 8*h*omega_y*q_x - 8*h*omega_x*q_y - (h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 - 16)*q_z)/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)]"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q = B*S\n",
    "q = q.simplify_full()\n",
    "q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "a7c04f3d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\frac{1}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\frac{1}{h^{2} \\omega_{x}^{2} - h^{2} \\omega_{y}^{2} + h^{2} \\omega_{z}^{2} + 16}$$"
      ],
      "text/plain": [
       "1/(h^2*omega_x^2 - h^2*omega_y^2 + h^2*omega_z^2 + 16)"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q = vector(q.list())\n",
    "gcd(q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "964c1e6d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\sqrt{\\frac{{\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 32 \\, h^{5} \\omega_{x} \\omega_{y} \\omega_{z}^{3} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} + 32 \\, h^{4} \\omega_{x}^{2} + 256 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} - 112 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} - 32 \\, {\\left(h^{5} \\omega_{x} \\omega_{y}^{3} - {\\left(h^{5} \\omega_{x}^{3} + 16 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}\\right)} \\omega_{z} + 4096\\right)} q_{w}^{2} - 32 \\, {\\left(h^{5} \\omega_{x} \\omega_{y}^{4} - h^{5} \\omega_{x} \\omega_{y}^{2} \\omega_{z}^{2} - 4 \\, h^{4} \\omega_{y} \\omega_{z}^{3} - {\\left(h^{5} \\omega_{x}^{3} + 16 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}^{2} - 4 \\, {\\left(3 \\, h^{4} \\omega_{y}^{3} + {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{y}\\right)} \\omega_{z}\\right)} q_{w} q_{x} + {\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} - 112 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} - 96 \\, h^{4} \\omega_{x}^{2} - 1792 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} - 48 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 4096\\right)} q_{x}^{2} + {\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} - 32 \\, h^{5} \\omega_{x} \\omega_{y} \\omega_{z}^{3} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} - 224 \\, h^{4} \\omega_{x}^{2} + 256 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 32 \\, {\\left(h^{5} \\omega_{x} \\omega_{y}^{3} - {\\left(h^{5} \\omega_{x}^{3} + 16 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}\\right)} \\omega_{z} + 4096\\right)} q_{y}^{2} + {\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} - 112 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} + 160 \\, h^{4} \\omega_{x}^{2} - 1792 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} + 80 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 4096\\right)} q_{z}^{2} + 32 \\, {\\left({\\left(h^{5} \\omega_{x}^{2} \\omega_{y}^{3} + h^{5} \\omega_{y} \\omega_{z}^{4} - 16 \\, h^{4} \\omega_{x} \\omega_{y}^{2} \\omega_{z} - {\\left(h^{5} \\omega_{y}^{3} - 16 \\, h^{3} \\omega_{y}\\right)} \\omega_{z}^{2} - {\\left(h^{5} \\omega_{x}^{4} + 16 \\, h^{3} \\omega_{x}^{2}\\right)} \\omega_{y}\\right)} q_{w} + {\\left(h^{5} \\omega_{y}^{2} \\omega_{z}^{3} - 12 \\, h^{4} \\omega_{x} \\omega_{y}^{3} - 4 \\, h^{4} \\omega_{x} \\omega_{y} \\omega_{z}^{2} - 4 \\, {\\left(h^{4} \\omega_{x}^{3} + 16 \\, h^{2} \\omega_{x}\\right)} \\omega_{y} - {\\left(h^{5} \\omega_{y}^{4} - {\\left(h^{5} \\omega_{x}^{2} + 16 \\, h^{3}\\right)} \\omega_{y}^{2}\\right)} \\omega_{z}\\right)} q_{x}\\right)} q_{y} - 32 \\, {\\left({\\left(h^{5} \\omega_{y}^{2} \\omega_{z}^{3} + 4 \\, h^{4} \\omega_{x} \\omega_{y}^{3} - 4 \\, h^{4} \\omega_{x} \\omega_{y} \\omega_{z}^{2} - 4 \\, {\\left(h^{4} \\omega_{x}^{3} + 16 \\, h^{2} \\omega_{x}\\right)} \\omega_{y} - {\\left(h^{5} \\omega_{y}^{4} - {\\left(h^{5} \\omega_{x}^{2} - 48 \\, h^{3}\\right)} \\omega_{y}^{2}\\right)} \\omega_{z}\\right)} q_{w} - {\\left(h^{5} \\omega_{y}^{5} - {\\left(h^{5} \\omega_{x}^{2} - 32 \\, h^{3}\\right)} \\omega_{y}^{3} - {\\left(h^{5} \\omega_{y}^{3} - 16 \\, h^{3} \\omega_{y}\\right)} \\omega_{z}^{2} + 16 \\, {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)} q_{x} + {\\left(h^{5} \\omega_{x} \\omega_{y}^{4} - h^{5} \\omega_{x} \\omega_{y}^{2} \\omega_{z}^{2} - 4 \\, h^{4} \\omega_{y} \\omega_{z}^{3} - {\\left(h^{5} \\omega_{x}^{3} - 48 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}^{2} + 4 \\, {\\left(h^{4} \\omega_{y}^{3} - {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{y}\\right)} \\omega_{z}\\right)} q_{y}\\right)} q_{z}}{h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} + 32 \\, h^{4} \\omega_{x}^{2} + 256 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 4096}}</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\sqrt{\\frac{{\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 32 \\, h^{5} \\omega_{x} \\omega_{y} \\omega_{z}^{3} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} + 32 \\, h^{4} \\omega_{x}^{2} + 256 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} - 112 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} - 32 \\, {\\left(h^{5} \\omega_{x} \\omega_{y}^{3} - {\\left(h^{5} \\omega_{x}^{3} + 16 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}\\right)} \\omega_{z} + 4096\\right)} q_{w}^{2} - 32 \\, {\\left(h^{5} \\omega_{x} \\omega_{y}^{4} - h^{5} \\omega_{x} \\omega_{y}^{2} \\omega_{z}^{2} - 4 \\, h^{4} \\omega_{y} \\omega_{z}^{3} - {\\left(h^{5} \\omega_{x}^{3} + 16 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}^{2} - 4 \\, {\\left(3 \\, h^{4} \\omega_{y}^{3} + {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{y}\\right)} \\omega_{z}\\right)} q_{w} q_{x} + {\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} - 112 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} - 96 \\, h^{4} \\omega_{x}^{2} - 1792 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} - 48 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 4096\\right)} q_{x}^{2} + {\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} - 32 \\, h^{5} \\omega_{x} \\omega_{y} \\omega_{z}^{3} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} - 224 \\, h^{4} \\omega_{x}^{2} + 256 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 32 \\, {\\left(h^{5} \\omega_{x} \\omega_{y}^{3} - {\\left(h^{5} \\omega_{x}^{3} + 16 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}\\right)} \\omega_{z} + 4096\\right)} q_{y}^{2} + {\\left(h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} - 112 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} + 160 \\, h^{4} \\omega_{x}^{2} - 1792 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} + 80 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 4096\\right)} q_{z}^{2} + 32 \\, {\\left({\\left(h^{5} \\omega_{x}^{2} \\omega_{y}^{3} + h^{5} \\omega_{y} \\omega_{z}^{4} - 16 \\, h^{4} \\omega_{x} \\omega_{y}^{2} \\omega_{z} - {\\left(h^{5} \\omega_{y}^{3} - 16 \\, h^{3} \\omega_{y}\\right)} \\omega_{z}^{2} - {\\left(h^{5} \\omega_{x}^{4} + 16 \\, h^{3} \\omega_{x}^{2}\\right)} \\omega_{y}\\right)} q_{w} + {\\left(h^{5} \\omega_{y}^{2} \\omega_{z}^{3} - 12 \\, h^{4} \\omega_{x} \\omega_{y}^{3} - 4 \\, h^{4} \\omega_{x} \\omega_{y} \\omega_{z}^{2} - 4 \\, {\\left(h^{4} \\omega_{x}^{3} + 16 \\, h^{2} \\omega_{x}\\right)} \\omega_{y} - {\\left(h^{5} \\omega_{y}^{4} - {\\left(h^{5} \\omega_{x}^{2} + 16 \\, h^{3}\\right)} \\omega_{y}^{2}\\right)} \\omega_{z}\\right)} q_{x}\\right)} q_{y} - 32 \\, {\\left({\\left(h^{5} \\omega_{y}^{2} \\omega_{z}^{3} + 4 \\, h^{4} \\omega_{x} \\omega_{y}^{3} - 4 \\, h^{4} \\omega_{x} \\omega_{y} \\omega_{z}^{2} - 4 \\, {\\left(h^{4} \\omega_{x}^{3} + 16 \\, h^{2} \\omega_{x}\\right)} \\omega_{y} - {\\left(h^{5} \\omega_{y}^{4} - {\\left(h^{5} \\omega_{x}^{2} - 48 \\, h^{3}\\right)} \\omega_{y}^{2}\\right)} \\omega_{z}\\right)} q_{w} - {\\left(h^{5} \\omega_{y}^{5} - {\\left(h^{5} \\omega_{x}^{2} - 32 \\, h^{3}\\right)} \\omega_{y}^{3} - {\\left(h^{5} \\omega_{y}^{3} - 16 \\, h^{3} \\omega_{y}\\right)} \\omega_{z}^{2} + 16 \\, {\\left(h^{3} \\omega_{x}^{2} + 16 \\, h\\right)} \\omega_{y}\\right)} q_{x} + {\\left(h^{5} \\omega_{x} \\omega_{y}^{4} - h^{5} \\omega_{x} \\omega_{y}^{2} \\omega_{z}^{2} - 4 \\, h^{4} \\omega_{y} \\omega_{z}^{3} - {\\left(h^{5} \\omega_{x}^{3} - 48 \\, h^{3} \\omega_{x}\\right)} \\omega_{y}^{2} + 4 \\, {\\left(h^{4} \\omega_{y}^{3} - {\\left(h^{4} \\omega_{x}^{2} + 16 \\, h^{2}\\right)} \\omega_{y}\\right)} \\omega_{z}\\right)} q_{y}\\right)} q_{z}}{h^{6} \\omega_{x}^{6} + h^{6} \\omega_{y}^{6} + h^{6} \\omega_{z}^{6} + 48 \\, h^{4} \\omega_{x}^{4} - {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{4} + {\\left(3 \\, h^{6} \\omega_{x}^{2} - h^{6} \\omega_{y}^{2} + 48 \\, h^{4}\\right)} \\omega_{z}^{4} + 768 \\, h^{2} \\omega_{x}^{2} - {\\left(h^{6} \\omega_{x}^{4} + 32 \\, h^{4} \\omega_{x}^{2} + 256 \\, h^{2}\\right)} \\omega_{y}^{2} + {\\left(3 \\, h^{6} \\omega_{x}^{4} - h^{6} \\omega_{y}^{4} + 96 \\, h^{4} \\omega_{x}^{2} - 2 \\, {\\left(h^{6} \\omega_{x}^{2} + 16 \\, h^{4}\\right)} \\omega_{y}^{2} + 768 \\, h^{2}\\right)} \\omega_{z}^{2} + 4096}}$$"
      ],
      "text/plain": [
       "sqrt(((h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 32*h^5*omega_x*omega_y*omega_z^3 + 48*h^4*omega_x^4 - (h^6*omega_x^2 + 16*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 + 32*h^4*omega_x^2 + 256*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 - 112*h^4)*omega_y^2 + 768*h^2)*omega_z^2 - 32*(h^5*omega_x*omega_y^3 - (h^5*omega_x^3 + 16*h^3*omega_x)*omega_y)*omega_z + 4096)*q_w^2 - 32*(h^5*omega_x*omega_y^4 - h^5*omega_x*omega_y^2*omega_z^2 - 4*h^4*omega_y*omega_z^3 - (h^5*omega_x^3 + 16*h^3*omega_x)*omega_y^2 - 4*(3*h^4*omega_y^3 + (h^4*omega_x^2 + 16*h^2)*omega_y)*omega_z)*q_w*q_x + (h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 48*h^4*omega_x^4 - (h^6*omega_x^2 - 112*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 - 96*h^4*omega_x^2 - 1792*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 - 48*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 4096)*q_x^2 + (h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 - 32*h^5*omega_x*omega_y*omega_z^3 + 48*h^4*omega_x^4 - (h^6*omega_x^2 + 16*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 - 224*h^4*omega_x^2 + 256*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 + 16*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 32*(h^5*omega_x*omega_y^3 - (h^5*omega_x^3 + 16*h^3*omega_x)*omega_y)*omega_z + 4096)*q_y^2 + (h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 48*h^4*omega_x^4 - (h^6*omega_x^2 - 112*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 + 160*h^4*omega_x^2 - 1792*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 + 80*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 4096)*q_z^2 + 32*((h^5*omega_x^2*omega_y^3 + h^5*omega_y*omega_z^4 - 16*h^4*omega_x*omega_y^2*omega_z - (h^5*omega_y^3 - 16*h^3*omega_y)*omega_z^2 - (h^5*omega_x^4 + 16*h^3*omega_x^2)*omega_y)*q_w + (h^5*omega_y^2*omega_z^3 - 12*h^4*omega_x*omega_y^3 - 4*h^4*omega_x*omega_y*omega_z^2 - 4*(h^4*omega_x^3 + 16*h^2*omega_x)*omega_y - (h^5*omega_y^4 - (h^5*omega_x^2 + 16*h^3)*omega_y^2)*omega_z)*q_x)*q_y - 32*((h^5*omega_y^2*omega_z^3 + 4*h^4*omega_x*omega_y^3 - 4*h^4*omega_x*omega_y*omega_z^2 - 4*(h^4*omega_x^3 + 16*h^2*omega_x)*omega_y - (h^5*omega_y^4 - (h^5*omega_x^2 - 48*h^3)*omega_y^2)*omega_z)*q_w - (h^5*omega_y^5 - (h^5*omega_x^2 - 32*h^3)*omega_y^3 - (h^5*omega_y^3 - 16*h^3*omega_y)*omega_z^2 + 16*(h^3*omega_x^2 + 16*h)*omega_y)*q_x + (h^5*omega_x*omega_y^4 - h^5*omega_x*omega_y^2*omega_z^2 - 4*h^4*omega_y*omega_z^3 - (h^5*omega_x^3 - 48*h^3*omega_x)*omega_y^2 + 4*(h^4*omega_y^3 - (h^4*omega_x^2 + 16*h^2)*omega_y)*omega_z)*q_y)*q_z)/(h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 48*h^4*omega_x^4 - (h^6*omega_x^2 + 16*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 + 32*h^4*omega_x^2 + 256*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 + 16*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 4096))"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Norm = norm(q).full_simplify()\n",
    "Norm = Norm.factor()\n",
    "Norm = Norm.subs({qw^2 + qx^2 +qy^2 + qz^2: 1})\n",
    "Norm = Norm.subs({wx^2 + wy^2 + wz^2: w^2})\n",
    "Norm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "558ba324",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sqrt(((h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 32*h^5*omega_x*omega_y*omega_z^3 + 48*h^4*omega_x^4 - (h^6*omega_x^2 + 16*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 + 32*h^4*omega_x^2 + 256*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 - 112*h^4)*omega_y^2 + 768*h^2)*omega_z^2 - 32*(h^5*omega_x*omega_y^3 - (h^5*omega_x^3 + 16*h^3*omega_x)*omega_y)*omega_z + 4096)*q_w^2 - 32*(h^5*omega_x*omega_y^4 - h^5*omega_x*omega_y^2*omega_z^2 - 4*h^4*omega_y*omega_z^3 - (h^5*omega_x^3 + 16*h^3*omega_x)*omega_y^2 - 4*(3*h^4*omega_y^3 + (h^4*omega_x^2 + 16*h^2)*omega_y)*omega_z)*q_w*q_x + (h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 48*h^4*omega_x^4 - (h^6*omega_x^2 - 112*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 - 96*h^4*omega_x^2 - 1792*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 - 48*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 4096)*q_x^2 + (h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 - 32*h^5*omega_x*omega_y*omega_z^3 + 48*h^4*omega_x^4 - (h^6*omega_x^2 + 16*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 - 224*h^4*omega_x^2 + 256*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 + 16*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 32*(h^5*omega_x*omega_y^3 - (h^5*omega_x^3 + 16*h^3*omega_x)*omega_y)*omega_z + 4096)*q_y^2 + (h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 48*h^4*omega_x^4 - (h^6*omega_x^2 - 112*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 + 160*h^4*omega_x^2 - 1792*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 + 80*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 4096)*q_z^2 + 32*((h^5*omega_x^2*omega_y^3 + h^5*omega_y*omega_z^4 - 16*h^4*omega_x*omega_y^2*omega_z - (h^5*omega_y^3 - 16*h^3*omega_y)*omega_z^2 - (h^5*omega_x^4 + 16*h^3*omega_x^2)*omega_y)*q_w + (h^5*omega_y^2*omega_z^3 - 12*h^4*omega_x*omega_y^3 - 4*h^4*omega_x*omega_y*omega_z^2 - 4*(h^4*omega_x^3 + 16*h^2*omega_x)*omega_y - (h^5*omega_y^4 - (h^5*omega_x^2 + 16*h^3)*omega_y^2)*omega_z)*q_x)*q_y - 32*((h^5*omega_y^2*omega_z^3 + 4*h^4*omega_x*omega_y^3 - 4*h^4*omega_x*omega_y*omega_z^2 - 4*(h^4*omega_x^3 + 16*h^2*omega_x)*omega_y - (h^5*omega_y^4 - (h^5*omega_x^2 - 48*h^3)*omega_y^2)*omega_z)*q_w - (h^5*omega_y^5 - (h^5*omega_x^2 - 32*h^3)*omega_y^3 - (h^5*omega_y^3 - 16*h^3*omega_y)*omega_z^2 + 16*(h^3*omega_x^2 + 16*h)*omega_y)*q_x + (h^5*omega_x*omega_y^4 - h^5*omega_x*omega_y^2*omega_z^2 - 4*h^4*omega_y*omega_z^3 - (h^5*omega_x^3 - 48*h^3*omega_x)*omega_y^2 + 4*(h^4*omega_y^3 - (h^4*omega_x^2 + 16*h^2)*omega_y)*omega_z)*q_y)*q_z)/(h^6*omega_x^6 + h^6*omega_y^6 + h^6*omega_z^6 + 48*h^4*omega_x^4 - (h^6*omega_x^2 + 16*h^4)*omega_y^4 + (3*h^6*omega_x^2 - h^6*omega_y^2 + 48*h^4)*omega_z^4 + 768*h^2*omega_x^2 - (h^6*omega_x^4 + 32*h^4*omega_x^2 + 256*h^2)*omega_y^2 + (3*h^6*omega_x^4 - h^6*omega_y^4 + 96*h^4*omega_x^2 - 2*(h^6*omega_x^2 + 16*h^4)*omega_y^2 + 768*h^2)*omega_z^2 + 4096))\n"
     ]
    }
   ],
   "source": [
    "print(Norm)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "a7ca2a3f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}1</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}1$$"
      ],
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Norm = sqrt((h^4*omega^4 - 32*h^2*omega^2 + 64*h^2*omega^2 + 256)/(h^4*omega^4 + 32*h^2*omega^2 + 256))\n",
    "Norm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "97cc2cb2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "-\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} & -\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} & -\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} & -\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16}\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{rrrr}\n",
       "-\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} & -\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} & -\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{z}}{h^{2} \\omega^{2} + 16} & \\frac{8 \\, h \\omega_{y}}{h^{2} \\omega^{2} + 16} & -\\frac{8 \\, h \\omega_{x}}{h^{2} \\omega^{2} + 16} & -\\frac{h^{2} \\omega^{2} - 16}{h^{2} \\omega^{2} + 16}\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[-(h^2*omega^2 - 16)/(h^2*omega^2 + 16)        -8*h*omega_x/(h^2*omega^2 + 16)        -8*h*omega_y/(h^2*omega^2 + 16)        -8*h*omega_z/(h^2*omega^2 + 16)]\n",
       "[        8*h*omega_x/(h^2*omega^2 + 16) -(h^2*omega^2 - 16)/(h^2*omega^2 + 16)         8*h*omega_z/(h^2*omega^2 + 16)         8*h*omega_y/(h^2*omega^2 + 16)]\n",
       "[        8*h*omega_y/(h^2*omega^2 + 16)        -8*h*omega_z/(h^2*omega^2 + 16) -(h^2*omega^2 - 16)/(h^2*omega^2 + 16)         8*h*omega_x/(h^2*omega^2 + 16)]\n",
       "[        8*h*omega_z/(h^2*omega^2 + 16)         8*h*omega_y/(h^2*omega^2 + 16)        -8*h*omega_x/(h^2*omega^2 + 16) -(h^2*omega^2 - 16)/(h^2*omega^2 + 16)]"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A = ((1 - w^2*h^2/16)/(1 + w^2*h^2/16))*I + h/(1 + w^2*h^2/16)*Q\n",
    "A.simplify_full()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "d10c3cab",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<html><script type=\"math/tex; mode=display\">\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{r}\n",
       "-\\frac{8 \\, h \\omega_{x} q_{x} + 8 \\, h \\omega_{y} q_{y} + 8 \\, h \\omega_{z} q_{z} + {\\left(h^{2} \\omega^{2} - 16\\right)} q_{w}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{x} q_{w} + 8 \\, h \\omega_{z} q_{y} + 8 \\, h \\omega_{y} q_{z} - {\\left(h^{2} \\omega^{2} - 16\\right)} q_{x}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{y} q_{w} - 8 \\, h \\omega_{z} q_{x} + 8 \\, h \\omega_{x} q_{z} - {\\left(h^{2} \\omega^{2} - 16\\right)} q_{y}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{z} q_{w} + 8 \\, h \\omega_{y} q_{x} - 8 \\, h \\omega_{x} q_{y} - {\\left(h^{2} \\omega^{2} - 16\\right)} q_{z}}{h^{2} \\omega^{2} + 16}\n",
       "\\end{array}\\right)</script></html>"
      ],
      "text/latex": [
       "$$\\newcommand{\\Bold}[1]{\\mathbf{#1}}\\left(\\begin{array}{r}\n",
       "-\\frac{8 \\, h \\omega_{x} q_{x} + 8 \\, h \\omega_{y} q_{y} + 8 \\, h \\omega_{z} q_{z} + {\\left(h^{2} \\omega^{2} - 16\\right)} q_{w}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{x} q_{w} + 8 \\, h \\omega_{z} q_{y} + 8 \\, h \\omega_{y} q_{z} - {\\left(h^{2} \\omega^{2} - 16\\right)} q_{x}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{y} q_{w} - 8 \\, h \\omega_{z} q_{x} + 8 \\, h \\omega_{x} q_{z} - {\\left(h^{2} \\omega^{2} - 16\\right)} q_{y}}{h^{2} \\omega^{2} + 16} \\\\\n",
       "\\frac{8 \\, h \\omega_{z} q_{w} + 8 \\, h \\omega_{y} q_{x} - 8 \\, h \\omega_{x} q_{y} - {\\left(h^{2} \\omega^{2} - 16\\right)} q_{z}}{h^{2} \\omega^{2} + 16}\n",
       "\\end{array}\\right)$$"
      ],
      "text/plain": [
       "[-(8*h*omega_x*q_x + 8*h*omega_y*q_y + 8*h*omega_z*q_z + (h^2*omega^2 - 16)*q_w)/(h^2*omega^2 + 16)]\n",
       "[ (8*h*omega_x*q_w + 8*h*omega_z*q_y + 8*h*omega_y*q_z - (h^2*omega^2 - 16)*q_x)/(h^2*omega^2 + 16)]\n",
       "[ (8*h*omega_y*q_w - 8*h*omega_z*q_x + 8*h*omega_x*q_z - (h^2*omega^2 - 16)*q_y)/(h^2*omega^2 + 16)]\n",
       "[ (8*h*omega_z*q_w + 8*h*omega_y*q_x - 8*h*omega_x*q_y - (h^2*omega^2 - 16)*q_z)/(h^2*omega^2 + 16)]"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q = A*S\n",
    "q = q.simplify_full()\n",
    "q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "57973320",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-(8*h*omega_x*q_x + 8*h*omega_y*q_y + 8*h*omega_z*q_z + (h^2*omega^2 - 16)*q_w)/(h^2*omega^2 + 16)]\n",
      "[ (8*h*omega_x*q_w + 8*h*omega_z*q_y + 8*h*omega_y*q_z - (h^2*omega^2 - 16)*q_x)/(h^2*omega^2 + 16)]\n",
      "[ (8*h*omega_y*q_w - 8*h*omega_z*q_x + 8*h*omega_x*q_z - (h^2*omega^2 - 16)*q_y)/(h^2*omega^2 + 16)]\n",
      "[ (8*h*omega_z*q_w + 8*h*omega_y*q_x - 8*h*omega_x*q_y - (h^2*omega^2 - 16)*q_z)/(h^2*omega^2 + 16)]\n"
     ]
    }
   ],
   "source": [
    "print(q)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "SageMath 9.5",
   "language": "sage",
   "name": "sagemath"
  },
  "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.10.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}