var6.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Рогачев Александр Викторович, 521 группа\n",
    "# Вариант 6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from math import *  # for math functions and constants. Trigonometry functions inputs are in radians\n",
    "import numpy as np  # for matrix operations such as matrix multiplication"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Первый блок задач"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. \n",
    "Пусть заданы гринвичские координаты $ η_1 , η_2 , η_3 $ :\n",
    "$$ 2550804.91, 2460159.42, 5285668.08 м.$$\n",
    "Вычислите для этих координат значения географических и геоцентрических координат. Приведите код программы."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.00669436619\n",
      "initial coarse lambda, phi, height:  43.9636650190588 56.336859891872116 400.28741593852465\n",
      "1 lambda (degrees), phi (degrees), height (meters):  43.9636650190588 56.33721761434298 387.21258556662553\n",
      "2 lambda (degrees), phi (degrees), height (meters):  43.9636650190588 56.337217621949414 386.96616586996197\n",
      "3 lambda (degrees), phi (degrees), height (meters):  43.9636650190588 56.33721762194956 386.96616062782533\n",
      "\n",
      "STOP\n",
      "\n",
      "Final results: Geographic Frame-of-Reference\n",
      " number of iterations: 3 \n",
      " lambda: 43.9636650190588 degrees\n",
      " phi:    56.33721762194956 degrees\n",
      " height: 386.96616062782533 meters\n",
      "Final results: Geodetic\n",
      " number of iterations: 3 \n",
      " lambda: 43.9636650190588 degrees\n",
      " phi0:   56.1594476762467 degrees\n",
      " height: 386.96616062782533 meters\n"
     ]
    }
   ],
   "source": [
    "# problem variables and constants using the GLONASS model \n",
    "eta_1, eta_2, eta_3 = 2550804.91, 2460159.42, 5285668.08\n",
    "a = 6378136.0  # GLONASS model\n",
    "e2 = 6.69436619 * 10**(-3) \n",
    "print(e2)\n",
    "eps = 1.0/298.257839303 \n",
    "\n",
    "stop_condition = 1\n",
    "\n",
    "# initial condition\n",
    "\n",
    "lambd = atan2(eta_2, eta_1) \n",
    "\n",
    "phi0 = atan2(eta_3, sqrt(eta_1**2 + eta_2**2))\n",
    "phi_0 = phi0 + 0.5 * e2 * sin(2 * phi0)\n",
    "h_0 = sqrt(eta_1**2 + eta_2**2 + eta_3**2) - a + 0.5 * a * e2 * sin(phi_0)**2 \n",
    "\n",
    "Re = a/sqrt(1 - e2*sin(phi_0)**2) \n",
    "iterate_count = 0\n",
    "print('initial coarse lambda, phi, height: ', lambd*180/pi, phi_0*180/pi, h_0)\n",
    "while stop_condition > 0.001**2:\n",
    "    iterate_count +=1\n",
    "\n",
    "    #calculate temp derivatives\n",
    "    Re = a/sqrt(1 - e2*sin(phi_0)**2) \n",
    "    dRe_dphi = - (a * e2 * sin(phi_0) * cos(phi_0))/((1 - e2 * sin(phi_0)**2)**(3/2)) \n",
    "    df1_dphi = - dRe_dphi * cos(phi_0) +(Re + h_0)*sin(phi_0) \n",
    "    df2_dphi = - (1 - e2) * dRe_dphi * sin(phi_0) - ((1 - e2) * Re + h_0) * cos(phi_0) \n",
    "    df1_dh   = - cos(phi_0) \n",
    "    df2_dh   = - sin(phi_0) \n",
    "    \n",
    "    f1 = sqrt(eta_1**2 + eta_2**2) - (Re + h_0) * cos(phi_0) \n",
    "    f2 = eta_3 - ((1 - e2) * Re + h_0)*sin(phi_0) \n",
    "    # solve the equation\n",
    "    eq_coeff = df1_dphi * df2_dh - df2_dphi * df1_dh \n",
    "    phi_1 = phi_0 - (1/eq_coeff) * (f1 * df2_dh - f2 * df1_dh) \n",
    "    h_1 = h_0 - (1/eq_coeff) * (- f1 * df2_dphi + f2 * df1_dphi) \n",
    "    \n",
    "    delta_phi = phi_1 - phi_0 \n",
    "    delta_h = h_1 - h_0 \n",
    "    stop_condition = delta_phi**2 * a**2 + delta_h**2 \n",
    "    print(iterate_count,'lambda (degrees), phi (degrees), height (meters): ', lambd*180/pi, phi_1*180/pi, h_1)\n",
    "    phi_0 = phi_1 \n",
    "    h_0 = h_1 \n",
    "print('\\nSTOP\\n')\n",
    "print('Final results: Geographic Frame-of-Reference\\n',\n",
    "      'number of iterations:',iterate_count,'\\n lambda:',lambd*180/pi,'degrees\\n',\n",
    "      'phi:   ',phi_1*180/pi, 'degrees\\n','height:',h_1,'meters')\n",
    "\n",
    "print('Final results: Geodetic\\n',\n",
    "      'number of iterations:',iterate_count,'\\n lambda:',lambd*180/pi,'degrees\\n',\n",
    "      'phi0:  ',phi0*180/pi, 'degrees\\n','height:',h_1,'meters')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. \n",
    "Пусть заданы географические координаты — долгота, широта, высота:\n",
    "$$\\lambda = 0^◦ 16' 46.6381'' ,\n",
    "\\varphi = 51^◦ 33' 21.1362'' ,\n",
    "h = 428.923 м.$$\n",
    "Вычислите для этих координат значение вектора удельной силы тяготения двумя\n",
    "способами — через формулу Гельмерта и модель ГЛОНАСС. Сравните результаты\n",
    "в осях географического трехгранника."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "initial parameters: 0.004880319228061092 0.8998197005923435 428.923\n",
      "gelmert_x0 and absolute value:\n",
      " [[ 0.        ]\n",
      " [ 0.01655529]\n",
      " [-9.83171829]] 9.831732229384796 m/s^2\n",
      "glonass_eta and absolute value:\n",
      " [[-6.12090208]\n",
      " [-0.02987219]\n",
      " [-7.6589277 ]] 9.804361687693426 m/s^2\n",
      "glonass_x0 and absolute value:\n",
      " [[ 3.46944695e-18]\n",
      " [ 3.20908944e-02]\n",
      " [-9.80430917e+00]] m/s^2\n",
      "delta_g: gelmert_x0 - glonass_x0\n",
      " [[-3.46944695e-18]\n",
      " [-1.55355996e-02]\n",
      " [-2.74091221e-02]] m/s^2\n"
     ]
    }
   ],
   "source": [
    "# problem variables and constants using the GLONASS model \n",
    "lam = (0 + (16/60) + (46.6381/3600))*pi/180  # decimal representation in radians\n",
    "phi = (51 + (33/60) + (21.1362/3600))*pi/180\n",
    "h = 428.923\n",
    "u = 7.2921158553 * 10 ** (-5)\n",
    "\n",
    "\n",
    "# Gelmert model\n",
    "\n",
    "\n",
    "om_02 = 1.543 * 10 ** (-6)\n",
    "print('initial parameters:', lam, phi, h)\n",
    "g = 9.78030*(1 + 0.005302*sin(phi)**2 - 0.000007*sin(2*phi)**2) - 0.00014 - 2*om_02 * h\n",
    "gelmert_x0 = np.matrix([0, \n",
    "          u**2 * (Re + h) * sin(phi)*cos(phi),\n",
    "          -g - u**2 * (Re + h) * cos(phi)]).T\n",
    "print('gelmert_x0 and absolute value:\\n',gelmert_x0, sqrt(gelmert_x0[0]**2+gelmert_x0[1]**2+gelmert_x0[2]**2), \n",
    "      'm/s^2')\n",
    "\n",
    "\n",
    "# GLONASS model\n",
    "\n",
    "\n",
    "Re = a/sqrt(1 - e2*sin(phi)**2) \n",
    "eta_1 = (Re + h)*cos(phi)*cos(lam)\n",
    "eta_2 = (Re + h)*cos(phi)*sin(lam)\n",
    "eta_3 = (Re + h)*sin(phi) - Re * e2 * sin(phi)\n",
    "mu = 398600.44 * 10**9\n",
    "a = 6378136\n",
    "e2 = 6.69436619 * 10**(-3)\n",
    "C20 = - 1082.6257 * 10**(-6)\n",
    "r = sqrt(eta_1**2+eta_2**2+eta_3**2)\n",
    "\n",
    "\n",
    "g_eta1 = -mu/(r**3)*(1+1.5*C20*(a**2)/(r**2)*(5*eta_3**2/(r**2)-1))*eta_1 \n",
    "g_eta2 = -mu/(r**3)*(1+1.5*C20*(a**2)/(r**2)*(5*eta_3**2/(r**2)-1))*eta_2\n",
    "g_eta3 = -mu/(r**3)*(1+1.5*C20*(a**2)/(r**2)*(5*eta_3**2/(r**2)-1))*eta_3\n",
    "glonass_eta = np.matrix([g_eta1, g_eta2, g_eta3]).T\n",
    "print('glonass_eta and absolute value:\\n', glonass_eta, sqrt(g_eta1**2+g_eta2**2+g_eta3**2), 'm/s^2')\n",
    "\n",
    "# transfer to the geografic frame\n",
    "\n",
    "A_x0eta = np.matrix([[-sin(lam)          ,  cos(lam)          , 0],\n",
    "                     [-cos(lam)*sin(phi) , -sin(lam)*sin(phi) , cos(phi)],\n",
    "                     [ cos(lam)*cos(phi) ,  sin(lam)*cos(phi) , sin(phi)]])\n",
    "glonass_x0 = A_x0eta @ glonass_eta  # matrix product is denoted by @\n",
    "print('glonass_x0 and absolute value:\\n',glonass_x0, 'm/s^2')\n",
    "\n",
    "\n",
    "print('delta_g: gelmert_x0 - glonass_x0\\n', gelmert_x0 - glonass_x0, 'm/s^2')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "3. Вычислите с точностью до $e^6$ модули радиусов кривизны $R_E, R_N$ . Сравните приближенные значения с точными, используя географические координаты из предыдущей\n",
    "задачи."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "approximate Rn accurate up to e^6: 6394640.8463787725 m \n",
      "approximate Re accurate up to e^6: 6397941.572579555 m\n",
      "precise Rn: 6394640.847238055 m \n",
      "precise Re: 6397941.575140457 m\n",
      "delta Re:   0.002560901455581188 m \n",
      "delta Rn:   0.000859282910823822 m\n"
     ]
    }
   ],
   "source": [
    "phi = 51 + (33/60) + (21.1362/3600)\n",
    "a = 6378136\n",
    "e2 = 6.69436619 * 10**(-3)\n",
    "# precise Re and Rn:\n",
    "Re = a/sqrt(1 - e2*sin(phi)**2)\n",
    "Rn = (a*(1-e2))/(1 - e2*sin(phi)**2)**1.5\n",
    "# Taylor series up to e^6 for Re and Rn :\n",
    "Re_taylor = a*(1 + (1/2) * e2 * sin(phi)**2 + (3/8)*e2**2 * sin(phi)**4 + (15/48)*e2**3 * sin(phi)**6)\n",
    "Rn_taylor = a*(1+e2*(1.5*sin(phi)**2-1) +\n",
    "               (3/2)*e2**2*sin(phi)**2*((5/4)*sin(phi)**2 - 1) +\n",
    "               (5/8)*e2**3 * sin(phi)**4*((7/2)*sin(phi)**2-3) )\n",
    "print('approximate Rn accurate up to e^6:',Rn_taylor, 'm',\n",
    "      '\\napproximate Re accurate up to e^6:',Re_taylor,'m')\n",
    "print('precise Rn:',Rn, 'm','\\nprecise Re:',Re, 'm')\n",
    "print('delta Re:  ',Re-Re_taylor, 'm','\\ndelta Rn:  ',Rn-Rn_taylor, 'm')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4.\n",
    "Вычислите кватернион ориентации приборного трехгранника $Ms$ относительно гринвичского трехгранника $Oη$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Решение.\n",
    "Имеем кватернионы ориентации гринвичского трехгранника $M\\eta $ относительно географического трехгранника $Mx^0$\n",
    "и кватернион ориентации географического трехгранника $Mx^0$ относительно приборного $Ms$:\n",
    "\n",
    "\\begin{eqnarray*}\n",
    "P^{  \\eta x^0}=\\frac{1}{2}\n",
    " \\left(\n",
    " \\begin{array}{c}\n",
    " \\phantom{-} \\cos \\frac{\\lambda + \\varphi}{2} - \\sin \\frac{ \\lambda - \\varphi}{2} \\cr\n",
    " -\\cos \\frac{\\lambda - \\varphi}{2} + \\sin \\frac{ \\lambda + \\varphi}{2} \\cr\n",
    " -\\cos \\frac{\\lambda + \\varphi}{2} - \\sin \\frac{ \\lambda - \\varphi}{2} \\cr\n",
    " -\\cos \\frac{\\lambda - \\varphi}{2} - \\sin \\frac{ \\lambda + \\varphi}{2}\n",
    "\\end{array}\n",
    "\\right),\n",
    "\\end{eqnarray*}\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\\begin{eqnarray*}\n",
    "P^{x^0s}= \\left( \\begin{array}{c}\n",
    "\\phantom{-} \\cos \\frac{\\psi}{2} \\cos \\frac{\\vartheta}{2}  \\cos \\frac{\\gamma}{2} + \\sin \\frac{\\psi}{2} \\sin \\frac{\\vartheta}{2}  \\sin \\frac{\\gamma}{2} \\cr\n",
    "-\\cos \\frac{\\psi}{2} \\sin \\frac{\\vartheta}{2}  \\cos \\frac{\\gamma}{2} - \\sin \\frac{\\psi}{2} \\cos \\frac{\\vartheta}{2}  \\sin \\frac{\\gamma}{2} \\cr\n",
    "- \\cos \\frac{\\psi}{2} \\cos \\frac{\\vartheta}{2}  \\sin \\frac{\\gamma}{2} + \\sin \\frac{\\psi}{2} \\sin \\frac{\\vartheta}{2}  \\cos \\frac{\\gamma}{2} \\cr\n",
    "\\phantom{-} \\sin \\frac{\\psi}{2} \\cos \\frac{\\vartheta}{2}  \\cos \\frac{\\gamma}{2} - \\cos \\frac{\\psi}{2} \\sin \\frac{\\vartheta}{2}  \\sin \\frac{\\gamma}{2}\n",
    "\\end{array} \\right).\n",
    "\\end{eqnarray*}\n",
    "\n",
    "Результирующий поворот вычисляется по формуле:\n",
    "\\begin{eqnarray*}\n",
    "r_0 &=& p_0 q_0 - p_1 q_1 - p_2 q_2 - p_3 q_3 \\nonumber \\\\\n",
    "r_1 &=& q_0 p_1 + p_0 q_1 - p_3 q_2 + p_2 q_3 \\nonumber \\\\\n",
    "r_2 &=& q_0 p_2 + p_0 q_2 + p_3 q_1  - p_1 q_3 \\nonumber \\\\\n",
    "r_3 &=& q_0 p_3 + p_0 q_3 - p_2 q_1 + p_1 q_2,\n",
    "\\end{eqnarray*}\n",
    "где параметры $p$ отвечают первому повороту, параметры $q$ - второму, а $r$ - результирующему повороту. Для получения верного ответа нужно найти сопряжённый кватернион\n",
    "$\\widetilde{P}^{\\eta s}= (r_0, -r_1, -r_2, -r_3)^T$, отвечающий обратному повороту:\n",
    "\\begin{eqnarray*}\n",
    "P^{s \\eta} = \\widetilde{P}^{\\eta s}.\n",
    "\\end{eqnarray*}\n",
    "\n",
    "Выражение вышло очень громоздким и трудносокращаемым. Чтобы перепроверить собственные выкладки, подключим библиотеку вычисления символьных математических выражений:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\sin{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} \n",
      "\n",
      "\\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} + \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} + \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} \n",
      "\n",
      "- \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2}\\right) + \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2}\\right) - \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} - \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \n",
      "\n",
      "- \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2}\\right) - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2}\\right) - \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} + \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \n",
      "\n"
     ]
    }
   ],
   "source": [
    "from sympy import * # allows using symbolic mathematical equations\n",
    "init_printing(use_unicode=True)\n",
    "\n",
    "# add our angles as symbols\n",
    "phi = symbols('varphi')\n",
    "lam = symbols('lambda')\n",
    "psi = symbols('psi')\n",
    "the = symbols('vartheta')\n",
    "gam = symbols('gamma')\n",
    "# 'the' stands for theta for sake of readability like 'gam' for gamma and 'lam' for lambda\n",
    "\n",
    "p0 =   (cos((lam+phi)/2) - sin((lam-phi)/2))/2\n",
    "p1 = - (cos((lam-phi)/2) + sin((lam+phi)/2))/2\n",
    "p2 = - (cos((lam+phi)/2) - sin((lam-phi)/2))/2\n",
    "p3 = - (cos((lam-phi)/2) - sin((lam+phi)/2))/2\n",
    "\n",
    "q0 =   cos(psi/2)*cos(the/2)*cos(gam/2) + sin(psi/2)*sin(the/2)*sin(gam/2)\n",
    "q1 = - cos(psi/2)*sin(the/2)*cos(gam/2) - sin(psi/2)*cos(the/2)*sin(gam/2)\n",
    "q2 = - cos(psi/2)*cos(the/2)*sin(gam/2) + sin(psi/2)*sin(the/2)*cos(gam/2)\n",
    "q3 =   sin(psi/2)*cos(the/2)*cos(gam/2) - cos(psi/2)*sin(the/2)*sin(gam/2)\n",
    "\n",
    "r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3\n",
    "r1 = q0*p1 + p0*q1 - p3*q2 + p2*q3\n",
    "r2 = q0*p2 + p0*q2 + p3*q1 - p1*q3\n",
    "r3 = q0*p3 + p0*q3 - p2*q1 + p1*q2\n",
    "\n",
    "# preparation to include equation in LaTeX\n",
    "print(latex(trigsimp(r0, fold_func_brackets = True)),'\\n')\n",
    "print(latex(trigsimp(-r1)),'\\n')\n",
    "print(latex(trigsimp(-r2)),'\\n')\n",
    "print(latex(trigsimp(-r3)),'\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Таким образом, результирующий кватернион ориентации приборного трехгранника $Ms$ относительно гринвичского трехгранника $Oη$ имеет такой вид (покоординатно):\n",
    "\n",
    "\\begin{eqnarray*}\n",
    "r_0 &=& \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\sin{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} \n",
    " \\nonumber \\\\\n",
    "\\end{eqnarray*}\n",
    "\\begin{eqnarray*}\n",
    "r_1 &=& \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} + \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} + \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} - \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\cos{\\left(\\frac{2 \\lambda + \\pi}{4} \\right)} \\cos{\\left(\\frac{2 \\varphi + \\pi}{4} \\right)} \n",
    "  \\nonumber \\\\\n",
    "\\end{eqnarray*}\n",
    "\\begin{eqnarray*}\n",
    "r_2 &=& - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2}\\right) + \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2}\\right) - \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} - \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)}    \\nonumber \\\\\n",
    "\\end{eqnarray*}\n",
    "\\begin{eqnarray*}\n",
    "r_3 &=& - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} + \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2}\\right) - \\left(\\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)}\\right) \\left(\\frac{\\sin{\\left(\\frac{\\lambda - \\varphi}{2} \\right)}}{2} - \\frac{\\cos{\\left(\\frac{\\lambda + \\varphi}{2} \\right)}}{2}\\right) - \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)}\\right) \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \\cos{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} + \\left(- \\sin{\\left(\\frac{\\gamma}{2} \\right)} \\cos{\\left(\\frac{\\psi}{2} \\right)} \\cos{\\left(\\frac{\\vartheta}{2} \\right)} + \\sin{\\left(\\frac{\\psi}{2} \\right)} \\sin{\\left(\\frac{\\vartheta}{2} \\right)} \\cos{\\left(\\frac{\\gamma}{2} \\right)}\\right) \\sin{\\left(\\frac{\\lambda}{2} + \\frac{\\pi}{4} \\right)} \\sin{\\left(\\frac{\\varphi}{2} + \\frac{\\pi}{4} \\right)} \n",
    "\\end{eqnarray*}\n",
    "\n",
    "Я честно пытался придумать какие-то замены, существенно упрощающие вид выражений, но не смог.\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. \n",
    "Приведите расчетные формулы (по Сэвиджу) для двухшагового метода в задаче\n",
    "ориентации (в скалярной форме)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Решение\n",
    "Имеем расчётную формулу (по Сэвиджу) в таком виде:\n",
    "\\begin{eqnarray*}\n",
    "\\gamma_y = \\sum\\limits_{k=1}^n \\gamma_{y_k} -\n",
    "\\sum\\limits_{k=2}^n\\left[\\frac12\\left(\\sum\\limits_{i=1}^{k-1}\\widehat{\\gamma}_{y_i} +\n",
    "\\frac16\\widehat{\\gamma}_{y_{k-1}}\\right)\\gamma_{y_k}\\right].\n",
    "\\end{eqnarray*}\n",
    "При $n = 2$ можем раскрыть знаки сумм следующим образом:\n",
    "\\begin{eqnarray*}\n",
    "\\gamma_y = \\gamma_{y_1} + \\gamma_{y_2} -\n",
    " \\frac12\\left(\\widehat{\\gamma}_{y_1} +\n",
    "\\frac16\\widehat{\\gamma}_{y_1}\\right)\\gamma_{y_2},\n",
    "\\end{eqnarray*}\n",
    "\n",
    "Далее получим:\n",
    "\\begin{eqnarray*}\n",
    "\\gamma_y = \\gamma_{y_1} + \\gamma_{y_2} -\n",
    " \\frac{7}{12}\\widehat{\\gamma}_{y_1}\\gamma_{y_2},\n",
    "\\end{eqnarray*}\n",
    "Чтобы не запутаться в индексах, однообразно обозначающих и момент времени (номер измерения), и номер координаты, будем обозначать номер измерения в скобках, а номер координаты -- нижним индексом, а именно так: $\\gamma_{y1}(2)$ -- значение первой координаты вектора $\\gamma$ в отсчёт времени 2.\n",
    "Тогда получим:\n",
    "\\begin{eqnarray*}\n",
    "\\gamma_{y1} &=& \\gamma_{y1}(1)+\\gamma_{y1}(2)-\\frac{7}{12}\\left(\\phantom{-}\\gamma_{y3}(1)\\gamma_{y2}(2)-\\gamma_{y2}(1)\\gamma_{y3}(2) \\right), \\\\\n",
    "\\gamma_{y2} &=& \\gamma_{y2}(1)+\\gamma_{y2}(2)-\\frac{7}{12}\\left(-\\gamma_{y3}(1)\\gamma_{y2}(2)+\\gamma_{y1}(1)\\gamma_{y3}(2) \\right), \\\\\n",
    "\\gamma_{y3} &=& \\gamma_{y3}(1)+\\gamma_{y3}(2)-\\frac{7}{12}\\left(\\phantom{-}\\gamma_{y2}(1)\\gamma_{y1}(2)-\\gamma_{y1}(1)\\gamma_{y2}(2) \\right). \\\\\n",
    "\\end{eqnarray*}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2 блок"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1 задача\n",
    "Введем «замороженный» относительно Земли трехгранник, совпадающий с приборным трехгранником самолета в начальный момент времени. Выведите динамические\n",
    "уравнения движения в абсолютных переменных в осях этого трехгранника."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Решение\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Сначала выпишем матрицы ориентации трехгранников друг относительно друга:\n",
    "\n",
    "\\begin{eqnarray*}\n",
    "O\\eta \\quad\n",
    "\\xrightarrow[3]{\\frac{\\pi}{2} +\\lambda}\\\n",
    "\\xrightarrow[1]{\\frac{\\pi}{2} -\\varphi}\n",
    "  \\quad Ox^0\\\n",
    "  \\xrightarrow[3]{-\\vartheta}\\\n",
    "  \\xrightarrow[1]{\\psi}\\\n",
    "  \\xrightarrow[2]{\\gamma}\\\n",
    "    \\quad Oz^0_0\\\n",
    "    \\xrightarrow[]{}\n",
    "    \\quad Oz^0\n",
    "\\end{eqnarray*}\n",
    "\n",
    "\\begin{eqnarray*}\n",
    "A_{ \\eta x^0} =\n",
    "\\left(\n",
    "  \\begin{array}{ccc}\n",
    "          -\\sin \\lambda & -\\cos \\lambda \\sin \\varphi & \\cos \\lambda \\cos \\varphi  \\cr\n",
    "          \\phantom{-}\\cos \\lambda & -\\sin \\lambda \\sin \\varphi & \\sin \\lambda \\cos \\varphi \\cr\n",
    "          0 & \\cos \\varphi  & \\sin \\varphi\n",
    "  \\end{array}\n",
    "\\right).\n",
    "\\end{eqnarray*}\n",
    "\n",
    "\\begin{eqnarray*}\n",
    "A_{z^0 x^0} = \\left( \\begin{array}{ccc}\n",
    "\\cos \\psi \\cos \\gamma + \\sin \\psi \\sin \\vartheta \\sin \\gamma & - \\sin \\psi \\cos \\gamma + \\cos \\psi \\sin \\vartheta \\sin \\gamma & -\\cos \\vartheta \\sin \\gamma \\cr\n",
    "\\sin \\psi \\cos \\vartheta   &  \\cos \\psi \\cos \\vartheta &  \\sin \\vartheta  \\cr\n",
    "\\cos \\psi \\sin \\gamma - \\sin \\psi \\sin \\vartheta \\cos \\gamma & -\\sin \\psi \\sin \\gamma - \\cos \\psi \\sin \\vartheta \\cos \\gamma & \\phantom{-} \\cos \\vartheta \\cos \\gamma\n",
    "\\end{array} \\right).\n",
    "\\end{eqnarray*}\n",
    "\n",
    "\\begin{eqnarray*}\n",
    "A_{x^0 s} = \\left( \\begin{array}{ccc}\n",
    "\\phantom{-}  \\cos \\psi \\cos \\gamma + \\sin \\psi \\sin \\vartheta \\sin \\gamma & \\sin \\psi \\cos \\vartheta   &\\phantom{-} \\cos \\psi \\sin \\gamma - \\sin \\psi \\sin \\vartheta \\cos \\gamma \\cr\n",
    "- \\sin \\psi \\cos \\gamma + \\cos \\psi \\sin \\vartheta \\sin \\gamma &  \\cos \\psi \\cos \\vartheta & -\\sin \\psi \\sin \\gamma - \\cos \\psi \\sin \\vartheta \\cos \\gamma \\cr\n",
    " -\\cos \\vartheta \\sin \\gamma &  \\sin \\vartheta  &  \\cos \\vartheta \\cos \\gamma\n",
    "\\end{array} \\right).\n",
    "\\end{eqnarray*}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}