In [16]:

    

from IPython.display import Image
Image(filename="figure.png",width=600)
from IPython.display import display, HTML

display(HTML(data="""
<style>
    div#notebook-container    { width: 95%; }
    div#menubar-container     { width: 65%; }
    div#maintoolbar-container { width: 99%; }
</style>
"""))


    

In [5]:

    

import sympy as sp
import numpy as np
from IPython.display import display
sp.init_printing(use_latex='mathjax')


    

In [6]:

    

# parameters
# Angular moment of inertia
J_B = 1e-2 * np.diag([1., 1., 1.])

# Gravity
g_I = np.array((-1, 0., 0.))

# Fuel consumption
alpha_m = 0.01

# Vector from thrust point to CoM
r_T_B = np.array([-1e-2, 0., 0.])


def dir_cosine(q):
        return np.matrix([
            [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] +
                                                   q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
            [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 *
             (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
            [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] -
                                                   q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
        ])

def omega(w):
        return np.matrix([
            [0, -w[0], -w[1], -w[2]],
            [w[0], 0, w[2], -w[1]],
            [w[1], -w[2], 0, w[0]],
            [w[2], w[1], -w[0], 0],
        ])

def skew(v):
    return np.matrix([
            [0, -v[2], v[1]],
            [v[2], 0, -v[0]],
            [-v[1], v[0], 0]
        ])


    

In [7]:

    

f = sp.zeros(14, 1)

x = sp.Matrix(sp.symbols(
    'm rx ry rz vx vy vz q0 q1 q2 q3 wx wy wz', real=True))
u = sp.Matrix(sp.symbols('ux uy uz', real=True))

g_I = sp.Matrix(g_I)
r_T_B = sp.Matrix(r_T_B)
J_B = sp.Matrix(J_B)

C_B_I = dir_cosine(x[7:11, 0])
C_I_B = C_B_I.transpose()

f[0, 0] = - alpha_m * u.norm()
f[1:4, 0] = x[4:7, 0]
f[4:7, 0] = 1 / x[0, 0] * C_I_B * u + g_I
f[7:11, 0] = 1 / 2 * omega(x[11:14, 0]) * x[7: 11, 0]
f[11:14, 0] = J_B ** -1 * \
    (skew(r_T_B) * u - skew(x[11:14, 0]) * J_B * x[11:14, 0])


    

In [8]:

    

display(sp.simplify(f)) # f


    

    






$$\left[\begin{matrix}- 0.01 \sqrt{ux^{2} + uy^{2} + uz^{2}}\\vx\\vy\\vz\\\frac{- 1.0 m - ux \left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\right) - 2 uy \left(q_{0} q_{3} - q_{1} q_{2}\right) + 2 uz \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m}\\\frac{2 ux \left(q_{0} q_{3} + q_{1} q_{2}\right) - uy \left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\right) - 2 uz \left(q_{0} q_{1} - q_{2} q_{3}\right)}{m}\\\frac{- 2 ux \left(q_{0} q_{2} - q_{1} q_{3}\right) + 2 uy \left(q_{0} q_{1} + q_{2} q_{3}\right) - uz \left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\right)}{m}\\- 0.5 q_{1} wx - 0.5 q_{2} wy - 0.5 q_{3} wz\\0.5 q_{0} wx + 0.5 q_{2} wz - 0.5 q_{3} wy\\0.5 q_{0} wy - 0.5 q_{1} wz + 0.5 q_{3} wx\\0.5 q_{0} wz + 0.5 q_{1} wy - 0.5 q_{2} wx\\0\\1.0 uz\\- 1.0 uy\end{matrix}\right]$$

In [12]:

    

display(sp.simplify(f.jacobian(x)))# A


    

    






$$\left[\begin{array}{cccccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{ux \left(2 q_{2}^{2} + 2 q_{3}^{2} - 1\right) + 2 uy \left(q_{0} q_{3} - q_{1} q_{2}\right) - 2 uz \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(q_{2} uz - q_{3} uy\right)}{m} & \frac{2 \left(q_{2} uy + q_{3} uz\right)}{m} & \frac{2 \left(q_{0} uz + q_{1} uy - 2 q_{2} ux\right)}{m} & \frac{2 \left(- q_{0} uy + q_{1} uz - 2 q_{3} ux\right)}{m} & 0 & 0 & 0\\\frac{- 2 ux \left(q_{0} q_{3} + q_{1} q_{2}\right) + uy \left(2 q_{1}^{2} + 2 q_{3}^{2} - 1\right) + 2 uz \left(q_{0} q_{1} - q_{2} q_{3}\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(- q_{1} uz + q_{3} ux\right)}{m} & \frac{2 \left(- q_{0} uz - 2 q_{1} uy + q_{2} ux\right)}{m} & \frac{2 \left(q_{1} ux + q_{3} uz\right)}{m} & \frac{2 \left(q_{0} ux + q_{2} uz - 2 q_{3} uy\right)}{m} & 0 & 0 & 0\\\frac{2 ux \left(q_{0} q_{2} - q_{1} q_{3}\right) - 2 uy \left(q_{0} q_{1} + q_{2} q_{3}\right) + uz \left(2 q_{1}^{2} + 2 q_{2}^{2} - 1\right)}{m^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & \frac{2 \left(q_{1} uy - q_{2} ux\right)}{m} & \frac{2 \left(q_{0} uy - 2 q_{1} uz + q_{3} ux\right)}{m} & \frac{2 \left(- q_{0} ux - 2 q_{2} uz + q_{3} uy\right)}{m} & \frac{2 \left(q_{1} ux + q_{2} uy\right)}{m} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - 0.5 wx & - 0.5 wy & - 0.5 wz & - 0.5 q_{1} & - 0.5 q_{2} & - 0.5 q_{3}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wx & 0 & 0.5 wz & - 0.5 wy & 0.5 q_{0} & - 0.5 q_{3} & 0.5 q_{2}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wy & - 0.5 wz & 0 & 0.5 wx & 0.5 q_{3} & 0.5 q_{0} & - 0.5 q_{1}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0.5 wz & 0.5 wy & - 0.5 wx & 0 & - 0.5 q_{2} & 0.5 q_{1} & 0.5 q_{0}\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$

In [10]:

    

sp.simplify(f.jacobian(u)) # B


    

    
Out[10]:





$$\left[\begin{matrix}- \frac{0.01 ux}{\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \frac{0.01 uy}{\sqrt{ux^{2} + uy^{2} + uz^{2}}} & - \frac{0.01 uz}{\sqrt{ux^{2} + uy^{2} + uz^{2}}}\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\\frac{- 2 q_{2}^{2} - 2 q_{3}^{2} + 1}{m} & \frac{2 \left(- q_{0} q_{3} + q_{1} q_{2}\right)}{m} & \frac{2 \left(q_{0} q_{2} + q_{1} q_{3}\right)}{m}\\\frac{2 \left(q_{0} q_{3} + q_{1} q_{2}\right)}{m} & \frac{- 2 q_{1}^{2} - 2 q_{3}^{2} + 1}{m} & \frac{2 \left(- q_{0} q_{1} + q_{2} q_{3}\right)}{m}\\\frac{2 \left(- q_{0} q_{2} + q_{1} q_{3}\right)}{m} & \frac{2 \left(q_{0} q_{1} + q_{2} q_{3}\right)}{m} & \frac{- 2 q_{1}^{2} - 2 q_{2}^{2} + 1}{m}\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 0\\0 & 0 & 1.0\\0 & -1.0 & 0\end{matrix}\right]$$