```python
import numpy as np
import scipy.constants as sc
import matplotlib.pyplot as plt
```

```python
kb = 8.617E-5 # eV/K
mu = 1 # eV
f = lambda e, t: 1/(np.exp((e-mu)/(kb*t)) + 1)
```

```python
n = 100
ts = [0.01, 300, 1000, 2000, 5000, 9000, 20000]
es = np.linspace(0, 1, num=n)

result = np.zeros([n, len(ts)])
for i, temp in enumerate(ts):
    result[:, i] = f(es, temp)
```

```python
plt.plot(es, result)
plt.xlabel('E(eV)')
plt.ylabel('f(E,$\mu$,T)')
plt.show()

```

Problem 2 (a)

\begin{align}
    C_{self} &= 4\pi \epsilon_{0} R
\end{align}

```python
ep = 8.854E-12 # F/m
d = 10E-9 # m
c_0 = 4*np.pi*ep*(d/2)
print(f'The self-capacitance is {c_0:.2e}F.')
```

    The self-capacitance is 5.56e-19F.

Problem 2 (b)

\begin{align}
    V &= \frac{Q}{C} \\
      &= \frac{e}{C} \\
\end{align}

```python
v_single = sc.elementary_charge/c_0
print(f'The voltage will change by {v_single:.2f}V.')
```

    The voltage will change by 0.29V.

Problem 2 (c)

```python
m_to_cm = 100 # cm/m
rho_al = 2.7 # g/cm^3
am_al = 26.982 # g/mol
e_per_atom = 3
v_sphere = 4/3 * np.pi * (d/2 * m_to_cm)**3
num_atom = rho_al * v_sphere / am_al * sc.N_A
num_e = num_atom * e_per_atom
print(f'There are {num_e:.2e} free electrons in the sample.')
```

    There are 9.47e+04 free electrons in the sample.

Problem 2 (e)

\begin{align}
    e \phi(V_{th}) &= \frac{e^2}{2C} \\
    e \frac{C_2 V_{th}}{C} &= \frac{e^2}{2C} \\
    V_{th} &= \frac{e}{2C_2}
\end{align}

```python
# Problem 2 (f)
c_2 = 8E-18 # F
v_th = sc.elementary_charge/(2*c_2)
print(f'The Coulomb blockade threshold voltage is {v_th:.2f} V.')
```

    The Coulomb blockade threshold voltage is 0.01 V.

Problem 3 (a)

\begin{align}
    D(E) &= \frac{1}{A} \frac{dN(E)}{dE} \\
    &= \frac{1}{A} \frac{d}{dE} \bigg(\frac{L^2}{4\pi}\bigg) k^2 \\
    &= \frac{1}{A} \frac{d}{dE} \bigg(\frac{L^2}{4\pi}\bigg) \bigg( \frac{E}{\hbar v_{f}} \bigg)^2 \\
    &= \frac{1}{A} \frac{L^2}{2\pi \hbar^2 v_{f}^2} E
\end{align}

The $L^2$ term is the area of the sample. So $A$ and $L^2$ canceled out. Also the $E(k)$ can be either positive
or negative, but the density of states cannot be negative. So we multiply it by $2$, and take $E$ to absolute value.
Therefore, the density of states per spin of graphene is given by

\begin{align}
    D(E) &= \frac{1}{\pi \hbar^2 v_{f}^2} |E|
\end{align}

```python
nu_f = 1E8
e_3b = 100E-3 # eV
hbar = sc.physical_constants['reduced Planck constant in eV s'][0]
d_e = (np.pi*(hbar**2)*(nu_f**2))**(-1)*np.abs(e_3b)
print(f'The value of D(E) is {d_e:.2e} (eV cm^2)^-1.')
```

    The value of D(E) is 7.35e+12 (eV cm^2)^-1.


Fourier Analysis using SymPy

 MCHE 485: Mechanical Vibrations

Dr. Joshua Vaughan

joshua.vaughan@louisiana.edu

http://www.ucs.louisiana.edu/~jev9637/

This notebook will look at the Fourier Analysis of periodic functions. In this notebook, we'll use [SymPy](http://sympy.org/en/index.html), "a Python library for symbolic mathematics."

We can write any periodic function as an infinite sum of sines and cosines:

$ \quad f(t) = \sum_{n=0}^{\infty}a_n\cos(n\omega_0t) + \sum_{n=1}^{\infty}b_n\sin(n\omega_0t) $

where

$ \quad a_n = \frac{\omega_0}{\pi}\int_0^{\frac{2\pi}{\omega_0}}f(t)\cos(n\omega_0t)dt $,

$ \quad b_n = \frac{\omega_0}{\pi}\int_0^{\frac{2\pi}{\omega_0}}f(t)\sin(n\omega_0t)dt $,

and

$ \quad a_0 = \frac{\omega_0}{2\pi}\int_0^{\frac{2\pi}{\omega_0}}f(t)dt $

For more information on this process, you can see the lectures at the [class website](http://www.ucs.louisiana.edu/~jev9637/MCHE485.html).

By changing the number of terms we use to approximate the original function we can approach its shape, as shown in Figure 1.

     Figure 1: Increasing the Number of Terms in the Approximation

The remainder of this notebook will focus on determining the $a_n$ and $b_n$ terms in the Fourier Expansion.

```python
# import Sympy and start "pretty printing"
import sympy
sympy.init_printing()
```

```python
# Define the sympy symbolic variables we'll need
t, w0, tau_0 = sympy.symbols(['t', 'omega_0', 'tau_0'], real=True, positive=True)
```

```python
#--------- Input your function to examine here --------

# Use the sympy Piecewise function to define the square wave - This matches the one in the Figure 1 above.
y = 2 + sympy.Piecewise((1, t < sympy.pi/w0), (-1, t > sympy.pi/w0))

# Use the sympy Piecewise function to define the triangle wave
# First define F0
# F0 = sympy.symbols('F0')
# y = sympy.Piecewise((F0/2*t, t < sympy.pi/w0), (-(F0/2)*t + 2*F0, t >= sympy.pi/w0))

# Use the sympy Piecewise function to define a trapezoid function
# y = sympy.Piecewise((3*F0*w0/(2*sympy.pi)*t, t < (2*sympy.pi/(3*w0))), (F0, t < (4*sympy.pi/(3*w0))),
#                     (-3*F0*w0/(2*sympy.pi)*t + 3*F0, t > (4*sympy.pi/(3*w0))))
```

```python
# define the number of terms to use in the approximation
num_terms = 7
```

```python
# get the a0 term
a0 = w0 / (2*sympy.pi) * sympy.integrate(y, (t, 0, 2*sympy.pi/w0))
```

```python
# Define matrices of 0s to fill the the an and bn terms
a = sympy.zeros(1, num_terms)
b = sympy.zeros(1, num_terms)

# cycle through the 1 to num_terms Fourier coefficients (a_n and b_n)
for n in range(num_terms):
    integral_cos = y * sympy.cos((n+1)*w0*t)         # define the integral "interior"
    a[n] = w0 / sympy.pi * sympy.integrate(integral_cos, (t, 0, 2*sympy.pi/w0))    # solve for a_n

    integral_sin = y * sympy.sin((n+1)*w0*t)         # define the integral "interior"
    b[n] = w0 / sympy.pi * sympy.integrate(integral_sin, (t, 0, 2*sympy.pi/w0))    # solve for b_n
```

```python
# Simplify and display a0
sympy.simplify(a0)
```

```python
# Simplify and display the an terms
sympy.simplify(a)
```

```python
# Simplify and diplay the bn terms
sympy.simplify(b)
```

#### Licenses
Code is licensed under a 3-clause BSD style license. See the licenses/LICENSE.md file.

Other content is provided under a [Creative Commons Attribution-NonCommercial 4.0 International License](http://creativecommons.org/licenses/by-nc/4.0/), CC-BY-NC 4.0.

```python
# This cell will just improve the styling of the notebook
from IPython.core.display import HTML
import urllib.request
response = urllib.request.urlopen("https://cl.ly/1B1y452Z1d35")
HTML(response.read().decode("utf-8"))
```

Jupyter Notebooks/Fourier Analysis - Symbolic.ipynb


#  École des Ponts ParisTech
##  SPH pour l'hydraulique
###  SPyH Séance de TD 3
####  Écoulement Confiné : Forces de viscosité, conditions au mur
 Rémi Carmigniani et Damien Violeau

But de la séance de TD3
--

Dans cette séance, nous allons résoudre les équations de Navier-Stokes pour un fluide faiblement compressible en utilisant la méthode SPH. Nous nous intéressons à l'écoulement de Poiseuille plan.

Les équations s'écrivent :

\begin{align}
\frac{\mathbf{v}}{dt} &= -\frac{1}{\rho} \mathbf{\nabla} p + \mathbf{g} + \nu \Delta \mathbf{v},\\
\frac{\rho}{dt} &= -\rho \nabla\cdot \mathbf{v},\\
p &= B \left(\frac{\rho^\gamma}{\rho_0^\gamma} - 1\right),
\end{align}

où la dernière equation correspond à l'équation d'état pour un gaz polytropique avec $B ={\rho_0} {c_0}^2/\gamma$.

```python
%load_ext autoreload
%autoreload 2
import numpy as np
from sys import exit
import os.path
from os import path
import csv
import time
import matplotlib
import matplotlib.pyplot as plt
matplotlib.rcParams['text.usetex'] = True
from src.spyh import *
from src.checkTD3 import *
from src.sphvar import *
from src.plotParticles import *
from src.state import *
from src.contrib import *
from src.analytical_solutions import *
```

Partie 1 : Un peu de théorie et construction du cas test
---

Nous considérons un écoulement entre deux plaques infinies séparées d'une distance de $2e$, d'un fluide de densité $\rho_0  = 1000$ kg.m$^{-3}$ dans un champ gravitaire $ \mathbf{g} = g \mathbf{e}_x$ m.s$^{-2}$ (qui joue le rôle d'un gradient de pression moteur de l'écoulement). La configuration est représentée dans la figure ci-dessous. L'écoulement est considéré permanent selon $\mathbf{e}_x$. Nous pouvons donc considérer un domaine périodique dans cette direction.

**1-a) Rappelez la solution de l'écoulement de Poiseuille en régime permanent.**

On définera $Re = \rho_0 U_0 e/\mu$, où $U_0$ est la vitesse maximum, $e$ la demi largeur, $\mu$ la viscosité dynamique.

**Montrez qu'il faut :**
**\begin{equation}
g = \frac{2U_0\mu}{\rho_0 e^2}
\end{equation}**

**1-b) Définissez dans la cellule suivante $\mu$ et $g$ en fonction des autres paramètres du problème.**

```python
#POISEUILLE PARAMETERS
e = 0.5 #half width in meters
U0 = 1 # maximum velocity m/s
Re = 1 # Reynolds number
#FLUID PARAMETERS
rhoF = 1000
#TODO : COMPLETE HERE
mu = rhoF*U0*e/Re #(Pa.s)
grav = np.array([1,0])*2*U0*mu/(rhoF*e**2)
#END
q1b(grav,mu)
```

    Your values :
    	 g = 4.00
    	 mu = 500.00
    1-b) All good!

Dans la cellule suivante on définit les paramètres de fluides, de géométrie et de simulation.

```python
#OTHER FLUID PARAMETERS
c0 = 10*U0
gamma = 7
B = rhoF*c0**2/gamma
#DENSITY & SHEPARD THRESHOLDS :
shepardMin = 10**(-6)
rhoMin = 0.5*rhoF
rhoMax = 1.5*rhoF

#PARTICLES & SPACES PARAMETERS :
N = 4
dr = 2*e/(4*N)
h = smthfc*dr
m=dr*dr*rhoF
lspace = 2*h

#COMPUTATION DOMAIN :
xOrigin = 0
yOrigin = -e-nBound*dr
xSize = 4*lspace
ySize = 2*e+2*nBound*dr
xMax = xOrigin+xSize
yMax = yOrigin+ySize
```

**1-c) Construisez la géométrie en utilisant la fonction *addBox* et les FLAG :  FLUID pour les particules de fluide et BOUND pour les particules de mur. Notez que la base fait $4\times l_s$ où $l_s = 2h$.**

Les FLAGs sont définis dans le fichier [src/sphvar.py](src/sphvar.py)

Les murs seront composés d'une couche de *nBound =4* particules *fictives*. Ce nombre est suffisant pour éviter la pénétration du mur.

Pour ajouter des particules au tableau *part*, utilisez la fonction *addBox* :

```python
part = addBox(part,[x_0,y_0,L_x,L_y],FLAG,dr,rhoF)
```

cette commande ajoute à part des particules de type FLAG dans le domaine rectangulaire : $\left[x_0,x_0+L_x\right]\times\left[y_0,y_0+L_y\right]$

```python
#INIT PART:
part = init_particles()
```

```python
#% COMPLETE HERE
part = addBox(part,[0,-e,4*lspace,2*e],FLUID,dr,rhoF)
part = addBox(part,[0,-e-nBound*dr,4*lspace,nBound*dr],BOUND,dr,rhoF)
part = addBox(part,[0,e,4*lspace,nBound*dr],BOUND,dr,rhoF)
# END
```

```python
len(part)
```

    384

Vous devriez avoir 384 particules avec dr = 2e/16 ou N=4.

**Affichage de la vitesse horizontale**

```python
%matplotlib notebook
Umax= 1
tabUx = part[:,VEL[0]]
domain = [xOrigin,xMax,yOrigin,yMax,0,Umax]
plotPropertiesWithBound(part,tabUx,r'$U_x$',domain,dr,1)
```

    <IPython.core.display.Javascript object>

**1-d) Le domaine que nous considérons est périodique selon $\mathbf{e}_x$. Pour définir cette périodicité dans le code, nous avons besoin de spécifier les *spaces* voisines. En particulier pour la *space* 0 : elle a pour voisine avec la résolution $N=15$, les *spaces* 1, 6 et 7 mais aussi les *spaces* 18 et 19 (voir figure)! Vous pouvez vérifier que la construction est bien faite en variant le nombre de particules.**

```python
#PERIODICITY VECTOR
vecPer = np.array([4*lspace,0])
posSpace,neibSpace,partSpace,listNeibSpace = \
init_spaces(xOrigin,yOrigin,xSize,ySize,lspace,dr,vecPer)
```

```python
plotSpaces(posSpace,'k',lspace,1)
neibSpace[0][neibSpace[0]>-1]
```

    array([ 0,  1,  6,  7, 18, 19])

```python
spacesOutline(posSpace[neibSpace[0][neibSpace[0]>-1]],'r',lspace,1)
```

```python
f = plt.figure(1)
figName = 'Figures/periodic.pdf'
f.savefig(figName,bbox_inches='tight')
```

Partie 2 : Calcul des forces de viscosité : Modèles de Morris *et al.* et de Monaghan & Gingold
---

Nous allons ajouter deux modèles de viscosité : Morris *et al.* puis de Monaghan \& Gingold . Nous rappelons :

\begin{align}
\frac{1}{\rho_i}\mu \mathbf{L}^{visc,Mor}_i \left\{ \mathbf{v}_j \right\}&=\sum_j 2 \mu \frac{m}{\rho_i\rho_j} (\mathbf{v}_i-\mathbf{v}_j) \frac{\mathbf{r}_{ij} \cdot \nabla w_{ij}}{r_{ij}^2} \\
&=\sum_j  \mathbf{F}^{Visc,Mor}_{ij}
\end{align}

\begin{align}
\frac{1}{\rho_i}\mu \mathbf{L}^{visc,M\&G}_i \left\{ \mathbf{v}_j \right\}&=\sum_j 2\left(d+2\right) \mu \frac{m}{\rho_i\rho_j} \frac{\mathbf{r}_{ij}\cdot (\mathbf{v}_i-\mathbf{v}_j)}{r^2_{ij}}\nabla w_{ij}\\
&=\sum_j  \mathbf{F}^{Visc,M\&G}_{ij}
\end{align}

**2-a) Complétez *contrib.py* afin de créer deux fonctions :**
* **MonaghanViscContrib**
* **MorrisViscContrib**

**Les deux fonctions prendront en argument :**
```python
F = xxxViscContrib(mu,rho_i, rho_j,dwdr,rVel,rPos,m)
```
**Elles retournent la force $F_{i,j}$.**

**Indications :** vous pouvez vous inspirer de ce qui est fait pour la viscosité artificielle.

Vous pouvez tester votre implémentation avec la cellule suivante.

```python
q2a()
```

    Morris is correctly implemented
    Monaghan is correctly implemented

Une fois que vous avez correctement implémenté vos fonctions, ajouté @njit avant la fonction pour autoriser la compilation et redémarrez le noyau.

**2-b) En vous inspirant de *computeForcesARTPeriodicX* dans *spyh.py*, créer des fonctions *computeForcesMorrisPeriodicX* et *computeForcesMonaghanPeriodicX* pour calculer les forces sur les particules avec les différentes forces de viscosité.**

**Notes :**

Nous avons ajouté le cas périodic selon X.

```python
part,partSpace = sortPart(part,posSpace,partSpace,xOrigin,yOrigin,xSize,ySize,lspace,dr)
listNeibSpace= getListNeib(partSpace,neibSpace,listNeibSpace)
```

```python
dt=CFLConditions(part[:,VEL],h,c0,grav,rhoF,mu)
part[:,RHO],part[:,VEL] =interpolateBoundaryPeriodicX((part[:,INFO]==BOUND),\
                                     part[:,SPID],\
                                     part[:,POS],\
                                     part[:,VEL],\
                                     part[:,RHO],\
                                     listNeibSpace,\
                                 aW,h,m,B,rhoF,gamma,grav,vecPer[0],shepardMin)
part[:,FORCES],part[:,DRHODT] = computeForcesMorrisPeriodicX((part[:,INFO]==FLUID),\
                                                 part[:,SPID],\
                                                 part[:,POS],\
                                                 part[:,VEL],\
                                                 part[:,RHO],\
                                                 listNeibSpace,\
                                                 aW,h,m,B,rhoF,gamma,grav,mu,vecPer[0])
part[:,POS],part[:,VEL],part[:,RHO] = integrationStepPeriodicX((part[:,INFO]==FLUID),\
                                                       part[:,POS],\
                                                       part[:,VEL],\
                                                       part[:,RHO],\
                                                       part[:,FORCES],\
                                                       part[:,DRHODT],\
                                                     0,vecPer[0])
```

```python
current_directory = os.getcwd()
case_directory = os.path.join(current_directory, r'Results/Poiseuille_'+time.strftime("%Y%m%d_%H%M%S"))
os.mkdir(case_directory)
data_directory = os.path.join(case_directory,r'Data')
figures_directory = os.path.join(case_directory,r'Figures')
os.mkdir(data_directory)
os.mkdir(figures_directory)
```

```python
# Here we specify the output frequencies
dt_figure = 0.025*e**2*rhoF/mu
t_print = 0
#final time :
t_end = 0.7
t=0
it=0
im_count=0
ytab = np.linspace(-1,1,100)
timetab = np.linspace(0,5,60)
tau = 2*e/U0
Uan_with_time = analyticalPoiseuilleFlow(0,Re,timetab*rhoF/mu*e**2/tau)
Utab = 1-ytab**2
timeTabU = np.empty((0,2),float)
```

```python
%matplotlib notebook
while t<t_end:
    #STEP1 : Calcul de la CFL
    dt = CFLConditions(part[:,VEL],h,c0,grav,rhoF,mu)
    #STEP2 : Interpolation des conditions au bord
    part[:,RHO],part[:,VEL] =interpolateBoundaryPeriodicX((part[:,INFO]==BOUND),\
                                     part[:,SPID],\
                                     part[:,POS],\
                                     part[:,VEL],\
                                     part[:,RHO],\
                                     listNeibSpace,\
                                 aW,h,m,B,rhoF,gamma,grav,vecPer[0],shepardMin)
    #STEP3 : Calcul des forces et des termes de densité
    part[:,FORCES],part[:,DRHODT] = computeForcesMorrisPeriodicX((part[:,INFO]==FLUID),\
                                                 part[:,SPID],\
                                                 part[:,POS],\
                                                 part[:,VEL],\
                                                 part[:,RHO],\
                                                 listNeibSpace,\
                                                 aW,h,m,B,rhoF,gamma,grav,mu,vecPer[0])
    #STEP4 : Integration en temps
    part[:,POS],part[:,VEL],part[:,RHO] = integrationStepPeriodicX((part[:,INFO]==FLUID),\
                                                       part[:,POS],\
                                                       part[:,VEL],\
                                                       part[:,RHO],\
                                                       part[:,FORCES],\
                                                       part[:,DRHODT],\
                                                       dt,vecPer[0])
    #STEP5 : Corriger densité trop basse
    part[:,RHO] = checkDensity(part[:,RHO],rhoMin,rhoMax)
    #STEP6 : Mise à jour des voisins (pas forcément à tous les pas de temps)
    part,partSpace = sortPart(part,posSpace,partSpace,xOrigin,yOrigin,xSize,ySize,lspace,dr)
    listNeibSpace= getListNeib(partSpace,neibSpace,listNeibSpace)
    t +=dt
    it +=1
    if t>=t_print:
        fig = plt.figure(1)
        plt.clf()
        plt.title(r'$t\nu/e^2 = %2.2f$'%(t*mu/rhoF/e**2))
        velMagn = (part[:,VEL[0]]*part[:,VEL[0]]+part[:,VEL[1]]*part[:,VEL[1]])**.5
        domain = [xOrigin,xMax,yOrigin,yMax,0,1]
        plotPropertiesWithBound(part,velMagn,r'$u/U_0$',domain,dr,1)
        figname = os.path.join(figures_directory,r'vel_%06d.png'%im_count)
        fig.savefig(figname,bbox_inches='tight')
        fig.canvas.draw()
        plt.pause(0.01)

        #FIGURE DISPLAY
        fig2 = plt.figure(2)
        plt.clf()
        plt.plot(part[:,POS[1]]/e,part[:,VEL[0]]/U0,'bo',label=r'$U_x$ SPH')
        plt.plot(part[:,POS[1]]/e,part[:,VEL[1]]/U0,'ro',label=r'$U_y$ SPH')
        plt.plot(ytab,Utab,'--k',label=r'Asymptotic')
        uan = analyticalPoiseuilleFlow(ytab,Re,t/tau)
        plt.plot(ytab,uan,'--r',label='Analytical')
        plt.xlabel('$z/e$',fontsize=18)
        plt.ylabel('$u/U_0$',fontsize=18)
        plt.xlim(-1-nBound*dr/e,1+nBound*dr/e)
        plt.ylim(-0.2,1.2)
        plt.legend(loc='upper left')
        plt.tight_layout()
        ax = plt.gca()
        ax.tick_params(axis = 'both', which = 'major', labelsize = 18)
        ax.xaxis.set_major_locator(plt.MaxNLocator(5))
        ax.yaxis.set_major_locator(plt.MaxNLocator(5))
        plt.tight_layout()
        plt.show(block=False)
        plt.draw()
        figname = os.path.join(figures_directory,r'UV_%06d.png'%im_count)
        fig2.savefig(figname,bbox_inches='tight')
        fig2.canvas.draw()
        plt.pause(0.01)

        #Figure
        fig3 = plt.figure(3)
        plt.clf()
        timeTabU = np.append(timeTabU,[[t*mu/rhoF/e**2,part[:,VEL[0]].max()/U0]],axis=0)
        plt.plot(timeTabU[:,0],timeTabU[:,1],'b.',label='SPH')
        plt.plot(timetab,Uan_with_time,'--r',label='Analytical')
        plt.xlim(0,3)
        plt.ylim(0,1.2)
        plt.xlabel(r'$t\nu/e^2$',fontsize=18)
        plt.ylabel(r'$u_{max}/U_0$',fontsize=18)
        plt.legend(loc='lower right')
        plt.tight_layout()
        ax = plt.gca()
        ax.tick_params(axis = 'both', which = 'major', labelsize = 18)
        ax.xaxis.set_major_locator(plt.MaxNLocator(5))
        ax.yaxis.set_major_locator(plt.MaxNLocator(5))
        plt.tight_layout()
        plt.show(block=False)
        plt.draw()
        figname = os.path.join(figures_directory,'VelMaxDT.png')
        fig3.savefig(figname,bbox_inches='tight')
        fig3.canvas.draw()
        plt.pause(0.01)

        im_count = im_count+1
        t_print +=dt_figure

```

Vous devriez voir que la simulation ne donne pas le résultat attendu. Ça vient du fait que les conditions au bord sont mal implémentées.

Nous allons corriger ce problème dans le projet fichier.
Ouvrez le dernier fichier [main_part2](main_part2.ipynb)

```python

```


```python
from sympy.physics.mechanics import dynamicsymbols, inertia
import sympy as sp, numpy as np
import sympy.physics.vector as vector
from sympy.algebras.quaternion import Quaternion
from sympy.utilities.lambdify import lambdastr
from sympy import symbols
import sys, os

from simupy import codegen
from simupy.matrices import construct_explicit_matrix

INCLUDE_NUTATION_PRECESSION = False # below is a prototype derivation of the equations of motion including nutation and precession but it is not currently supported.

def display_dict(d):
    for k,v in d.items():
        display(sp.Eq(k,v))
```

# Kinematics Problem setup

## Symbol Table

Symbols are defined in this cell with their definitions neary by in in-line comments or through explicit naming.

```python
t = dynamicsymbols._t # time symbol

omega_p = symbols('omega_p') # planet rotation angular velocity

if INCLUDE_NUTATION_PRECESSION:
    # pci2pcf_rotation_matrix = construct_explicit_matrix('pci2pcf', 3,3)
    pci2pcf_rotation_matrix = sp.MatrixSymbol('pci2pcf', 3,3)

lamda_D, phi_D, h_D = dynamicsymbols('lamda_D phi_D h_D')
# planetodetic longitude, planetodetic latitude, planetodetic altitude
# order selected to math erfa's gc2gd signature
# symbol choice to match NESC's "Fundamentals of Geodetic Kinematics"
# swapping _E (earth) to _D for generic planetoDetic

a, f = symbols('a f', real=True, nonnegative=True) # planet equitorial radius and flattening coefficient

# placeholder derivatives for local acceleration trimming
lamda_D_dot, phi_D_dot, h_D_dot = dynamicsymbols('lamdadot_D phidot_D hdot_D')

planetodetic_pos_derivs = {}
for orig, dotted in zip((lamda_D, phi_D, h_D,),
                        (lamda_D_dot, phi_D_dot)):
    planetodetic_pos_derivs[orig.diff(t)] = dotted

psi, theta, phi = dynamicsymbols('psi theta phi') # euler angles local to body : yaw pitch roll

V_N, V_E, V_D = V_NED = sp.Array(
    # relative velocity in local frame (NED) -- add winds to get aero
    dynamicsymbols('V_N V_E V_D')
)

W_N, W_E, W_D = W_NED = sp.Array(
    dynamicsymbols('W_N W_E W_D')
)

# inertial position, velocity, and acceleration of vehicle center of mass
# expressed in PCI frame
px, py, pz = inertial_pos_I = sp.Array(dynamicsymbols('p_x p_y p_z'))
vx, vy, vz = inertial_vel_I = sp.Array(dynamicsymbols('v_x v_y v_z'))
ax, ay, az = inertial_acc_I = sp.Array(dynamicsymbols('a_x a_y a_z'))

# planet fixed rectangular position coordinates
planet_fixed_position_x, planet_fixed_position_y, planet_fixed_position_z = planet_fixed_position = sp.Array(
    dynamicsymbols('ap_x ap_y ap_z')
)

planet_fixed_func_args = (t, lamda_D, phi_D, h_D)

# atmospheric properties
density, speed_of_sound, viscosity = dynamicsymbols('rho c_s mu')

# translational acceleration due to gravity
gx, gy, gz = g = sp.Array(dynamicsymbols('g_x g_y g_z'))

# non-unit quaternion component symbols to represent I to body B attitude
q = sp.Array(
    dynamicsymbols('q_0:4')
)
qmag = sp.sqrt(sp.tensorcontraction(sp.tensorproduct(q, q), (0,1))) # magnitude of non-unit quaternion
qq = Quaternion(*(q/qmag)) # q as unit-quaternion Quaternion object
cq = dynamicsymbols('c_q') # scalar control variable for derivative of q. See Rucker 2018

# angular velocity of vehicle body B w.r.t. inertial in body-fixed coordinates
omega_X, omega_Y, omega_Z = omega = I_ang_vel_B = sp.Array(dynamicsymbols('omega_X omega_Y omega_Z'))

# angular acceleration of vehicle body B w.r.t. inertial in body-fixed coordinates
alpha_X, alpha_Y, alpha_Z = I_ang_acc_B = sp.Array(dynamicsymbols('alpha_X alpha_Y alpha_Z'))

# inertial translational acceleration EXCLUDING GRAVITY of vehicle expressed in body-fixed coordinates
# intended to capture vehicle dynamics based on vehicle configuration (aero, prop, other control effectors, etc)
# input to kinematics block
AX, AY, AZ = inertial_acc_B = sp.Array(dynamicsymbols('A_X A_Y A_Z'))

airspeed_in_B = sp.Array(dynamicsymbols('u_B v_B w_B')) # true airspeed components in body-fixed coordinates
V_T, alpha, beta = sp.Array(dynamicsymbols('V_T alpha beta')) # true airspeed magnitude, angles of attack and sideslip
NED_ang_vel_B = sp.Array(dynamicsymbols('p_B q_B r_B')) # components of angular velocity of the body in air in body-fixed coordinate system

kinematics_state = sp.Array((*inertial_pos_I, *q, *inertial_vel_I, *I_ang_vel_B))
kinematics_input = sp.Array((*inertial_acc_B, *I_ang_acc_B, cq))

kinematics_output = sp.Array((
   *kinematics_state,

   # these are guidance and control feedback outputs, essentially
   lamda_D, phi_D, h_D, # planet-fixed planetodetic spherical position coordinates -- follow logic of planet_fixed_position -- can be used for navigation/guidance problems
   psi, theta, phi, # NED-to-body yaw pitch roll, if possible

   # what is needed for aerodynamics
   density, speed_of_sound, viscosity, # speed of sound, viscosity, and true airspeed give the needed atmospheric conditions
   V_T, alpha, beta, # true air speed, angles of attack and sideslip
   *NED_ang_vel_B, # components of angular velocity of the body in air in body-fixed coordinate system for computing aerodynamic damping

   *V_NED, # planet-fixed velocities in local frame
   *W_NED, # local wind
))

```

## Symbolic function placeholders

These symbols represent numerical functions to assign particular variables in the codegeneration. This approach is used to allow for parameterized models and to take advantage of numerically efficient algorithms when appropriate, such as for the planetodetic problem.

```python
kinematics_function_modules = {} # defines the namespace of the functions used in the Planet

# symbolic placeholder for planet centered fixed to planetodetic coordinates and back
if INCLUDE_NUTATION_PRECESSION:
    pci2pcf_rotation_matrix_function = symbols('pci2pcf', cls=sp.Function)(t)
    pci2pcf_rotation_matrix_function.shape = (3,3)
    kinematics_function_modules[pci2pcf_rotation_matrix_function] = 'self'

    pcf2pd = symbols('planetodetics.pcf2pd', cls=sp.Function)(*planet_fixed_position)
else:
    pcf2pd = symbols('planetodetics.pcf2pd', cls=sp.Function)(*inertial_pos_I)

pcf2pd.shape = (3,)

pd2pcf = symbols('planetodetics.pd2pcf', cls=sp.Function)(lamda_D, phi_D, h_D)
pd2pcf.shape = (3,)

kinematics_function_modules[pcf2pd] = 'self'

# function for acceleration due to gravity
if not INCLUDE_NUTATION_PRECESSION:
    gravity_func = sp.symbols('gravity', cls=sp.Function)(*inertial_pos_I)
else:
    gravity_func = sp.symbols('gravity', cls=sp.Function)(*planet_fixed_position)
gravity_func.shape = (3,)
kinematics_function_modules[gravity_func] = 'self'

# function for wind model
winds_func = sp.symbols('winds', cls=sp.Function)(*planet_fixed_func_args)
winds_func.shape = (3,)
kinematics_function_modules[winds_func] = 'self'

# function for atmospheric model
atmosphere_func = symbols('atmosphere', cls=sp.Function)(*planet_fixed_func_args)
atmosphere_func.shape = (3,)
kinematics_function_modules[atmosphere_func] = 'self'

```

## Kinematic Relationship

Construct frames of reference, points, and kinematic relationships similar to Kane's method.

```python
# planet centered Inertial
I = vector.frame.ReferenceFrame('I')

# Planet-fixed reference frame
if INCLUDE_NUTATION_PRECESSION:
    P = I.orientnew('P', 'DCM', sp.Matrix(pci2pcf_rotation_matrix), '321') # planet centered, planet fixed
    P.set_ang_vel(I, omega_p*P.z)
else:
    P = I.orientnew('P', 'Body', [omega_p*t, 0, 0], '321') # planet centered, planet fixed

# Local NED frame
NED = P.orientnew('NED', 'Body', [lamda_D, -phi_D-sp.pi/2, 0], '321') # local/NED on planet surface

# Define planet center as stationary
I0 = vector.Point('I_0') # planet center

# reference frame for vehicle body B
B = I.orientnew('B', 'quaternion', qq.args)

B.set_ang_vel(I, omega_X*B.x + omega_Y*B.y + omega_Z*B.z)

B_cm = vector.Point('B_cm') # center of mass of vehicle body B

I0.set_vel(I, 0)
I0.set_acc(I, 0)
I0.set_vel(P, 0)
I0.set_acc(P, 0)

B_cm.set_pos(I0, px*I.x + py*I.y + pz*I.z)

B_cm.set_vel(I, vx*I.x + vy*I.y + vz*I.z)
B_cm.set_vel(B, 0)
B_cm.set_acc(I, ax*I.x + ay*I.y + az*I.z)
B_cm.set_acc(B, 0)

# components of acceleration
translation_acceleration_due_to_gravity = gx*I.x + gy*I.y + gz*I.z # from the gravitational model
translation_acceleration_due_to_body = AX*B.x + AY*B.y + AZ*B.z # from the dynamics model

```

# Derive Equations of Motion

## Inertial translational position derivative

Inertial position derivative is a simple derivative in the inertial frame.

```python
inertial_pos_I_deriv_dict = sp.solve(inertial_pos_I.diff(t) - inertial_vel_I, inertial_pos_I.diff(t))
display_dict(inertial_pos_I_deriv_dict)
```

$\displaystyle \frac{d}{d t} \operatorname{p_{x}}{\left(t \right)} = \operatorname{v_{x}}{\left(t \right)}$

$\displaystyle \frac{d}{d t} \operatorname{p_{y}}{\left(t \right)} = \operatorname{v_{y}}{\left(t \right)}$

$\displaystyle \frac{d}{d t} \operatorname{p_{z}}{\left(t \right)} = \operatorname{v_{z}}{\left(t \right)}$

## Quaternion derivative

The derivative for non-unit quaternion comes from [TODO: ADD CITATION]. It is straightforward from unit quaternion derivatives after a normalization. This expression includes a numerical control term $c_q$.

```python
# quaternion kinematics
def Omega(ang_vel):
    return sp.Matrix(
        [
            [0, -ang_vel[0], -ang_vel[1], -ang_vel[2]],
            [ang_vel[0], 0, ang_vel[2], -ang_vel[1]],
            [ang_vel[1], -ang_vel[2], 0, ang_vel[0]],
            [ang_vel[2], ang_vel[1], -ang_vel[0], 0]
        ]
    )

qvec = sp.Matrix(q)
qdot_vec_expr = Omega(I_ang_vel_B)@qvec/2 + cq*qvec
qdotvec = sp.Matrix(q.diff(t))
q_deriv_dict = sp.solve(qdot_vec_expr - qdotvec, qdotvec)

display_dict(q_deriv_dict)
```

$\displaystyle \frac{d}{d t} \operatorname{q_{0}}{\left(t \right)} = \operatorname{c_{q}}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} - \frac{\omega_{X}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)}}{2} - \frac{\omega_{Y}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)}}{2} - \frac{\omega_{Z}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)}}{2}$

$\displaystyle \frac{d}{d t} \operatorname{q_{1}}{\left(t \right)} = \operatorname{c_{q}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} + \frac{\omega_{X}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)}}{2} - \frac{\omega_{Y}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)}}{2} + \frac{\omega_{Z}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)}}{2}$

$\displaystyle \frac{d}{d t} \operatorname{q_{2}}{\left(t \right)} = \operatorname{c_{q}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} + \frac{\omega_{X}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)}}{2} + \frac{\omega_{Y}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)}}{2} - \frac{\omega_{Z}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)}}{2}$

$\displaystyle \frac{d}{d t} \operatorname{q_{3}}{\left(t \right)} = \operatorname{c_{q}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} - \frac{\omega_{X}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)}}{2} + \frac{\omega_{Y}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)}}{2} + \frac{\omega_{Z}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)}}{2}$

## Inertial translational velocity derivative

We assume that the `Vehicle` outputs the translational acceleration due non-gravitational forces and the gravitaty model incorporates the translational acceleration due to gravity.

```python
translational_acceleration_in_I_vec = translation_acceleration_due_to_gravity + translation_acceleration_due_to_body
translational_acceleration_in_I_expr = translational_acceleration_in_I_vec.to_matrix(I).simplify()
inertial_vel_I_deriv_dict = sp.solve(translational_acceleration_in_I_expr - sp.Matrix(inertial_vel_I.diff(t)), inertial_vel_I.diff(t))

display_dict(inertial_vel_I_deriv_dict)
# TODO: simplify the gravity components to reduce number of multiply and add
```

$\displaystyle \frac{d}{d t} \operatorname{v_{x}}{\left(t \right)} = \frac{\operatorname{A_{X}}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{A_{X}}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} - \operatorname{A_{X}}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} - \operatorname{A_{X}}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} - 2 \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + 2 \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} + 2 \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} + 2 \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + \operatorname{g_{x}}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{g_{x}}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{g_{x}}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{g_{x}}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)}}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}$

$\displaystyle \frac{d}{d t} \operatorname{v_{y}}{\left(t \right)} = \frac{2 \operatorname{A_{X}}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + 2 \operatorname{A_{X}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} + \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} - \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} - \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} - 2 \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} + 2 \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + \operatorname{g_{y}}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{g_{y}}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{g_{y}}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{g_{y}}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)}}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}$

$\displaystyle \frac{d}{d t} \operatorname{v_{z}}{\left(t \right)} = \frac{- 2 \operatorname{A_{X}}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} + 2 \operatorname{A_{X}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + 2 \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} + 2 \operatorname{A_{Y}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} - \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} - \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{A_{Z}}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} + \operatorname{g_{z}}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{g_{z}}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{g_{z}}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{g_{z}}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)}}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}$

## Inertial angular velocity derivative

The derivative of the angular velocity of the vehicle body relative to the inertial frame expressed in body-fixed coordinates is a simple time-derivative in the inertial frame.

```python
I_ang_vel_B_deriv_dict = sp.solve(I_ang_vel_B.diff(t) - I_ang_acc_B, I_ang_vel_B.diff(t))
display_dict(I_ang_vel_B_deriv_dict)
```

$\displaystyle \frac{d}{d t} \omega_{X}{\left(t \right)} = \alpha_{X}{\left(t \right)}$

$\displaystyle \frac{d}{d t} \omega_{Y}{\left(t \right)} = \alpha_{Y}{\left(t \right)}$

$\displaystyle \frac{d}{d t} \omega_{Z}{\left(t \right)} = \alpha_{Z}{\left(t \right)}$

## Combine and prepare for codegen

We combine the derivative dictionaries into the `kinematics_state_equation_dict` to provide expressions for the state derivative function to return (the "expression dictionary" for the function to be implemented by codegen). We leverage the fact that Python 3.6+ has ordered dictionaries to the "extra-assignments" dictionary `kinematics_state_extra_dict` for assigning values prior to CSE.

```python
kinematics_state_equation_dict = {}
for deriv_dict in inertial_pos_I_deriv_dict, inertial_vel_I_deriv_dict, q_deriv_dict, I_ang_vel_B_deriv_dict:
    kinematics_state_equation_dict.update(deriv_dict)

kinematics_state_extra_dict = {}
# If using nutation/precession, would need to compute planet fixed rectangular position before calling gravity
kinematics_state_extra_dict[g] = gravity_func

```

# Kinematic Outputs

Next we construct the additional outputs that are used in the `Vehicle` models for common flight vehicles. The `kinematics_output_dict` is expression dictionary and `kinematics_output_equation_extra_assinments_dict` is the extra-assignments dictionary.

## Initialize with planetodetics, gravity, and winds

```python
kinematics_output_dict = {}
kinematics_output_equation_extra_assinments_dict = {}

# output all of the kinematic state veriables
for state_var in kinematics_state:
    kinematics_output_dict[state_var] = state_var

if INCLUDE_NUTATION_PRECESSION:
    # compute the rotation matrix and planet-fixed position in rectangular coordinates
    kinematics_output_equation_extra_assinments_dict[pci2pcf_rotation_matrix] = pci2pcf_rotation_matrix_function
    kinematics_output_equation_extra_assinments_dict[planet_fixed_position] = pci2pcf_rotation_matrix_function * inertial_pos_I

# use the planetodesy model to compute longitude, latitude, and altitude
kinematics_output_equation_extra_assinments_dict[sp.Array((lamda_D, phi_D, h_D))] = pcf2pd

if not INCLUDE_NUTATION_PRECESSION:
    # without nutation and precession, the side-reel time is accounted for by shifting the latitude
    kinematics_output_equation_extra_assinments_dict[lamda_D] = lamda_D - omega_p*t

kinematics_output_dict[lamda_D] = lamda_D
kinematics_output_dict[phi_D] = phi_D
kinematics_output_dict[h_D] = h_D

# wind model assumed to depend on the planetodetic position
kinematics_output_equation_extra_assinments_dict[W_NED] = winds_func

```

## Calculate local-to-body euler angles

Here we compute the euler angles roll ($\phi$), pitch ($\theta$), and yaw ($\phi$). We construct the local NED to body rotation matrix, then determine the euler angles following "Review of Attitude Representations Used for Aircraft Kinematics" by Phillips et. al., 2001.

```python
NED_rotation_B = NED.dcm(I) @ I.dcm(B) # equivalent to NED.dcm(B),
NED_rotation_B_simplified = NED_rotation_B.simplify()

# Phillips eq. 1 and 25 defines Rotatation matrix from inertia to body
# NED_rotation_B is body to NED
# this follows Eq. 53 from Phillips.

kinematics_output_dict[psi] = sp.atan2(NED_rotation_B_simplified[1,0], NED_rotation_B_simplified[0,0])
kinematics_output_dict[theta] = sp.asin(-NED_rotation_B_simplified[2,0])
kinematics_output_dict[phi] = sp.atan2(NED_rotation_B_simplified[2,1], NED_rotation_B_simplified[2,2])

```

## Atmosphere models

```python
kinematics_output_equation_extra_assinments_dict[sp.Array([density, speed_of_sound, viscosity,])] = atmosphere_func

kinematics_output_dict[density] = density
kinematics_output_dict[speed_of_sound] = speed_of_sound
kinematics_output_dict[viscosity] = viscosity
```

## Computing elements for aerodynamics

Here, we calculate the wind experienced on the body by determining the velocity of the of the vehicle's center of mass in the planet-fixed frame $P$, incorporating the winds, and expressing the computed wind in body-fixed coordinates.

```python
B_cm.v1pt_theory(I0, P, I) # calculate the velocity of the vehicle's center of mass in the planet-fixed frame
local_wind = W_N*NED.x + W_E*NED.y + W_D*NED.z
body_wind = B_cm.vel(P) - local_wind # incorporate the wind model
body_wind_I = body_wind.to_matrix(I).simplify() # express in inertial coordinates

body_wind_B = B.dcm(I).simplify()@body_wind_I # express the winds in body-fixed coordinates
body_wind_B_simplified = body_wind_B.simplify() # simplify

body_wind_B_simplified
```

$\displaystyle \left[\begin{matrix}\frac{2 \left(- \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} + \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)}\right) \left(\operatorname{W_{D}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{W_{N}}{\left(t \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{v_{z}}{\left(t \right)}\right) + 2 \left(\operatorname{q_{0}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)}\right) \left(- \omega_{p} \operatorname{p_{x}}{\left(t \right)} + \operatorname{W_{D}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{W_{E}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{W_{N}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{v_{y}}{\left(t \right)}\right) + \left(\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} - \operatorname{q_{2}}^{2}{\left(t \right)} - \operatorname{q_{3}}^{2}{\left(t \right)}\right) \left(\omega_{p} \operatorname{p_{y}}{\left(t \right)} + \operatorname{W_{D}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{W_{E}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{W_{N}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{v_{x}}{\left(t \right)}\right)}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}\\\frac{2 \left(\operatorname{q_{0}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} + \operatorname{q_{2}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)}\right) \left(\operatorname{W_{D}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{W_{N}}{\left(t \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{v_{z}}{\left(t \right)}\right) + 2 \left(- \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)}\right) \left(\omega_{p} \operatorname{p_{y}}{\left(t \right)} + \operatorname{W_{D}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{W_{E}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{W_{N}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{v_{x}}{\left(t \right)}\right) + \left(\operatorname{q_{0}}^{2}{\left(t \right)} - \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} - \operatorname{q_{3}}^{2}{\left(t \right)}\right) \left(- \omega_{p} \operatorname{p_{x}}{\left(t \right)} + \operatorname{W_{D}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{W_{E}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{W_{N}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{v_{y}}{\left(t \right)}\right)}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}\\\frac{2 \left(- \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} + \operatorname{q_{2}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)}\right) \left(- \omega_{p} \operatorname{p_{x}}{\left(t \right)} + \operatorname{W_{D}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{W_{E}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{W_{N}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{v_{y}}{\left(t \right)}\right) + 2 \left(\operatorname{q_{0}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} + \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)}\right) \left(\omega_{p} \operatorname{p_{y}}{\left(t \right)} + \operatorname{W_{D}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{W_{E}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{W_{N}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{v_{x}}{\left(t \right)}\right) + \left(\operatorname{W_{D}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{W_{N}}{\left(t \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{v_{z}}{\left(t \right)}\right) \left(\operatorname{q_{0}}^{2}{\left(t \right)} - \operatorname{q_{1}}^{2}{\left(t \right)} - \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}\right)}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}\end{matrix}\right]$

Next we compute the total true wind $V_T$ (the magnitude of the wind calculated above) and the angles of attack and sideslip.

```python
V_T_simp = (body_wind.magnitude()**2).simplify()
kinematics_output_dict[V_T] = sp.sqrt(V_T_simp*sp.Heaviside(V_T_simp))
kinematics_output_dict[alpha] = sp.atan2(body_wind_B_simplified[2], body_wind_B_simplified[0])
kinematics_output_dict[beta] = sp.asin(
    sp.Piecewise( # This piecewise expression is used to provide the correct output at singularities
        (1., (body_wind_B_simplified[1]/kinematics_output_dict[V_T])>1.0),
        (-1., (body_wind_B_simplified[1]/kinematics_output_dict[V_T])<-1.0),
        (body_wind_B_simplified[1]/kinematics_output_dict[V_T], kinematics_output_dict[V_T]>0.),
        (0., True)
    )
)
```

## Translational velocity in local NED frame

Here, we calculate the local velocity (in the planet centered planet fixed frame $P$)

```python
B_cm.v1pt_theory(I0, P, I)
V_NED_expr = B_cm.vel(P).to_matrix(NED)
V_NED_expr_dict = sp.solve(V_NED_expr - sp.Matrix(V_NED), V_NED)

V_NED_cse = sp.cse(V_NED_expr, symbols=sp.numbered_symbols(prefix='V_NED_'), order='none')
V_NED_cse_assignments = {k: v for k,v in V_NED_cse[0]}
V_NED_cse_assignments.update({k: v for k, v in zip(V_NED, V_NED_cse[1][0])})
kinematics_output_equation_extra_assinments_dict.update(V_NED_cse_assignments)

display_dict(V_NED_expr_dict)
```

$\displaystyle \operatorname{V_{N}}{\left(t \right)} = \omega_{p} \operatorname{p_{x}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} - \omega_{p} \operatorname{p_{y}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - \operatorname{v_{x}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - \operatorname{v_{y}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)} + \operatorname{v_{z}}{\left(t \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)}$

$\displaystyle \operatorname{V_{E}}{\left(t \right)} = - \omega_{p} \operatorname{p_{x}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - \omega_{p} \operatorname{p_{y}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - \operatorname{v_{x}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \operatorname{v_{y}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)}$

$\displaystyle \operatorname{V_{D}}{\left(t \right)} = \omega_{p} \operatorname{p_{x}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} - \omega_{p} \operatorname{p_{y}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{v_{x}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{v_{y}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} \cos{\left(\phi_{D}{\left(t \right)} \right)} - \operatorname{v_{z}}{\left(t \right)} \sin{\left(\phi_{D}{\left(t \right)} \right)}$

## Angular velocity in local NED frame

Here we calculate the angular velocity components of the vehicle in air, used for the dynamic (or damping) aerodynamic moment derivatives.

```python
e_squared = 1-(1-f)**2 # eccentricity squared
# radius in prime meridian
rho_M_expr = a*(1-e_squared)/sp.sqrt(1-e_squared*sp.sin(phi_D)**2)**(3)
# radius in prime vertical
rho_V_expr = a/sp.sqrt(1-e_squared*sp.sin(phi_D)**2)

navigation_kinematics = {
    phi_D_dot: sp.Piecewise((V_N /(rho_M_expr + h_D), a>0), (0., True)),
    lamda_D_dot:  sp.Piecewise((V_E /((rho_V_expr + h_D)*sp.cos(phi_D)), a>0), (0., True)),
    h_D_dot: -V_D
}

display_dict(navigation_kinematics)

navigation_cse = sp.cse(sp.Matrix([navigation_kinematics[key] for key in navigation_kinematics]), symbols=sp.numbered_symbols(prefix='navigation_'), order='none')

navigation_cse_assignments = {k: v for k,v in navigation_cse[0]}
navigation_cse_assignments.update({k: v for k, v in zip(navigation_kinematics.keys(), navigation_cse[1][0])})
kinematics_output_equation_extra_assinments_dict.update(navigation_cse_assignments)

```

$\displaystyle \dot{\phi}_{D}{\left(t \right)} = \begin{cases} \frac{\operatorname{V_{N}}{\left(t \right)}}{\frac{a \left(1 - f\right)^{2}}{\left(- \left(1 - \left(1 - f\right)^{2}\right) \sin^{2}{\left(\phi_{D}{\left(t \right)} \right)} + 1\right)^{\frac{3}{2}}} + \operatorname{h_{D}}{\left(t \right)}} & \text{for}\: a > 0 \\0.0 & \text{otherwise} \end{cases}$

$\displaystyle \dot{\lambda}_{D}{\left(t \right)} = \begin{cases} \frac{\operatorname{V_{E}}{\left(t \right)}}{\left(\frac{a}{\sqrt{- \left(1 - \left(1 - f\right)^{2}\right) \sin^{2}{\left(\phi_{D}{\left(t \right)} \right)} + 1}} + \operatorname{h_{D}}{\left(t \right)}\right) \cos{\left(\phi_{D}{\left(t \right)} \right)}} & \text{for}\: a > 0 \\0.0 & \text{otherwise} \end{cases}$

$\displaystyle \dot{h}_{D}{\left(t \right)} = - \operatorname{V_{D}}{\left(t \right)}$

```python
NED_ang_vel_B_expr_dict = sp.solve(B.ang_vel_in(NED).to_matrix(B) - sp.Matrix(NED_ang_vel_B), NED_ang_vel_B)
NED_ang_vel_B_expr_simplified_dict = {}
for k,v in NED_ang_vel_B_expr_dict.items():
    NED_ang_vel_B_expr_simplified_dict[k] = v.subs(planetodetic_pos_derivs)
kinematics_output_dict.update(NED_ang_vel_B_expr_simplified_dict)
display_dict(NED_ang_vel_B_expr_simplified_dict)
```

$\displaystyle \operatorname{p_{B}}{\left(t \right)} = \frac{2 \omega_{p} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} - 2 \omega_{p} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + 2 \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} - 2 \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + \omega_{X}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \omega_{X}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \omega_{X}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} + \omega_{X}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} - \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)}}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}$

$\displaystyle \operatorname{q_{B}}{\left(t \right)} = \frac{- 2 \omega_{p} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} - 2 \omega_{p} \operatorname{q_{2}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} - 2 \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} - 2 \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} + \omega_{Y}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \omega_{Y}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \omega_{Y}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} + \omega_{Y}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} + \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)}}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}$

$\displaystyle \operatorname{r_{B}}{\left(t \right)} = \frac{- \omega_{p} \operatorname{q_{0}}^{2}{\left(t \right)} + \omega_{p} \operatorname{q_{1}}^{2}{\left(t \right)} + \omega_{p} \operatorname{q_{2}}^{2}{\left(t \right)} - \omega_{p} \operatorname{q_{3}}^{2}{\left(t \right)} - \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} - \dot{\lambda}_{D}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} + \omega_{Z}{\left(t \right)} \operatorname{q_{0}}^{2}{\left(t \right)} + \omega_{Z}{\left(t \right)} \operatorname{q_{1}}^{2}{\left(t \right)} + \omega_{Z}{\left(t \right)} \operatorname{q_{2}}^{2}{\left(t \right)} + \omega_{Z}{\left(t \right)} \operatorname{q_{3}}^{2}{\left(t \right)} - 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{0}}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} - 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{1}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} \sin{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)} + 2 \dot{\phi}_{D}{\left(t \right)} \operatorname{q_{2}}{\left(t \right)} \operatorname{q_{3}}{\left(t \right)} \cos{\left(\omega_{p} t + \lambda_{D}{\left(t \right)} \right)}}{\operatorname{q_{0}}^{2}{\left(t \right)} + \operatorname{q_{1}}^{2}{\left(t \right)} + \operatorname{q_{2}}^{2}{\left(t \right)} + \operatorname{q_{3}}^{2}{\left(t \right)}}$

Here we assign the local velocity and wind components in the local NED frame in the specified order.

```python
for vel_var in V_NED:
    kinematics_output_dict[vel_var] = vel_var

# add the wind model outputs at the end to match the specified order
for wind_var in W_NED:
    kinematics_output_dict[wind_var] = wind_var
```

# Computing inertial initial conditions

Here we write a helper function that can compute the inertial initial conditions

```python
B2 = NED.orientnew('B2', 'Body', [psi, theta, phi], '321')
P_rotation_B = P.dcm(B2).simplify() # should be equivalent to LaRC slides [T^{BP}] at t=0, and taking into account PCI --> NED frames
# LaRC provides a formula that seems to be the special case of Phillips 2001 for q0**2  > q1**2, q2**2, q3**2, with some sign flips
# I'll stick with LaRC slides, but we should be aware of it based on

half_root_trace_plus_1 = sp.sqrt(P_rotation_B.trace()+1)/2
quaternion_from_spec = sp.Array([
    half_root_trace_plus_1,
    (P_rotation_B[2,1] - P_rotation_B[1,2])/(4*half_root_trace_plus_1),
    (P_rotation_B[0,2] - P_rotation_B[2,0])/(4*half_root_trace_plus_1),
    (P_rotation_B[1,0] - P_rotation_B[0,1])/(4*half_root_trace_plus_1),

])
quaternion_from_euler_dict = sp.solve(quaternion_from_spec- q, q)

inertial_trans_vel_from_relative_vel_NED_dict = sp.solve(B_cm.vel(P).to_matrix(NED) - sp.Matrix(V_NED), inertial_vel_I)

inertial_ang_vel_from_local = sp.solve(B.ang_vel_in(NED).to_matrix(B).subs(planetodetic_pos_derivs) - sp.Matrix(NED_ang_vel_B), I_ang_vel_B)

ic_from_pd_dict = {}
ic_from_pd_dict.update(inertial_trans_vel_from_relative_vel_NED_dict)
ic_from_pd_dict.update(quaternion_from_euler_dict)
ic_from_pd_dict.update(inertial_ang_vel_from_local)
ic_from_pd = kinematics_state.subs(ic_from_pd_dict).subs(omega_p*t, 0)

sp.physics.mechanics.functions.find_dynamicsymbols(quaternion_from_spec)
ic_extra = {}
ic_extra[inertial_pos_I] = pd2pcf
ic_extra.update(navigation_cse_assignments)
ic_from_pd_args = (lamda_D, phi_D, h_D, V_N, V_E, V_D, psi, theta, phi, *NED_ang_vel_B)

```

# Compute local trim residual

In order to trim the vehicle, we must calculate the acceleration of the vehicle's center of mass in terms of the local acceleration $\dot{V}_N$, $\dot{V}_E$, $\dot{V}_D$. The vehicle is trim whent he local acceleration is zero.

```python
V_NED_vec = np.sum(np.array(V_NED)*np.array([NED.x, NED.y, NED.z]))
V_NED_dot_vec = np.sum(np.array(V_NED.diff(t))*np.array([NED.x, NED.y, NED.z]))

B_cm.set_vel(P, V_NED_vec)
V_NED_dot_residual = sp.Array(
    (translational_acceleration_in_I_vec - B_cm.a1pt_theory(I0, I, P)).simplify().to_matrix(NED).subs(planetodetic_pos_derivs) + V_NED_dot_vec.to_matrix(NED)
).reshape(3).subs(omega_p*t, 0)
```

```python
trim_residual_extras = {}
trim_residual_extras[sp.Array((lamda_D, phi_D, h_D))] = pcf2pd
trim_residual_extras[g] = gravity_func

V_NED_cse_no_time = sp.cse(V_NED_expr.subs(omega_p*t, 0), symbols=sp.numbered_symbols(prefix='V_NED_'), order='none')
trim_residual_extras.update({k: v for k,v in V_NED_cse_no_time[0]})
trim_residual_extras.update({k: v for k, v in zip(V_NED, V_NED_cse_no_time[1][0])})
trim_residual_extras.update(navigation_cse_assignments)

```

# Kinematic codegen

```python
transformation_func_to_print = [
    dict(num_name='inertial_to_body_dcm', input_args=q,
        sym_expr=sp.Array(B.dcm(I).simplify()),
        extra_assignments={}, ),

    dict(num_name='body_to_NED_dcm', input_args=sp.Array([phi, theta, psi]),
        sym_expr=sp.Array(B2.dcm(NED).simplify()),
        extra_assignments={}, ),

]

transformation_printer = codegen.ModuleNumPyPrinter(
    for_class=False,
)

transformation_evaluator = codegen.ModulePrinter(transformation_printer)

transformation_print_params = codegen.process_funcs_to_print(transformation_func_to_print)

transformation_code = '\n'.join(transformation_evaluator.codeprint(transformation_print_params).split('\n')[2:])
```

```python
kin_funcs_to_print = [
    dict(num_name='kinematics_state_function', input_args=sp.Array([t, *kinematics_state, *kinematics_input]),
        sym_expr=kinematics_state.diff(t).subs(kinematics_state_equation_dict),
        extra_assignments=kinematics_state_extra_dict ),

    dict(num_name='ic_from_planetodetic', input_args=sp.Array([*ic_from_pd_args]),
        sym_expr=ic_from_pd,
        extra_assignments=ic_extra, ),

    dict(num_name='kinematics_output_function', input_args=sp.Array([t, *kinematics_state]),
        sym_expr=kinematics_output.subs(kinematics_output_dict),
        extra_assignments=kinematics_output_equation_extra_assinments_dict, ),

    dict(num_name='inertial_to_NED_dcm', input_args=sp.Array([t, lamda_D, phi_D]),
        sym_expr=sp.Array(NED.dcm(I).simplify()),
        extra_assignments={}, ),

    dict(num_name='local_translational_trim_residual', input_args=sp.Array([*kinematics_state[:-3], *kinematics_input[:3]]),
        sym_expr=V_NED_dot_residual,
        extra_assignments=trim_residual_extras, ),

]

static_kinematics_function_modules = {}
for key in kinematics_function_modules:
    static_kinematics_function_modules[key.func] = kinematics_function_modules[key]

constant_modules = {omega_p: 'self', a: 'self', f: 'self'}
constant_names= {omega_p: 'planetodetics.'+str(omega_p), a: 'planetodetics.'+str(a), f: 'planetodetics.'+str(f)}

kin_code_print_params = codegen.process_funcs_to_print(kin_funcs_to_print)
kin_printer = codegen.ModuleNumPyPrinter(
    for_class=True,
    function_modules=static_kinematics_function_modules,# function_names=func_names,
    constant_modules=constant_modules,
    constant_names = constant_names,
)
kin_evaluator = codegen.ModulePrinter(kin_printer)
kin_code = kin_evaluator.codeprint(
    (kin_code_print_params)
)
with open('kinematics.py', "w") as open_file:
    open_file.write(kin_code)
    open_file.write(transformation_code)
```

# Dynamics Modeling

Much of the effort around the `Vehicle` design is to provide sufficient API "hooks" to facilitate modeling of different vehicles without re-computing (or even re-`connect`ing, in the context of SimuPy) a cumbersome number of variables. To that end, we provide hooks for extra forces and moments directly as well as extra aerodynamic coefficients. Since the mathematical modeling is simpler but with several decompositions for the purposes of API, this section has fewer distinct sections and more and more explicitly named variables.

## Constants, Atmospheric flight condition, and other setup

Here we define the constants used in the inertia and aerodynamic models. We also label and define atmospheric quantities used for the aerodynamics modeling. The dynamic pressure $\bar{q}$ and Mach number $M$ could be calculated in the Kinematics model, however since Reynolds number $R_e$ also depends on the density and a vehicle-specific parameter, I kept these functions of the flight condition as part of the Dynamics model.

The dynamics model expression dictionary is `dynamics_output_dict` and the extra-assignments dictionary is `dynamics_output_extra_dict`.

```python
dynamics_output_dict = {}
dynamics_output_extra_dict = {}

# Inertia symbols
m = symbols('m')
Ixx, Iyy, Izz, Ixy, Iyz, Ixz = inertia_symbols = \
    symbols('I_xx, I_yy, I_zz, I_xy, I_yz, I_xz')
com_x, com_y, com_z = com_pos = sp.Array(sp.symbols('x_com y_com z_com'))

# Aerodynamic parameters
mrc_x, mrc_y, mrc_z = mrc_pos = sp.Array(sp.symbols('x_mrc y_mrc z_mrc'))
S_A, a_l, b_l, c_l, d_l = sp.symbols('S_A a_l b_l c_l d_l') # reference area; reference directional, lateral, longitudinal lengths; Reynolds number reference length
ref_lengths = np.array([a_l, c_l, b_l])

inertia_constants = (m,
                   Ixx, Iyy, Izz, Ixy, Iyz, Ixz,
                   com_x, com_y, com_z)
aerodynamics_constants =  (mrc_x, mrc_y, mrc_z,
                   S_A, a_l, b_l, c_l, d_l)
dynamics_constants = (*inertia_constants, *aerodynamics_constants)

# atmospheric parameters
qbar, mach, reynolds = sp.symbols('qbar Ma Re')

qbar_expr = V_T**2*density/2
mach_expr = V_T/speed_of_sound
Re_expr = V_T*d_l*density/viscosity

dynamics_output_extra_dict[qbar] = qbar_expr
dynamics_output_extra_dict[mach] = mach_expr
dynamics_output_extra_dict[reynolds] = Re_expr

starargs = symbols('*args')

```

## Defining aerodynamic coefficients and their decompositions

```python
# Wind frame, freestream wind in -W.x direction -- potentially
W = vector.frame.ReferenceFrame('W')
# Stability frame, Sf.x and Sf.z in same plane as B.x, B.z
Sf = W.orientnew('Sf', 'Body', [-beta, 0, 0], '312')
Bw = Sf.orientnew('Bw', 'Body', [alpha, 0, 0], '231') # Body frame relative to wind

aero_forces = sp.Array(dynamicsymbols('F_ax F_ay F_az')) # aerodynamic forces in body-fixed coordinate system
Lcal, Mcal, Ncal = aero_moments = sp.Array(dynamicsymbols('Lcal Mcal Ncal')) # aerodynamic Roll, Pitch, Yaw moments (about body-fixed coordiante system)

# total aero coefficients
CD, CS, CL = total_aero_force_coeffs = sp.Array(dynamicsymbols('CD CS CL')) # static aero forces
CLcal, CMcal, CNcal = total_aero_moment_coeffs = sp.Array(dynamicsymbols('CLcal CMcal CNcal')) # static aero moments
# CLcal_s, CMcal_s, CNcal_s = transformed_static_aero_moment_coeffs = sp.Array(dynamicsymbols('CLcal_s CMcal_s CNcal_s')) # static aero moments
Cp, Cq, Cr = total_aero_damping_coeffs = sp.Array(dynamicsymbols('Cp Cq Cr')) # dynamic aero moments

# base aero coefficients
CDb, CSb, CLb = base_aero_force_coeffs =  np.array(dynamicsymbols('CD_b CS_b CL_b'))
CLcalb, CMcalb, CNcalb = base_aero_moment_coeffs = np.array(dynamicsymbols('CLcal_b CMcal_b CNcal_b'))
Cpb, Cqb, Crb = base_aero_damping_coeffs = np.array(dynamicsymbols('Cp_b Cq_b Cr_b'))

base_aero_coeffs = sp.Array([
    *base_aero_force_coeffs,
    *base_aero_moment_coeffs,
    *base_aero_damping_coeffs,
])

# extra aero coefficients -- inputs, can be used to model aerodynamic control surfaces or do configuration management on aero (i.e., add wings or tail to base vehicle)
# assumed to use same references areas/lengths as base, and moments are about same MRC
CDe, CSe, CLe = input_aero_force_coeffs = np.array(dynamicsymbols('CD_e CS_e CL_e'))
CLcale, CMcale, CNcale = input_aero_moment_coeffs = np.array(dynamicsymbols('CLcal_e CMcal_e CNcal_e'))
Cpe, Cqe, Cre = input_aero_damping_coeffs = np.array(dynamicsymbols('Cp_e Cq_e Cr_e'))

input_aero_coeffs = sp.Array([
    *input_aero_force_coeffs,
    *input_aero_moment_coeffs,
    *input_aero_damping_coeffs,
])

total_aero_force_coeffs = base_aero_force_coeffs + input_aero_force_coeffs
total_aero_damping_coeffs_mrc = base_aero_damping_coeffs + input_aero_damping_coeffs
total_static_aero_moment_coeffs_mrc = base_aero_moment_coeffs + input_aero_moment_coeffs
total_aero_moment_coeffs_mrc = total_static_aero_moment_coeffs_mrc + total_aero_damping_coeffs_mrc*ref_lengths*NED_ang_vel_B*sp.Piecewise((1/(2*V_T), V_T> 0.), (0., True))
```

## Total Aerodynamic Forces and Moments

For the aerodynamic forces, we must transform the wind-frame Lift, Drag, and Side-forces to body-fixed Normal, Axial, and Side-forces. Note that these two side-forces do not point in the same direction for non-zero sideslip.

For the aerodynamic moments, we assume the moment coefficients are about a moment reference frame (MRC) so the moment coefficients must be transformed to the center of mass position. Note that we apply the same transformation to the damping coefficients.

```python
body_unit_vecs = np.array([Bw.x, Bw.y, Bw.z])
wind_unit_vecs = np.array([W.x, W.y, W.z])

# force transformation
total_aero_force_coeffs_vec = np.sum(np.array([-1, 1, -1])*total_aero_force_coeffs*wind_unit_vecs)
total_aero_forces_expr = qbar*S_A*np.array(total_aero_force_coeffs_vec.to_matrix(Bw)).squeeze()

# moment transformation
total_aero_moment_coeffs_mrc_vec = np.sum(total_aero_moment_coeffs_mrc*body_unit_vecs)
mrc_to_com = np.array(com_pos - mrc_pos)
mrc_to_com_vec = np.sum(mrc_to_com*body_unit_vecs)
total_aero_moment_coeffs_com = np.array(total_aero_moment_coeffs_mrc - np.array((total_aero_force_coeffs_vec.cross(mrc_to_com_vec)).to_matrix(Bw)).squeeze()/ref_lengths)
total_aero_moments_com_expr = qbar*S_A*total_aero_moment_coeffs_com*ref_lengths

# in general, moment coefficient = static coefficient + ((dynamic coefficient * angular rate * ref length) / (2 * true airspeed))
# LaRC slides show an extra half factor on dynamic term, but NESC description doesn't have that... coefficients can be re-normalized
# so just need to clarify the definition

```

## Preparing for Codegen

```python
#
total_aero_forces_moments_syms = sp.Array([*aero_forces, *aero_moments])
total_aero_forces_moments_expr = sp.Array([*total_aero_forces_expr, *total_aero_moments_com_expr])

total_aero_forces_moments_func = sp.Function('tot_aero_forces_moments')(qbar, mach, reynolds, V_T, alpha, beta, *NED_ang_vel_B, starargs)
total_aero_forces_moments_func.shape = (6,)
dynamics_output_extra_dict[total_aero_forces_moments_syms] = total_aero_forces_moments_func
tot_aero_extra_dict = {}

base_aero_coeffs_func = sp.Function('base_aero_coeffs')(alpha, beta, mach, reynolds)
base_aero_coeffs_func.shape = (9,)
tot_aero_extra_dict[base_aero_coeffs] = base_aero_coeffs_func

input_aero_coeffs_func = sp.Function('_input_aero_coeffs')(alpha, beta, mach, reynolds, starargs)
input_aero_coeffs_func.shape = (9,)
tot_aero_extra_dict[input_aero_coeffs] = input_aero_coeffs_func

# input forces and torques/moments in body-fixed coordinates (moments about center of mass)
Phix, Phiy, Phiz = input_forces = sp.Array(dynamicsymbols('Phi_x Phi_y Phi_z'))
taux, tauy, tauz = input_moments = sp.Array(dynamicsymbols('tau_x tau_y tau_z'))
input_forces_moments = sp.Array([*input_forces, *input_moments])

input_forces_moments_func = sp.Function('_input_force_moment')(t, *kinematics_output, qbar, mach, reynolds, starargs)
input_forces_moments_func.shape = (6,)
dynamics_output_extra_dict[input_forces_moments] = input_forces_moments_func

# total forces and moments
Fx, Fy, Fz = tot_body_fixed_forces_var = sp.Array(dynamicsymbols('F_x F_y F_z')) # total body-fixed forces
Mx, My, Mz = tot_body_fixed_moments_var = sp.Array(dynamicsymbols('M_x M_y M_z')) # total body-fixed moments

tot_forces_moments_syms = sp.Array([*tot_body_fixed_forces_var, *tot_body_fixed_moments_var])
tot_forces_moments_expr = total_aero_forces_moments_syms + input_forces_moments
dynamics_output_extra_dict[tot_forces_moments_syms] = tot_forces_moments_expr

dynamics_input = sp.Array([
    t,
    *kinematics_output,
    starargs,
])

dynamics_output = sp.Array([*inertial_acc_B, *I_ang_acc_B])
# TODO: make the angular and translational accelerations independent hooked functions that take (Mx My Mz) and (Fx Fy Fz), respectively.
```

# Dynamics output

The translational acceleration of the vehicle center of mass and the angular acceleration of the vehicle body about the center of mass is computed using the Newton-Euler equation. For translational acceleration, we simply sum the forces projected in the body-fixed axes and divide by the (assumed constant) mass $m$ as shown below. The angular acceleration for a constant inertia $\mathbb{I}$ is given by

$$\left[\begin{matrix}\dot{\omega}_{x}\\
\dot{\omega}_{y}\\
\dot{\omega}_{z}
\end{matrix}\right]=\mathbb{I}^{-1}\left(\left[\begin{matrix}M_{x}\\
M_{y}\\
M_{z}
\end{matrix}\right]-\left[\begin{matrix}\omega_{x}\\
\omega_{y}\\
\omega_{z}
\end{matrix}\right]\times\mathbb{I}\cdot\left[\begin{matrix}\omega_{x}\\
\omega_{y}\\
\omega_{z}
\end{matrix}\right]\right)$$

Below, we see the operations of this vector-matrix notation carried out.

```python
inertial_acc_B_dict = sp.solve(m*inertial_acc_B - tot_body_fixed_forces_var, inertial_acc_B) # this assumes constant mass
dynamics_output_dict.update(inertial_acc_B_dict)
display_dict(inertial_acc_B_dict)
```

$\displaystyle \operatorname{A_{X}}{\left(t \right)} = \frac{\operatorname{F_{x}}{\left(t \right)}}{m}$

$\displaystyle \operatorname{A_{Y}}{\left(t \right)} = \frac{\operatorname{F_{y}}{\left(t \right)}}{m}$

$\displaystyle \operatorname{A_{Z}}{\left(t \right)} = \frac{\operatorname{F_{z}}{\left(t \right)}}{m}$

```python
II = inertia(Bw, Ixx, Iyy, Izz, -Ixy, -Iyz, -Ixz)
Bw_ang_vel_I = omega_X*Bw.x + omega_Y*Bw.y + omega_Z*Bw.z
Bw_ang_acc_I = alpha_X*Bw.x + alpha_Y*Bw.y + alpha_Z*Bw.z
inertial_ang_acc_B_dict = sp.solve(sp.Array((II.dot(Bw_ang_acc_I) + Bw_ang_vel_I.cross(II.dot(Bw_ang_vel_I))).to_matrix(Bw))[:, 0] - tot_body_fixed_moments_var, I_ang_acc_B)
dynamics_output_dict.update(inertial_ang_acc_B_dict)

display_dict(inertial_ang_acc_B_dict)
```

$\displaystyle \alpha_{X}{\left(t \right)} = - \frac{\left(I_{xy} I_{yz} + I_{xz} I_{yy}\right) \left(I_{xx} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{xy} \omega_{X}^{2}{\left(t \right)} - I_{xy} \omega_{Y}^{2}{\left(t \right)} - I_{xz} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} - I_{yy} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{yz} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{z}}{\left(t \right)}\right) + \left(I_{xy} I_{zz} + I_{xz} I_{yz}\right) \left(- I_{xx} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{xy} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} - I_{xz} \omega_{X}^{2}{\left(t \right)} + I_{xz} \omega_{Z}^{2}{\left(t \right)} - I_{yz} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{zz} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{y}}{\left(t \right)}\right) + \left(I_{yy} I_{zz} - I_{yz}^{2}\right) \left(- I_{xy} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{xz} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{yy} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{yz} \omega_{Y}^{2}{\left(t \right)} - I_{yz} \omega_{Z}^{2}{\left(t \right)} - I_{zz} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{x}}{\left(t \right)}\right)}{- I_{xx} I_{yy} I_{zz} + I_{xx} I_{yz}^{2} + I_{xy}^{2} I_{zz} + 2 I_{xy} I_{xz} I_{yz} + I_{xz}^{2} I_{yy}}$

$\displaystyle \alpha_{Y}{\left(t \right)} = - \frac{\left(I_{xx} I_{yz} + I_{xy} I_{xz}\right) \left(I_{xx} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{xy} \omega_{X}^{2}{\left(t \right)} - I_{xy} \omega_{Y}^{2}{\left(t \right)} - I_{xz} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} - I_{yy} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{yz} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{z}}{\left(t \right)}\right) + \left(I_{xx} I_{zz} - I_{xz}^{2}\right) \left(- I_{xx} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{xy} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} - I_{xz} \omega_{X}^{2}{\left(t \right)} + I_{xz} \omega_{Z}^{2}{\left(t \right)} - I_{yz} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{zz} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{y}}{\left(t \right)}\right) + \left(I_{xy} I_{zz} + I_{xz} I_{yz}\right) \left(- I_{xy} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{xz} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{yy} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{yz} \omega_{Y}^{2}{\left(t \right)} - I_{yz} \omega_{Z}^{2}{\left(t \right)} - I_{zz} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{x}}{\left(t \right)}\right)}{- I_{xx} I_{yy} I_{zz} + I_{xx} I_{yz}^{2} + I_{xy}^{2} I_{zz} + 2 I_{xy} I_{xz} I_{yz} + I_{xz}^{2} I_{yy}}$

$\displaystyle \alpha_{Z}{\left(t \right)} = - \frac{\left(I_{xx} I_{yy} - I_{xy}^{2}\right) \left(I_{xx} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{xy} \omega_{X}^{2}{\left(t \right)} - I_{xy} \omega_{Y}^{2}{\left(t \right)} - I_{xz} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} - I_{yy} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{yz} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{z}}{\left(t \right)}\right) + \left(I_{xx} I_{yz} + I_{xy} I_{xz}\right) \left(- I_{xx} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{xy} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} - I_{xz} \omega_{X}^{2}{\left(t \right)} + I_{xz} \omega_{Z}^{2}{\left(t \right)} - I_{yz} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{zz} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{y}}{\left(t \right)}\right) + \left(I_{xy} I_{yz} + I_{xz} I_{yy}\right) \left(- I_{xy} \omega_{X}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{xz} \omega_{X}{\left(t \right)} \omega_{Y}{\left(t \right)} + I_{yy} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} + I_{yz} \omega_{Y}^{2}{\left(t \right)} - I_{yz} \omega_{Z}^{2}{\left(t \right)} - I_{zz} \omega_{Y}{\left(t \right)} \omega_{Z}{\left(t \right)} + \operatorname{M_{x}}{\left(t \right)}\right)}{- I_{xx} I_{yy} I_{zz} + I_{xx} I_{yz}^{2} + I_{xy}^{2} I_{zz} + 2 I_{xy} I_{xz} I_{yz} + I_{xz}^{2} I_{yy}}$

```python

transformation_func_to_print = [
    dict(num_name='body_to_wind_dcm', input_args=sp.Array([alpha, beta]),
        sym_expr=sp.Array(W.dcm(Bw)),
        extra_assignments={}, ),

]

transformation_printer = codegen.ModuleNumPyPrinter(
    for_class=False,
)

transformation_evaluator = codegen.ModulePrinter(transformation_printer)

transformation_print_params = codegen.process_funcs_to_print(transformation_func_to_print)

transformation_code = '\n'.join(transformation_evaluator.codeprint(transformation_print_params).split('\n')[2:])

```

```python
def CPM(vec):
    return sp.Array(
        [
            [0, -vec[2], vec[1],],
            [vec[2], 0, -vec[0],],
            [-vec[1], vec[0], 0,],
        ]
    )
```

```python
dyn_funcs_to_print = [
    dict(num_name=total_aero_forces_moments_func.func.__name__, input_args=sp.Array(total_aero_forces_moments_func.args),
        sym_expr=total_aero_forces_moments_expr,
        extra_assignments=tot_aero_extra_dict, ),

    dict(num_name='dynamics_output_function', input_args=dynamics_input,
        sym_expr=dynamics_output.subs(dynamics_output_dict),
        extra_assignments=dynamics_output_extra_dict, ),

    dict(num_name='mrc_to_com_cpm', input_args=None,
        sym_expr=CPM(mrc_to_com_vec.to_matrix(Bw)),
        extra_assignments={}, ),

]

dyn_printer = codegen.ModuleNumPyPrinter(
    for_class=True, function_modules={total_aero_forces_moments_func.func: 'self', base_aero_coeffs_func.func: 'self',},
#     function_modules=func_modules, function_names=func_names,
    constant_modules={var: 'self' for var in dynamics_constants}
)
dyn_evaluator = codegen.ModulePrinter(dyn_printer)

dyn_code_print_params = codegen.process_funcs_to_print(dyn_funcs_to_print)

dyn_code = dyn_evaluator.codeprint(dyn_code_print_params)

with open('dynamics.py', "w") as open_file:
    open_file.write(dyn_code)
    open_file.write(transformation_code)
```

Here we print plain-text column names, LaTeX column names, and a set of index constants to facilitate user development with the SimuPy Flight Vehicle Toolkit blocks.

```python
print([str(arg) for arg in codegen.staticfy_expressions(kinematics_output)] + [str(arg) for arg in codegen.staticfy_expressions(dynamics_output)], "\n")
print(['$'+sp.latex(arg)+'$' for arg in codegen.staticfy_expressions(kinematics_output)] + ['$'+sp.latex(arg)+'$' for arg in codegen.staticfy_expressions(dynamics_output)],"\n")
for idx, arg in enumerate(codegen.staticfy_expressions(kinematics_output)):
    print("%s_idx = %d" % (str(arg), idx))
print("\n")
for idx, arg in enumerate(codegen.staticfy_expressions(sp.Array(input_forces_moments_func.args))):
    print("%s_arg_idx = %d" % (str(arg), idx))
```

    ['p_x', 'p_y', 'p_z', 'q_0', 'q_1', 'q_2', 'q_3', 'v_x', 'v_y', 'v_z', 'omega_X', 'omega_Y', 'omega_Z', 'lamda_D', 'phi_D', 'h_D', 'psi', 'theta', 'phi', 'rho', 'c_s', 'mu', 'V_T', 'alpha', 'beta', 'p_B', 'q_B', 'r_B', 'V_N', 'V_E', 'V_D', 'W_N', 'W_E', 'W_D', 'A_X', 'A_Y', 'A_Z', 'alpha_X', 'alpha_Y', 'alpha_Z']

    ['$p_{x}$', '$p_{y}$', '$p_{z}$', '$q_{0}$', '$q_{1}$', '$q_{2}$', '$q_{3}$', '$v_{x}$', '$v_{y}$', '$v_{z}$', '$\\omega_{X}$', '$\\omega_{Y}$', '$\\omega_{Z}$', '$\\lambda_{D}$', '$\\phi_{D}$', '$h_{D}$', '$\\psi$', '$\\theta$', '$\\phi$', '$\\rho$', '$c_{s}$', '$\\mu$', '$V_{T}$', '$\\alpha$', '$\\beta$', '$p_{B}$', '$q_{B}$', '$r_{B}$', '$V_{N}$', '$V_{E}$', '$V_{D}$', '$W_{N}$', '$W_{E}$', '$W_{D}$', '$A_{X}$', '$A_{Y}$', '$A_{Z}$', '$\\alpha_{X}$', '$\\alpha_{Y}$', '$\\alpha_{Z}$']

    p_x_idx = 0
    p_y_idx = 1
    p_z_idx = 2
    q_0_idx = 3
    q_1_idx = 4
    q_2_idx = 5
    q_3_idx = 6
    v_x_idx = 7
    v_y_idx = 8
    v_z_idx = 9
    omega_X_idx = 10
    omega_Y_idx = 11
    omega_Z_idx = 12
    lamda_D_idx = 13
    phi_D_idx = 14
    h_D_idx = 15
    psi_idx = 16
    theta_idx = 17
    phi_idx = 18
    rho_idx = 19
    c_s_idx = 20
    mu_idx = 21
    V_T_idx = 22
    alpha_idx = 23
    beta_idx = 24
    p_B_idx = 25
    q_B_idx = 26
    r_B_idx = 27
    V_N_idx = 28
    V_E_idx = 29
    V_D_idx = 30
    W_N_idx = 31
    W_E_idx = 32
    W_D_idx = 33

    t_arg_idx = 0
    p_x_arg_idx = 1
    p_y_arg_idx = 2
    p_z_arg_idx = 3
    q_0_arg_idx = 4
    q_1_arg_idx = 5
    q_2_arg_idx = 6
    q_3_arg_idx = 7
    v_x_arg_idx = 8
    v_y_arg_idx = 9
    v_z_arg_idx = 10
    omega_X_arg_idx = 11
    omega_Y_arg_idx = 12
    omega_Z_arg_idx = 13
    lamda_D_arg_idx = 14
    phi_D_arg_idx = 15
    h_D_arg_idx = 16
    psi_arg_idx = 17
    theta_arg_idx = 18
    phi_arg_idx = 19
    rho_arg_idx = 20
    c_s_arg_idx = 21
    mu_arg_idx = 22
    V_T_arg_idx = 23
    alpha_arg_idx = 24
    beta_arg_idx = 25
    p_B_arg_idx = 26
    q_B_arg_idx = 27
    r_B_arg_idx = 28
    V_N_arg_idx = 29
    V_E_arg_idx = 30
    V_D_arg_idx = 31
    W_N_arg_idx = 32
    W_E_arg_idx = 33
    W_D_arg_idx = 34
    qbar_arg_idx = 35
    Ma_arg_idx = 36
    Re_arg_idx = 37
    *args_arg_idx = 38

```python

```


# 1. Building a Symbolic Regressor

In this short notebook, we detail how to use the *symbolic-pursuit* package that we developped to build a concise symbolic model for a black-box model. Here, the black-box model whe shall use is a *MLP* regressor model for a UCI dataset *wine quality red* .
Note that our implementation of the meijer G-functions relies on the *pysymbolic* package.
Let us simply start by importing the package we are going to use.

```python
from datasets.data_loader_UCI import data_loader, mixup  # dataset loader for the UCI dataset
from symbolic_pursuit.models import SymbolicRegressor  # our symbolic model is an instance of this class
from sklearn.neural_network import MLPRegressor # we use a MLP regressor as the black-box model
from sklearn.metrics import mean_squared_error # we are going to assess the quality of the model based on the generalization MSE
from sympy import init_printing # We use sympy to display mathematical expresssions
import numpy as np # we use numpy to deal with arrays
init_printing()
```

We now split the dataset into a trainig and a test subest. All the features are normalized to the range $[0,1]$ and the labels are divided by the average of their absolute value.

```python
X_train, y_train, X_test, y_test = data_loader("wine-quality-red")
```

A MLP regressor is fitted to the training subset:

```python
model = MLPRegressor()
model.fit(X_train, y_train)
```

    MLPRegressor()

Now, we shall build the training set for the *symbolic model*. To capture the peculiarities of our black-box, this is done by using a mixup strategy on the original training set .

```python
X_random = mixup(X_train)
```

We use these as training points to fit a *symbolic model* to the black-box MLP regressor.
This model is built by using a projection pursuit strategy . Note that the evaluation of Meijer G-functions is slow in the current Python implementations so this step might take a while.

```python
symbolic_model = SymbolicRegressor()
symbolic_model.fit(model.predict, X_random)
```

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Now working on term number  1 .
    ====================================================================================================
    Now working on hyperparameter tree number  1 .
    Warning: Maximum number of iterations has been exceeded.
             Current function value: 0.000874
             Iterations: 100
             Function evaluations: 4066
             Gradient evaluations: 271
    ====================================================================================================
    The algorithm stopped because the desired precision was achieved.
    ====================================================================================================
    The tree number  1  was selected as the best.
    ====================================================================================================
    Backfitting complete.
    ====================================================================================================
    ====================================================================================================
    The current model has the following expression:  2.02497523215338*[ReLU(P1)]**(-0.574534824570374)*exp(-0.717164049104568*I*pi)*besseli(1.43432809820914, 2.0*sqrt([ReLU(P1)])*exp_polar(I*pi/2))
    The current value of the loss is:  0.0008740979630138096 .
    ----------------------------------------------------------------------------------------------------
    ----------------------------------------------------------------------------------------------------
    The final model has the following expression:
    2.02497523215338*[ReLU(P1)]**(-0.574534824570374)*exp(-0.717164049104568*I*pi)*besseli(1.43432809820914, 2.0*sqrt([ReLU(P1)])*exp_polar(I*pi/2))
    P1 =  0.0821157076919267*X1 + 2.2222385474471*X10 + 2.86348529417163*X11 - 2.00819120966791*X2 - 0.0472597539114896*X3 - 0.019596897290765*X4 - 1.52651591902618*X5 - 0.0995295576817738*X6 - 0.74165017805444*X7 + 0.580946231471347*X8 + 0.0842686829440613*X9
    The number of terms inside the expansion is 1 .
    The current loss is 0.0008740979630138096 .
    ----------------------------------------------------------------------------------------------------

We can now compare the performance of the two models in terms of their MSE evaluated on the test set.

```python
print("MSE score for the MLP Regressor: ", mean_squared_error(y_test, model.predict(X_test)))
print("MSE score for the Symbolic Regressor: ", mean_squared_error(y_test, symbolic_model.predict(X_test)))
```

    MSE score for the MLP Regressor:  0.01293668006861153
    MSE score for the Symbolic Regressor:  0.013986550167855516

As we can see, the performance of both model is comparable. The difference between the two model is the fact that the symbolic model is expressed in terms of analytic *Meijer G-functions* whose expression is short and concise. Let us display the epxression for the faithful model we just obtained.

```python
symbolic_model.get_expression()
```

As we can see, this model only involves one Bessel function.This model is expressed in terms of the following linear combinations of the features:

```python
symbolic_model.print_projections()
```

    P1 =  0.0821157076919267*X1 + 2.2222385474471*X10 + 2.86348529417163*X11 - 2.00819120966791*X2 - 0.0472597539114896*X3 - 0.019596897290765*X4 - 1.52651591902618*X5 - 0.0995295576817738*X6 - 0.74165017805444*X7 + 0.580946231471347*X8 + 0.0842686829440613*X9

## References

alg/Symbolic-Pursuit/1. Building a Symbolic Regressor.ipynb