```python
from sympy import *
init_printing()
```

```python
def skew(l):
    l1, l2, l3 = l
    return Matrix([
        [0, -l3, l2],
        [l3, 0, -l1],
        [-l2, l1, 0]
    ])
```

```python
# define state variables
x, y, z, eta0, eps1, eps2, eps3, u, v, w, p, q, r = symbols('x y z et0 eps1 eps2 eps3 u v w p q r', real=True)
s = Matrix([x, y, z, eta0, eps1, eps2, eps3, u, v, w, p, q, r])

# position and orientation
eta = Matrix([x, y, z, eta0, eps1, eps2, eps3])
nu = Matrix([u, v, w, p, q, r])

# centre of gravity
xg, yg, zg = symbols('xg yg zg', real=True)
rg = Matrix([xg, yg, zg])

# centre of bouyancy
xb, yb, zb = symbols('xb yb zb', real=True)
rb = Matrix([xb, yb, zb])

# center of pressure
xcp, ycp, zcp = symbols('xcp ycp zcp', real=True)
rcp = Matrix([xcp, ycp, zcp])

# mass matrix
m = symbols('m', real=True, positive=True)
Ixx, Iyy, Izz = symbols('Ixx Iyy Izz')
I0 = diag(Ixx, Iyy, Izz)
M = BlockMatrix([
    [m*eye(3), -m*skew(rg)],
    [m*skew(rg), I0]
])
M = Matrix(M)
# M = simplify(M)

# Coriolis and centripetal matrix
nu1 = Matrix([u, v, w])
nu2 = Matrix([p, q, r])
crb = BlockMatrix([
    [zeros(3), -m*skew(nu1)-m*skew(nu2)*skew(rg)],
    [-m*skew(nu1)+m*skew(rg)*skew(nu2), -skew(I0*nu2)]
])
crb = Matrix(crb)
# crb = simplify(crb)

# damping matrix
Xuu, Yvv, Zww, Kpp, Mqq, Nrr = symbols(
    'Xuu Yvv Zww Kpp Mqq Nrr', real=True
)
D = Matrix([
    [Xuu*abs(u), 0, 0, 0, 0, 0],
    [0, Yvv*abs(v), 0, 0, 0, 0],
    [0, 0, Zww*abs(w), 0, 0, 0],
    [0, -zcp*Yvv*abs(v), ycp*Zww*abs(w), Kpp*abs(p), 0, 0],
    [zcp*Xuu*abs(u), 0, -xcp*Zww*abs(w), 0, Mqq*abs(q), 0],
    [-ycp*Xuu*abs(u), xcp*Yvv*abs(v), 0, 0, 0, Nrr*abs(r)]
])
# D = simplify(D)

# rotational transform between body and NED quaternions
Tq = Rational(1,2)*Matrix([
    [-eps1, -eps2, -eps3],
    [eta0, -eps3, eps2],
    [eps3, eta0, -eps1],
    [-eps2, eps1, eta0]
])
# Tq = simplify(Tq)

Rq = Matrix([
    [1-2*(eps2**2+eps3**2), 2*(eps1*eps2-eps3*eta0), 2*(eps1*eps3+eps2*eta0)],
    [2*(eps1*eps2+eps3*eta0), 1-2*(eps1**2+eps3**2), 2*(eps2*eps3-eps1*eta0)],
    [2*(eps1*eps3-eps2*eta0), 2*(eps2*eps3+eps1*eta0), 1-2*(eps1**2+eps2**2)]
])
Jeta = BlockMatrix([
    [Rq, zeros(3)],
    [zeros(4,3), Tq]
])
Jeta = Matrix(Jeta)
# Jeta = simplify(Jeta)

# bouyancy in quaternions
W, B = symbols('W B', real=True)
fg = Matrix([0, 0, W])
fb = Matrix([0, 0, -B])
Rqinv = Rq.inv()
geta = Matrix([
    Rqinv*(fg+fb),
    skew(rg)*Rqinv*fg + skew(rb)*Rqinv*fb
])
# geta = simplify(geta)
```

```python
# print(cse(Jeta))
```

```python
# thrust model
Kt0, Kt1 = symbols('Kt0 Kt1', real=True)
Kt = Matrix([Kt0, Kt1])
Qt0, Qt1 = symbols('Qt0 Qt1', real=True)
Qt = Matrix([Qt0, Qt1])

# control inputs
rpm0, rpm1 = symbols('rpm0 rpm1', real=True)
rpm = Matrix([rpm0, rpm1])
de, dr = symbols('de dr', real=True)
control_vector = Matrix([rpm0, rpm1, de, dr])

# control force vector
Ft = Kt.dot(rpm)
Mt = Qt.dot(rpm)

# coefficient for each element in cost function
tauc = Matrix([
    Ft*cos(de)*cos(dr),
    -Ft*sin(dr),
    Ft*sin(de)*cos(dr),
    Mt*cos(de)*cos(dr),
    -Mt*sin(dr),
    Mt*sin(de)*cos(dr)
])
```

```python
etadot = Jeta*nu
nudot = M.inv()*(tauc - (crb + D)*nu - geta)
```

```python
sdot = Matrix([
   etadot,
   nudot
])
```

```python
sdot
```

$\displaystyle \left[\begin{matrix}u \left(- 2 eps_{2}^{2} - 2 eps_{3}^{2} + 1\right) + v \left(2 eps_{1} eps_{2} - 2 eps_{3} et_{0}\right) + w \left(2 eps_{1} eps_{3} + 2 eps_{2} et_{0}\right)\\u \left(2 eps_{1} eps_{2} + 2 eps_{3} et_{0}\right) + v \left(- 2 eps_{1}^{2} - 2 eps_{3}^{2} + 1\right) + w \left(- 2 eps_{1} et_{0} + 2 eps_{2} eps_{3}\right)\\u \left(2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right) + v \left(2 eps_{1} et_{0} + 2 eps_{2} eps_{3}\right) + w \left(- 2 eps_{1}^{2} - 2 eps_{2}^{2} + 1\right)\\- \frac{eps_{1} p}{2} - \frac{eps_{2} q}{2} - \frac{eps_{3} r}{2}\\\frac{eps_{2} r}{2} - \frac{eps_{3} q}{2} + \frac{et_{0} p}{2}\\- \frac{eps_{1} r}{2} + \frac{eps_{3} p}{2} + \frac{et_{0} q}{2}\\\frac{eps_{1} q}{2} - \frac{eps_{2} p}{2} + \frac{et_{0} r}{2}\\\frac{\left(- Iyy m xg yg zg + Izz m xg yg zg\right) \left(B \left(\frac{yb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Iyy q r - Izz q r - Kpp p \left|{p}\right| - W \left(\frac{yg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- m q yg - m r zg\right) - v \left(- Yvv zcp \left|{v}\right| + m p yg + m w\right) - w \left(Zww ycp \left|{w}\right| + m p zg - m v\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx Iyy yg - Ixx m xg^{2} yg - Iyy m yg^{3} - Iyy m yg zg^{2}\right) \left(B \left(\frac{xb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Ixx p q - Iyy p q - Nrr r \left|{r}\right| - W \left(\frac{xg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- Xuu ycp \left|{u}\right| + m r xg + m v\right) - v \left(Yvv xcp \left|{v}\right| + m r yg - m u\right) - w \left(- m p xg - m q yg\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Izz zg + Ixx m xg^{2} zg + Izz m yg^{2} zg + Izz m zg^{3}\right) \left(B \left(- \frac{xb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - Ixx p r + Izz p r - Mqq q \left|{q}\right| - W \left(- \frac{xg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(Xuu zcp \left|{u}\right| + m q xg - m w\right) - v \left(- m p xg - m r zg\right) - w \left(- Zww xcp \left|{w}\right| + m q zg + m u\right) + \left(- Qt_{0} rpm_{0} - Qt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Iyy xg yg + Ixx m xg^{3} yg + Iyy m xg yg^{3} + Izz m xg yg zg^{2}\right) \left(- Yvv v \left|{v}\right| - p \left(- m p yg - m w\right) - q \left(m p xg + m r zg\right) - r \left(- m r yg + m u\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(- Kt_{0} rpm_{0} - Kt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Izz xg zg + Ixx m xg^{3} zg + Iyy m xg yg^{2} zg + Izz m xg zg^{3}\right) \left(- Zww w \left|{w}\right| - p \left(- m p zg + m v\right) - q \left(- m q zg - m u\right) - r \left(m p xg + m q yg\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Xuu u \left|{u}\right| - p \left(m q yg + m r zg\right) - q \left(- m q xg + m w\right) - r \left(- m r xg - m v\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right) \left(Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Izz m xg^{2} + Ixx m^{2} xg^{4} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Izz m^{2} xg^{2} zg^{2}\right)}{Ixx Iyy Izz m - Ixx Iyy m^{2} xg^{2} - Ixx Iyy m^{2} yg^{2} - Ixx Izz m^{2} xg^{2} - Ixx Izz m^{2} zg^{2} + Ixx m^{3} xg^{4} + Ixx m^{3} xg^{2} yg^{2} + Ixx m^{3} xg^{2} zg^{2} - Iyy Izz m^{2} yg^{2} - Iyy Izz m^{2} zg^{2} + Iyy m^{3} xg^{2} yg^{2} + Iyy m^{3} yg^{4} + Iyy m^{3} yg^{2} zg^{2} + Izz m^{3} xg^{2} zg^{2} + Izz m^{3} yg^{2} zg^{2} + Izz m^{3} zg^{4}}\\\frac{\left(Ixx m xg yg zg - Izz m xg yg zg\right) \left(B \left(- \frac{xb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - Ixx p r + Izz p r - Mqq q \left|{q}\right| - W \left(- \frac{xg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(Xuu zcp \left|{u}\right| + m q xg - m w\right) - v \left(- m p xg - m r zg\right) - w \left(- Zww xcp \left|{w}\right| + m q zg + m u\right) + \left(- Qt_{0} rpm_{0} - Qt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Iyy xg + Ixx m xg^{3} + Ixx m xg zg^{2} + Iyy m xg yg^{2}\right) \left(B \left(\frac{xb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Ixx p q - Iyy p q - Nrr r \left|{r}\right| - W \left(\frac{xg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- Xuu ycp \left|{u}\right| + m r xg + m v\right) - v \left(Yvv xcp \left|{v}\right| + m r yg - m u\right) - w \left(- m p xg - m q yg\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Iyy Izz zg - Iyy m yg^{2} zg - Izz m xg^{2} zg - Izz m zg^{3}\right) \left(B \left(\frac{yb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Iyy q r - Izz q r - Kpp p \left|{p}\right| - W \left(\frac{yg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- m q yg - m r zg\right) - v \left(- Yvv zcp \left|{v}\right| + m p yg + m w\right) - w \left(Zww ycp \left|{w}\right| + m p zg - m v\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Iyy xg yg + Ixx m xg^{3} yg + Iyy m xg yg^{3} + Izz m xg yg zg^{2}\right) \left(- Xuu u \left|{u}\right| - p \left(m q yg + m r zg\right) - q \left(- m q xg + m w\right) - r \left(- m r xg - m v\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx m xg^{2} yg zg - Iyy Izz yg zg + Iyy m yg^{3} zg + Izz m yg zg^{3}\right) \left(- Zww w \left|{w}\right| - p \left(- m p zg + m v\right) - q \left(- m q zg - m u\right) - r \left(m p xg + m q yg\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Yvv v \left|{v}\right| - p \left(- m p yg - m w\right) - q \left(m p xg + m r zg\right) - r \left(- m r yg + m u\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(- Kt_{0} rpm_{0} - Kt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right) \left(Ixx Iyy Izz - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{2} yg^{2} - Iyy Izz m yg^{2} + Iyy m^{2} yg^{4} + Izz m^{2} yg^{2} zg^{2}\right)}{Ixx Iyy Izz m - Ixx Iyy m^{2} xg^{2} - Ixx Iyy m^{2} yg^{2} - Ixx Izz m^{2} xg^{2} - Ixx Izz m^{2} zg^{2} + Ixx m^{3} xg^{4} + Ixx m^{3} xg^{2} yg^{2} + Ixx m^{3} xg^{2} zg^{2} - Iyy Izz m^{2} yg^{2} - Iyy Izz m^{2} zg^{2} + Iyy m^{3} xg^{2} yg^{2} + Iyy m^{3} yg^{4} + Iyy m^{3} yg^{2} zg^{2} + Izz m^{3} xg^{2} zg^{2} + Izz m^{3} yg^{2} zg^{2} + Izz m^{3} zg^{4}}\\\frac{\left(- Ixx m xg yg zg + Iyy m xg yg zg\right) \left(B \left(\frac{xb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Ixx p q - Iyy p q - Nrr r \left|{r}\right| - W \left(\frac{xg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- Xuu ycp \left|{u}\right| + m r xg + m v\right) - v \left(Yvv xcp \left|{v}\right| + m r yg - m u\right) - w \left(- m p xg - m q yg\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx Izz xg - Ixx m xg^{3} - Ixx m xg yg^{2} - Izz m xg zg^{2}\right) \left(B \left(- \frac{xb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - Ixx p r + Izz p r - Mqq q \left|{q}\right| - W \left(- \frac{xg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(Xuu zcp \left|{u}\right| + m q xg - m w\right) - v \left(- m p xg - m r zg\right) - w \left(- Zww xcp \left|{w}\right| + m q zg + m u\right) + \left(- Qt_{0} rpm_{0} - Qt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Iyy Izz yg + Iyy m xg^{2} yg + Iyy m yg^{3} + Izz m yg zg^{2}\right) \left(B \left(\frac{yb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Iyy q r - Izz q r - Kpp p \left|{p}\right| - W \left(\frac{yg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- m q yg - m r zg\right) - v \left(- Yvv zcp \left|{v}\right| + m p yg + m w\right) - w \left(Zww ycp \left|{w}\right| + m p zg - m v\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Izz xg zg + Ixx m xg^{3} zg + Iyy m xg yg^{2} zg + Izz m xg zg^{3}\right) \left(- Xuu u \left|{u}\right| - p \left(m q yg + m r zg\right) - q \left(- m q xg + m w\right) - r \left(- m r xg - m v\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx m xg^{2} yg zg - Iyy Izz yg zg + Iyy m yg^{3} zg + Izz m yg zg^{3}\right) \left(- Yvv v \left|{v}\right| - p \left(- m p yg - m w\right) - q \left(m p xg + m r zg\right) - r \left(- m r yg + m u\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(- Kt_{0} rpm_{0} - Kt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Zww w \left|{w}\right| - p \left(- m p zg + m v\right) - q \left(- m q zg - m u\right) - r \left(m p xg + m q yg\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right) \left(Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}\right)}{Ixx Iyy Izz m - Ixx Iyy m^{2} xg^{2} - Ixx Iyy m^{2} yg^{2} - Ixx Izz m^{2} xg^{2} - Ixx Izz m^{2} zg^{2} + Ixx m^{3} xg^{4} + Ixx m^{3} xg^{2} yg^{2} + Ixx m^{3} xg^{2} zg^{2} - Iyy Izz m^{2} yg^{2} - Iyy Izz m^{2} zg^{2} + Iyy m^{3} xg^{2} yg^{2} + Iyy m^{3} yg^{4} + Iyy m^{3} yg^{2} zg^{2} + Izz m^{3} xg^{2} zg^{2} + Izz m^{3} yg^{2} zg^{2} + Izz m^{3} zg^{4}}\\\frac{\left(- Iyy m xg yg zg + Izz m xg yg zg\right) \left(- Xuu u \left|{u}\right| - p \left(m q yg + m r zg\right) - q \left(- m q xg + m w\right) - r \left(- m r xg - m v\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Iyy Izz yg + Iyy m xg^{2} yg + Iyy m yg^{3} + Izz m yg zg^{2}\right) \left(- Zww w \left|{w}\right| - p \left(- m p zg + m v\right) - q \left(- m q zg - m u\right) - r \left(m p xg + m q yg\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Iyy Izz zg - Iyy m yg^{2} zg - Izz m xg^{2} zg - Izz m zg^{3}\right) \left(- Yvv v \left|{v}\right| - p \left(- m p yg - m w\right) - q \left(m p xg + m r zg\right) - r \left(- m r yg + m u\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(- Kt_{0} rpm_{0} - Kt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Iyy m xg zg + m^{2} xg^{3} zg + m^{2} xg yg^{2} zg + m^{2} xg zg^{3}\right) \left(B \left(\frac{xb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Ixx p q - Iyy p q - Nrr r \left|{r}\right| - W \left(\frac{xg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- Xuu ycp \left|{u}\right| + m r xg + m v\right) - v \left(Yvv xcp \left|{v}\right| + m r yg - m u\right) - w \left(- m p xg - m q yg\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Izz m xg yg + m^{2} xg^{3} yg + m^{2} xg yg^{3} + m^{2} xg yg zg^{2}\right) \left(B \left(- \frac{xb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - Ixx p r + Izz p r - Mqq q \left|{q}\right| - W \left(- \frac{xg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(Xuu zcp \left|{u}\right| + m q xg - m w\right) - v \left(- m p xg - m r zg\right) - w \left(- Zww xcp \left|{w}\right| + m q zg + m u\right) + \left(- Qt_{0} rpm_{0} - Qt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Iyy Izz - Iyy m xg^{2} - Iyy m yg^{2} - Izz m xg^{2} - Izz m zg^{2} + m^{2} xg^{4} + m^{2} xg^{2} yg^{2} + m^{2} xg^{2} zg^{2}\right) \left(B \left(\frac{yb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Iyy q r - Izz q r - Kpp p \left|{p}\right| - W \left(\frac{yg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- m q yg - m r zg\right) - v \left(- Yvv zcp \left|{v}\right| + m p yg + m w\right) - w \left(Zww ycp \left|{w}\right| + m p zg - m v\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}}\\\frac{\left(Ixx m xg yg zg - Izz m xg yg zg\right) \left(- Yvv v \left|{v}\right| - p \left(- m p yg - m w\right) - q \left(m p xg + m r zg\right) - r \left(- m r yg + m u\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(- Kt_{0} rpm_{0} - Kt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx Izz xg - Ixx m xg^{3} - Ixx m xg yg^{2} - Izz m xg zg^{2}\right) \left(- Zww w \left|{w}\right| - p \left(- m p zg + m v\right) - q \left(- m q zg - m u\right) - r \left(m p xg + m q yg\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Izz zg + Ixx m xg^{2} zg + Izz m yg^{2} zg + Izz m zg^{3}\right) \left(- Xuu u \left|{u}\right| - p \left(m q yg + m r zg\right) - q \left(- m q xg + m w\right) - r \left(- m r xg - m v\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx m yg zg + m^{2} xg^{2} yg zg + m^{2} yg^{3} zg + m^{2} yg zg^{3}\right) \left(B \left(\frac{xb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Ixx p q - Iyy p q - Nrr r \left|{r}\right| - W \left(\frac{xg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- Xuu ycp \left|{u}\right| + m r xg + m v\right) - v \left(Yvv xcp \left|{v}\right| + m r yg - m u\right) - w \left(- m p xg - m q yg\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Izz m xg yg + m^{2} xg^{3} yg + m^{2} xg yg^{3} + m^{2} xg yg zg^{2}\right) \left(B \left(\frac{yb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Iyy q r - Izz q r - Kpp p \left|{p}\right| - W \left(\frac{yg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- m q yg - m r zg\right) - v \left(- Yvv zcp \left|{v}\right| + m p yg + m w\right) - w \left(Zww ycp \left|{w}\right| + m p zg - m v\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx Izz - Ixx m xg^{2} - Ixx m yg^{2} - Izz m yg^{2} - Izz m zg^{2} + m^{2} xg^{2} yg^{2} + m^{2} yg^{4} + m^{2} yg^{2} zg^{2}\right) \left(B \left(- \frac{xb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - Ixx p r + Izz p r - Mqq q \left|{q}\right| - W \left(- \frac{xg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(Xuu zcp \left|{u}\right| + m q xg - m w\right) - v \left(- m p xg - m r zg\right) - w \left(- Zww xcp \left|{w}\right| + m q zg + m u\right) + \left(- Qt_{0} rpm_{0} - Qt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}}\\\frac{\left(- Ixx m xg yg zg + Iyy m xg yg zg\right) \left(- Zww w \left|{w}\right| - p \left(- m p zg + m v\right) - q \left(- m q zg - m u\right) - r \left(m p xg + m q yg\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx Iyy xg + Ixx m xg^{3} + Ixx m xg zg^{2} + Iyy m xg yg^{2}\right) \left(- Yvv v \left|{v}\right| - p \left(- m p yg - m w\right) - q \left(m p xg + m r zg\right) - r \left(- m r yg + m u\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(- Kt_{0} rpm_{0} - Kt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx Iyy yg - Ixx m xg^{2} yg - Iyy m yg^{3} - Iyy m yg zg^{2}\right) \left(- Xuu u \left|{u}\right| - p \left(m q yg + m r zg\right) - q \left(- m q xg + m w\right) - r \left(- m r xg - m v\right) - \frac{\left(- B + W\right) \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \left(Kt_{0} rpm_{0} + Kt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Ixx m yg zg + m^{2} xg^{2} yg zg + m^{2} yg^{3} zg + m^{2} yg zg^{3}\right) \left(B \left(- \frac{xb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - Ixx p r + Izz p r - Mqq q \left|{q}\right| - W \left(- \frac{xg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} + \frac{zg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(Xuu zcp \left|{u}\right| + m q xg - m w\right) - v \left(- m p xg - m r zg\right) - w \left(- Zww xcp \left|{w}\right| + m q zg + m u\right) + \left(- Qt_{0} rpm_{0} - Qt_{1} rpm_{1}\right) \sin{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(- Iyy m xg zg + m^{2} xg^{3} zg + m^{2} xg yg^{2} zg + m^{2} xg zg^{3}\right) \left(B \left(\frac{yb \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Iyy q r - Izz q r - Kpp p \left|{p}\right| - W \left(\frac{yg \left(4 eps_{1}^{2} eps_{3}^{2} - 2 eps_{1}^{2} + 4 eps_{2}^{2} eps_{3}^{2} - 2 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{zg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- m q yg - m r zg\right) - v \left(- Yvv zcp \left|{v}\right| + m p yg + m w\right) - w \left(Zww ycp \left|{w}\right| + m p zg - m v\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \cos{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}} + \frac{\left(Ixx Iyy - Ixx m xg^{2} - Ixx m zg^{2} - Iyy m yg^{2} - Iyy m zg^{2} + m^{2} xg^{2} zg^{2} + m^{2} yg^{2} zg^{2} + m^{2} zg^{4}\right) \left(B \left(\frac{xb \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yb \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) + Ixx p q - Iyy p q - Nrr r \left|{r}\right| - W \left(\frac{xg \left(4 eps_{1}^{2} eps_{2} eps_{3} + 2 eps_{1} et_{0} + 4 eps_{2}^{3} eps_{3} + 4 eps_{2} eps_{3}^{3} + 4 eps_{2} eps_{3} et_{0}^{2} - 2 eps_{2} eps_{3}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1} - \frac{yg \left(4 eps_{1}^{3} eps_{3} + 4 eps_{1} eps_{2}^{2} eps_{3} + 4 eps_{1} eps_{3}^{3} + 4 eps_{1} eps_{3} et_{0}^{2} - 2 eps_{1} eps_{3} - 2 eps_{2} et_{0}\right)}{4 eps_{1}^{4} + 8 eps_{1}^{2} eps_{2}^{2} + 8 eps_{1}^{2} eps_{3}^{2} + 4 eps_{1}^{2} et_{0}^{2} - 4 eps_{1}^{2} + 4 eps_{2}^{4} + 8 eps_{2}^{2} eps_{3}^{2} + 4 eps_{2}^{2} et_{0}^{2} - 4 eps_{2}^{2} + 4 eps_{3}^{4} + 4 eps_{3}^{2} et_{0}^{2} - 4 eps_{3}^{2} + 1}\right) - u \left(- Xuu ycp \left|{u}\right| + m r xg + m v\right) - v \left(Yvv xcp \left|{v}\right| + m r yg - m u\right) - w \left(- m p xg - m q yg\right) + \left(Qt_{0} rpm_{0} + Qt_{1} rpm_{1}\right) \sin{\left(de \right)} \cos{\left(dr \right)}\right)}{Ixx Iyy Izz - Ixx Iyy m xg^{2} - Ixx Iyy m yg^{2} - Ixx Izz m xg^{2} - Ixx Izz m zg^{2} + Ixx m^{2} xg^{4} + Ixx m^{2} xg^{2} yg^{2} + Ixx m^{2} xg^{2} zg^{2} - Iyy Izz m yg^{2} - Iyy Izz m zg^{2} + Iyy m^{2} xg^{2} yg^{2} + Iyy m^{2} yg^{4} + Iyy m^{2} yg^{2} zg^{2} + Izz m^{2} xg^{2} zg^{2} + Izz m^{2} yg^{2} zg^{2} + Izz m^{2} zg^{4}}\end{matrix}\right]$

```python

```


# My First Likelihood

🎶 I'm the very Model of a simple HEP-like measurement... 🎶

## HEP-like?

So what do we do as experimentalists in High Energy Physics? We have a gigantic detector that serves as a counting machine for physics. We smash particles and take pictures of the aftermath. Then we propose hypotheses for new physics that we want to test using this data. So we will have, at the very least:

* signal
* background (with some uncertainty)
* some observations

This is probably the simplest model one could use in HistFactory... so let's make it.

```python
import json
import numpy as np
import matplotlib.pyplot as plt

import pyhf
from pyhf.contrib.viz import brazil  # not imported by default!
```

```python
model = pyhf.simplemodels.hepdata_like(
    signal_data=[5.0, 10.0], bkg_data=[50.0, 60.0], bkg_uncerts=[5.0, 12.0]
)
model
```

What did we just make? This returns a [`pyhf.Model`](https://pyhf.readthedocs.io/en/v0.6.0/_generated/pyhf.pdf.Model.html#pyhf.pdf.Model) object. Let's check out the specification.

```python
print(json.dumps(model.spec, indent=2))
```

As we can see from the specification, we defined a single two-bin channel ('singlechannel') with two samples: a **signal sample** and a **background sample**. The signal sample has an _unconstrained normalization factor_ $\mu$, the signal strength, while the background sample carries an _uncorrelated shape systematic_ controlled by parameters $\gamma_{1}$ and $\gamma_{2}$. The _background uncertainty_ for the bins is $10\%$ and $20\%$ respectively.

These uncertainties are **absolute** (not relative!). We have 10% and 20% relative uncertainty for the bins in the background sample respectively.

(*Note: we have a workspace defined with this simple model in `data/2-bin_1-channel.json` which we will look at later.*)

Let's explore the model a little bit.

```python
print(f"  channels: {model.config.channels}")
print(f"     nbins: {model.config.channel_nbins}")
print(f"   samples: {model.config.samples}")
print(f" modifiers: {model.config.modifiers}")
print(f"parameters: {model.config.parameters}")
print(f"  nauxdata: {model.config.nauxdata}")
print(f"   auxdata: {model.config.auxdata}")
```

## Auxiliary Data

Whoa, hold up! What's with the auxiliary data? Recall the HistFactory definition

$$
p(n,a|\theta) = \underbrace{\prod_{\mathrm{channel}\ c}\prod_{\mathrm{bin}\ b} \mathrm{Pois}(n_{cb} | \lambda_{cb}(\theta))}_{\text{main}} \underbrace{\prod_{\mathrm{constraint}\ \chi} p_\chi(a_\chi | \chi)}_{\text{auxiliary}} \qquad \lambda_{cb}(\theta) = \sum_{\mathrm{sample}\ s} \lambda_{cbs}(\theta)
$$

and the auxiliary data here is passed into a constraint function? There's two things going on here:

1. the uncorrelated shapesys modifier is constrained by a Poisson
2. the auxiliary data is fully determined by the shapesys 'data' and the 'background' data

So if we were to explicitly write out the likelihood we were seeing symbolically, it would be:

$$
\begin{align}
p_\text{main} \cdot p_\text{aux} &= \text{Pois}(n | \lambda) \cdot p_\text{aux}\\
                                 &= \text{Pois}(n | \mu s + \gamma b) \cdot p_\text{aux}\\
                                 &= \text{Pois}(n | \mu s + \gamma b) \cdot \text{Pois}(n_\text{aux} = (b/\delta b)^2 | \mu = (b/\delta b)^2 \gamma )\\
\end{align}
$$

where $n = \{n_1, n_2\}$ for a 2-bin model (we're being slightly fast and loose with our mathematical notation here), and similarly for $s$, $b$, and $\gamma$.

The 'shapesys' is defined in the [HistFactory paper](https://cds.cern.ch/record/1456844)... however, it can be a little hard to extract out the necessary information. We've provided a nice table of [Modifiers and Constraints](https://pyhf.readthedocs.io/en/v0.6.0/intro.html#id24) in the introduction of our pyhf documentation to use as reference.

Let's look at the first row here which is our uncorrelated shape modifier. This is a multiplicative modifier denoted by $\kappa$ per-bin (denoted by $\gamma_b$). Notice that the input for the constraint term requires $\sigma_b$ which is the relative uncertainty of that modifier. This is Poisson-constrained by $\sigma_b^{-2}$. Let's quickly verify by hand to convince ourselves of what's going on here:

```python
(np.array([5.0, 12.0]) / np.array([50.0, 60.0])) ** -2
```

```python
model.config.auxdata
```

Whew. Ok, I guess that makes sense.

## Data and Parameters

What about the actual data for the model then?

```python
model.expected_data()
```

Hrmm, that didn't work. It seems we need to specify the parameter values to get the data for the model at those parameter values. Well, we know the default for $\mu=1$ and $\gamma = 1$. Also recall that the uncorrelated shape allocates a parameter per bin, so our model has **3 parameters** with **2 modifiers**. So let's just try that:

```python
model.expected_data([1.0, 1.0, 1.0])
```

This returns the data for the entire likelihood, the main model as well as the constraint (or auxiliary) model. We can also drop the auxdata to get the actual data.

```python
model.expected_data([1.0, 1.0, 1.0], include_auxdata=False)
```

Similarly, there are also methods separately for the actual data and the auxdata, both which take in all the parameters:

```python
model.expected_actualdata([1.0, 1.0, 1.0])
```

```python
model.expected_auxdata([1.0, 1.0, 1.0])
```

### The Parameter Ordering

So how do we know what these parameters correspond to? The way `pyhf` works under the hood, you can imagine an N-dimensional tensor that is purely bookkeeping all the different expected rates and modifications (read: systematics) involved that we can apply tensor algebra calculations to. This means that the ordering of the entries in each dimension is important, so we need to keep track of the order of the channels, the samples, the parameters. In `pyhf`, you can ask for the parameters or modifiers in a model:

```python
model.config.parameters
```

```python
model.config.modifiers
```

However, these might not necessarily correspond with the order of the parameters when we build this tensor internally, and we keep this order under a different variable

```python
model.config.par_order
```

### The Parameter Multiplicity

But this clearly doesn't answer everything, because we were using three numbers assigned to two parameters! How does that work? Well, recall that the only channel in this model has 2 bins:

```python
model.config.channel_nbins
```

and one of the parameters (the uncorrelated shape systematic) will be configured by $\gamma_b$ - that is, one parameter for each bin of the sample in that channel. How does `pyhf` know that the parameter set is associated with two parameters? We can ask for information about the `paramset`:

```python
model.config.param_set("uncorr_bkguncrt")
```

which we can see is constrained by a Poisson and

```python
model.config.param_set("uncorr_bkguncrt").n_parameters
```

### The Parameter Defaults

So how do we know what we should set the parameters to? The nice thing is that this is up to you! The model will come with some suggested defaults for parameters...

```python
model.config.suggested_init()
```

... and their bounds...

```python
model.config.suggested_bounds()
```

... and if the parameter should be held constant in a fit or not ...

```python
model.config.suggested_fixed()
```

So armed with this knowledge, we could have also done

```python
init_pars = model.config.suggested_init()
model.expected_actualdata(init_pars)
```

To get the expected actual data from the model (signal + background). We could turn off the signal, corresponding with the normalization factor. We just need to know the index of the parameter:

```python
model.config.poi_index
```

and then just change the parameter

```python
bkg_pars = [*init_pars]  # my clever way to "copy" the list by value
bkg_pars[model.config.poi_index] = 0
model.expected_actualdata(bkg_pars)
```

## Simple Inference

The core of statistical analysis is the statistical model. For inference, it's viewed as a function of the parameters with the data fixed.

$$
\log L(\theta | x) = \log p(x | \theta)
$$

The value of the likelihood is a float. Let's try it for both the background-only model as well as the signal+background model.

```python
observations = [52.5, 65.0] + model.config.auxdata  # this is a common pattern!

model.logpdf(pars=bkg_pars, data=observations)
```

```python
model.logpdf(pars=init_pars, data=observations)
```

We're not performing inference just yet. We're simply computing the 'logpdf' of the model specified by the parameters $\theta$ against the provided data. To perform a fit, we use the [inference API](https://pyhf.readthedocs.io/en/v0.6.0/api.html#inference) via `pyhf.infer`.

To fit a model to data, we usually want to find the $\hat{\theta}$ which refers to the "Maximum Likelihood Estimate". This is often referred to mathematically by

$$
\hat{\theta}_\text{MLE} = \text{argmax}_\theta L(\theta | x)
$$

Let's perform a fit on this model to the provided observations we just made up.

```python
pyhf.infer.mle.fit(data=observations, pdf=model)
```

So what can we say? With nominal signal `[5, 10]` and nominal background = `[50, 60]`, an observed count of `[52.5, 65]` suggests best fit values:
* $\hat{\mu} \approx 0.5$,
* $\hat{\gamma} \approx [1,1]$.

## Simple Hypothesis Testing

Great, so we can fit. But that's not usually the interesting part, since we cannot make any statements about this. What we prefer to do is perform a hypothesis test to either:

* reject Standard Model hypothesis - a discovery fit
* reject Beyond the Standard Model hypothesis - an exclusion fit

So we need to compute test statistics in order to evaluate the hypothesis. If we know the distribution of the test statistic under two different hypotheses, we can compute a [CLs](https://en.wikipedia.org/wiki/CLs_method_(particle_physics)) value. This is a modified pseudo-frequentist *p*-value.

$$
\text{CL}_\text{s} = \frac{\text{CL}_\text{s+b}}{\text{CL}_\text{b}}
$$

with

$$
\text{CL}_\text{s+b} = \int_{t_\text{obs}}^\infty \text{d}t\ p_0(t | H_0) \qquad\qquad \text{CL}_\text{b} = \int_{t_\text{obs}}^\infty \text{d}t\ p_1(t | H_1)
$$

This is done by splitting the model parameters up into two groups:

* parameters of interest (POI - can have multiple!)
* nuisance parameters (NP)

Recall that additionally, all parameters are either constrained or unconstrained. Parameters of interest happen to always be unconstrained parameters.

$$
    f(\boldsymbol{x}|\phi) = f(\boldsymbol{x}|\overbrace{\eta}^{\llap{\text{free}}},\underbrace{\chi}_{\llap{\text{constrained}}}) = f(\boldsymbol{x}|\overbrace{\psi}^{\rlap{\text{parameters of interest}}},\underbrace{\theta}_{\rlap{\text{nuisance parameters}}})
$$

In the vast majority of cases, the test statistic is the profile likelihood ratio, or variations of it:

$$
t_\psi(x) = \frac{L(\psi, \hat{\hat{\theta}})}{L({\hat{\psi}, \hat{\theta}})}
$$

where

* $\hat{\hat{\theta}}$ is the best fitted value of the nuisance parameters (for fixed POIs)
* $\hat{\psi}$ and $\hat{\theta}$ are the best fitted values in a global fit

So let's run a hypothesis test for

* null hypothesis ($\mu = 1$) - "SUSY is real"
* alternate hypothesis ($\mu = 0$) - "Standard Model explains it all"

```python
CLs_obs, CLs_exp = pyhf.infer.hypotest(
    1.0,  # null hypothesis
    [52.5, 65.0] + model.config.auxdata,
    model,
    test_stat="q",
    return_expected_set=True,
)
print(f"      Observed CLs: {CLs_obs:.4f}")
for expected_value, n_sigma in zip(CLs_exp, np.arange(-2, 3)):
    print(f"Expected CLs({n_sigma:2d} σ): {expected_value:.4f}")
```

Nicely enough, `pyhf` is smart enough to let us know that, if for a model where $\hat{\psi} < 0$, we're not going to be able to catch it because our default bounds were bounding the parameter of interest from below at 0. For more information, see Equation 14 in [[1007.1727]](https://arxiv.org/abs/1007.1727).

```python
model.config.suggested_bounds()[model.config.poi_index]
```

We should heed the warning and switch our test statistic in this case to a variation of it $q_\mu \to \tilde{q}_\mu$. We could also change the bounds from the default to allow $\mu$ to float below zero. In this case, we're just going to use a more appropriate test statistic as we know $\hat{\mu} \geq 0$.

```python
CLs_obs, CLs_exp = pyhf.infer.hypotest(
    1.0,  # null hypothesis
    [52.5, 65.0] + model.config.auxdata,
    model,
    test_stat="qtilde",
    return_expected_set=True,
)
print(f"      Observed CLs: {CLs_obs:.4f}")
for expected_value, n_sigma in zip(CLs_exp, np.arange(-2, 3)):
    print(f"Expected CLs({n_sigma:2d} σ): {expected_value:.4f}")
```

## Simple Upper Limit

To get upper limits, we just need to run multiple hypothesis tests for a lot of different null hypotheses of BSM with $\mu \in [0, \ldots, 5.0]$ and then find the value of $\mu$ for which the null hypothesis is rejected (a 95% $\text{CL}_\text{s}$).

```python
poi_values = np.linspace(0.1, 5, 50)
results = [
    pyhf.infer.hypotest(
        poi_value,
        observations,
        model,
        test_stat="qtilde",
        return_expected_set=True,
    )
    for poi_value in poi_values
]
```

So this ran pretty fast. We can plot the "standard Brazil band" using the `pyhf.contrib` module (which needs `pyhf[contrib]`):

```python
fig, ax = plt.subplots()
fig.set_size_inches(10.5, 7)
ax.set_title("Hypothesis Tests")
ax.set_xlabel(r"$\mu$")
ax.set_ylabel(r"$\mathrm{CL}_{s}$")

brazil.plot_results(ax, poi_values, results)
```

And then just calculate the upper limit by interpolating:

```python
observed = np.asarray([h[0] for h in results]).ravel()
expected = np.asarray([h[1][2] for h in results]).ravel()
print(f"Upper limit (obs): μ = {np.interp(0.05, observed[::-1], poi_values[::-1]):.4f}")
print(f"Upper limit (exp): μ = {np.interp(0.05, expected[::-1], poi_values[::-1]):.4f}")
```

Alternatively, you can also just use the [`upperlimit` API](https://pyhf.readthedocs.io/en/v0.6.0/_generated/pyhf.infer.intervals.upperlimit.html)

```python
obs_limit, exp_limits, (scan, results) = pyhf.infer.intervals.upperlimit(
    observations, model, poi_values, level=0.05, return_results=True
)
print(f"Upper limit (obs): μ = {obs_limit:.4f}")
print(f"Upper limit (exp): μ = {exp_limits[2]:.4f}")
```


# Algorithms Exercise 2

## Imports

```python
%matplotlib inline
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
```

## Peak finding

Write a function `find_peaks` that finds and returns the indices of the local maxima in a sequence. Your function should:

* Properly handle local maxima at the endpoints of the input array.
* Return a Numpy array of integer indices.
* Handle any Python iterable as input.

```python
def find_peaks(a):
    """Find the indices of the local maxima in a sequence."""
    c = []
    for n in range(0,len(a)):
        if n >= 1 and n <= len(a)-2:
            if a[n] > a[n+1] and a[n] > a[n-1]:
                c.append(n)
        elif n == 0:
            if a[n] > a[n+1]:
                c.append(n)
        elif n == len(a)-1:
            if a[n] > a[n-1]:
                c.append(n)
    d = np.array(c)
    return d
```

```python
p1 = find_peaks([2,0,1,0,2,0,1])
assert np.allclose(p1, np.array([0,2,4,6]))
p2 = find_peaks(np.array([0,1,2,3]))
assert np.allclose(p2, np.array([3]))
p3 = find_peaks([3,2,1,0])
assert np.allclose(p3, np.array([0]))
```

Here is a string with the first 10000 digits of $\pi$ (after the decimal). Write code to perform the following:

* Convert that string to a Numpy array of integers.
* Find the indices of the local maxima in the digits of $\pi$.
* Use `np.diff` to find the distances between consequtive local maxima.
* Visualize that distribution using an appropriately customized histogram.

```python
from sympy import pi, N
pi_digits_str = str(N(pi, 10001))[2:]
```

```python
z = []
for n in pi_digits_str:
    z.append(int(n))
x = np.array(z)
```

```python
y = find_peaks(x)
```

```python
dif = np.diff(y)
```

```python
f = plt.figure(figsize=(7,8))
plt.hist(dif,bins=15)
plt.title('Distance between local maxima of first 10000 digits of pi')
plt.ylabel('count')
plt.xlabel('distance')
```

```python
assert True # use this for grading the pi digits histogram
```


# Algorithms Exercise 2

## Imports

```python
%matplotlib inline
from matplotlib import pyplot as plt
import seaborn as sns
import numpy as np
```

## Peak finding

Write a function `find_peaks` that finds and returns the indices of the local maxima in a sequence. Your function should:

* Properly handle local maxima at the endpoints of the input array.
* Return a Numpy array of integer indices.
* Handle any Python iterable as input.

```python
def find_peaks(a):
    """Find the indices of the local maxima in a sequence."""
    peaks = []
    for i in range(len(a)):
        if i == 0 and a[i] > a[i+1]:
            peaks.append(i)
        elif i == (len(a)-1) and a[i] > a[i-1]:
            peaks.append(i)
        elif a[i] > a[i-1] and a[i] > a[i+1]:
            peaks.append(i)
    return np.array(peaks)
```

```python
find_peaks([2,0,1,0,2,0,1])
```

    array([0, 2, 4, 6])

```python
p1 = find_peaks([2,0,1,0,2,0,1])
assert np.allclose(p1, np.array([0,2,4,6]))
p2 = find_peaks(np.array([0,1,2,3]))
assert np.allclose(p2, np.array([3]))
p3 = find_peaks([3,2,1,0])
assert np.allclose(p3, np.array([0]))
```

Here is a string with the first 10000 digits of $\pi$ (after the decimal). Write code to perform the following:

* Convert that string to a Numpy array of integers.
* Find the indices of the local maxima in the digits of $\pi$.
* Use `np.diff` to find the distances between consequtive local maxima.
* Visualize that distribution using an appropriately customized histogram.

```python
from sympy import pi, N
pi_digits_str = str(N(pi, 10001))[2:]
```

```python
a = pi_digits_str
b = np.array([int(a[i]) for i in range(len(a))])
```

```python
plt.figure(figsize=(12,6))
plt.hist(np.diff(find_peaks(b)), bins=50)
plt.xlim(0,18)
plt.xlabel("Distance")
plt.ylabel("Occurences")
plt.title("Distance Between Consecutive Maxima for First 10,000 Decimals of Pi")
plt.show()

```

```python
assert True # use this for grading the pi digits histogram
```