Equations of Motion v1#

Introduction#

This document briefly describes the equations of motion which govern the flight of a rocket used in v1.0 onwards. This document simply shows some of the algebraic steps used to get to the final form of the equations of motion used in the code. For a more detailed explanation of the equations of motion, please refer to Equations of Motion v0.

Development#

Linear Equation:

\[\begin{split}m \left( \dot{\mathbf{v}} + \dot{\omega} \times \mathbf{r}_{\mathrm{CM}} \right. & \left.+\boldsymbol{\omega} \times\left(\boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}} \right) + \mathbf{r}_{CM}^{\prime \prime}+2 \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime}\right) \\ = & \mathbf{T} - 2 \dot{m} \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\ddot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\mathbf{A}+\sum_i \mathbf{N}_i-m g \hat{\mathbf{a}}_3\end{split}\]

Angular Equation:

\[\mathbf{I} \cdot \dot{\boldsymbol{\omega}}+\boldsymbol{\omega} \times(\mathbf{I} \cdot \boldsymbol{\omega})+\mathbf{I}^{\prime} \cdot \boldsymbol{\omega}+m \mathbf{r}_{\mathrm{CM}} \times \dot{\mathbf{v}}=\left(\dot{m} \mathbf{S}_{\mathrm{noz}}\right) \cdot \boldsymbol{\omega}+\sum_i \mathbf{r}_i \times \mathbf{N}_i-\mathbf{r}_{\mathrm{CM}} \times m g \hat{\mathbf{a}}_3\]

Cross multiplying the linear equation by \(r_{CM}\) :

\[\begin{split}\begin{aligned} m \mathbf{r}_{\mathrm{CM}} \times \dot{\mathbf{v}}+m \mathbf{r}_{\mathrm{CM}} \times\left(\dot{\boldsymbol{\omega}} \times \mathbf{r}_{\mathrm{CM}}\right)+m \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times\left(\boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}\right)+m \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime \prime}+2 m \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime} \\ \quad=\mathbf{r}_{\mathrm{CM}} \times \mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+m \mathbf{r}_{\mathrm{CM}}^{\prime} \times\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right) \\ \quad+\mathbf{r}_{\mathrm{CM}} \times \mathbf{A}+\mathbf{r}_{\mathrm{CM}} \times \sum_i \mathbf{N}_i-m \mathbf{r}_{\mathrm{CM}} \times g \hat{\mathbf{a}}_3 \end{aligned}\end{split}\]

Simplifying:

\[ \begin{align}\begin{aligned}\mathbf{r}_{\mathrm{CM}} \times\left(\dot{\omega} \times \mathbf{r}_{\mathrm{CM}}\right)=\left(\mathbf{r}_{\mathrm{CM}} \cdot \mathbf{r}_{\mathrm{CM}}\right) \dot{\boldsymbol{\omega}}-\left(\mathbf{r}_{\mathrm{CM}} \cdot \dot{\boldsymbol{\omega}}\right) \mathbf{r}_{\mathrm{CM}}\\\mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times\left(\boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}\right)=\mathbf{r}_{\mathrm{CM}} \times\left(\left(\boldsymbol{\omega} \cdot \mathbf{r}_{\mathrm{CM}}\right) \boldsymbol{\omega}-(\boldsymbol{\omega} \cdot \boldsymbol{\omega}) \mathbf{r}_{\mathrm{CM}}\right)=\left(\boldsymbol{\omega} \cdot \mathbf{r}_{\mathrm{CM}}\right) \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega}\end{aligned}\end{align} \]

\[\begin{split}\begin{aligned} m \mathbf{r}_{\mathrm{CM}} \times \dot{\mathbf{v}}=- & m\left(\mathbf{r}_{\mathrm{CM}} \cdot \mathbf{r}_{\mathrm{CM}}\right) \dot{\boldsymbol{\omega}}+m\left(\mathbf{r}_{\mathrm{CM}} \cdot \dot{\boldsymbol{\omega}}\right) \mathbf{r}_{\mathrm{CM}}-m\left(\boldsymbol{\omega} \cdot \mathbf{r}_{\mathrm{CM}}\right) \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega}-m \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime \prime} \\ & -2 m \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime}+\mathbf{r}_{\mathrm{CM}} \times \mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right) \\ & +\ddot{m} \mathbf{r}_{\mathrm{CM}} \times\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\mathbf{r}_{\mathrm{CM}} \times \mathbf{A}+\mathbf{r}_{\mathrm{CM}} \times \sum_i \mathbf{N}_i-m \mathbf{r}_{\mathrm{CM}} \times g \hat{\mathbf{a}}_3 \end{aligned}\end{split}\]

Substituting in the Angular equation:

\[\begin{split}\begin{aligned} \mathbf{I} \cdot \dot{\boldsymbol{\omega}}+\boldsymbol{\omega} \times(\mathbf{I} \cdot \boldsymbol{\omega})+\mathbf{I}^{\prime} \cdot \boldsymbol{\omega}-m\left(\mathbf{r}_{\mathrm{CM}} \cdot \mathbf{r}_{\mathrm{CM}}\right) \dot{\boldsymbol{\omega}}+m\left(\mathbf{r}_{\mathrm{CM}} \cdot \dot{\boldsymbol{\omega}}\right) \mathbf{r}_{\mathrm{CM}}-m\left(\boldsymbol{\omega} \cdot \mathbf{r}_{\mathrm{CM}}\right) \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega}-m \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime \prime} \\ -2 m \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime}+\mathbf{r}_{\mathrm{CM}} \times \mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right) \\ +\ddot{m} \mathbf{r}_{\mathrm{CM}} \times\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\mathbf{r}_{\mathrm{CM}} \times \mathbf{A}+\mathbf{r}_{\mathrm{CM}} \times \sum_i \mathbf{N}_i-m \mathbf{r}_{\mathrm{CM}} \times g \hat{\mathbf{a}}_3 \\ =\left(\dot{m} \mathbf{S}_{\mathrm{noz}}\right) \cdot \boldsymbol{\omega}+\sum_i \mathbf{r}_i \times \mathbf{N}_i-\mathbf{r}_{\mathrm{CM}} \times m g \hat{\mathbf{a}}_3 \end{aligned}\end{split}\]

\[\begin{split}\begin{aligned} & \mathbf{I} \cdot \dot{\boldsymbol{\omega}}-m\left(\mathbf{r}_{\mathrm{CM}} \cdot \mathbf{r}_{\mathrm{CM}}\right) \dot{\boldsymbol{\omega}}+m\left(\mathbf{r}_{\mathrm{CM}} \cdot \dot{\boldsymbol{\omega}}\right) \mathbf{r}_{\mathrm{CM}}=-\boldsymbol{\omega} \times(\mathbf{I} \cdot \boldsymbol{\omega})-\mathbf{I}^{\prime} \cdot \boldsymbol{\omega}+m\left(\boldsymbol{\omega} \cdot \mathbf{r}_{\mathrm{CM}}\right) \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega}+ \\ & m \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime \prime}+2 m \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime}-\mathbf{r}_{\mathrm{CM}} \times \mathbf{T}+2 \dot{m} \mathbf{r}_{\mathrm{CM}} \times \mathbf{r}_{\mathrm{CM}}^{\prime}-2 \mathbf{r}_{\mathrm{CM}} \times \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)- \\ & \ddot{m} \mathbf{r}_{\mathrm{CM}} \times\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)-\mathbf{r}_{\mathrm{CM}} \times \mathbf{A}-\mathbf{r}_{\mathrm{CM}} \times \sum_i \mathbf{N}_i+\left(\dot{m} \mathbf{S}_{\mathrm{noz}}\right) \cdot \boldsymbol{\omega}+\sum_i \mathbf{r}_i \times \mathbf{N}_i \end{aligned}\end{split}\]

Solving for 𝐯̇:

Linear

\[\begin{split}\begin{gathered} \dot{\mathbf{v}}=\frac{\left(\mathbf{T}-2 \dot{m} \mathbf{r}_{\mathrm{CM}}^{\prime}+2 \boldsymbol{\omega} \times \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)+\ddot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right) \mathbf{A}+\sum_i \mathbf{N}_i\right)}{m}-g \hat{a}_3-\dot{\boldsymbol{\omega}} \times \mathbf{r}_{\mathrm{CM}} \\ -\boldsymbol{\omega} \times\left(\boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}\right)-\mathbf{r}_{\mathrm{CM}}^{\prime \prime}-2 \boldsymbol{\omega} \times \mathbf{r}_{\mathrm{CM}}^{\prime} \end{gathered}\end{split}\]

Angular

\[\mathbf{r}_{\mathrm{CM}} \times \dot{\mathbf{v}}=\frac{\left(\left(\dot{m} \mathbf{S}_{\mathrm{noz}}\right) \cdot \boldsymbol{\omega}+\sum_i \mathbf{r}_i \times \mathbf{N}_i-\mathbf{I} \cdot \dot{\boldsymbol{\omega}}-\boldsymbol{\omega} \times(\mathbf{I} \cdot \boldsymbol{\omega})-\mathbf{I}^{\prime} \cdot \boldsymbol{\omega}\right)}{\boldsymbol{m}}-\mathbf{r}_{\mathrm{CM}} \times g \hat{\mathbf{a}}_3\]

\[\mathbf{b}=\frac{\left(\left(\dot{m} \mathbf{S}_{\mathrm{noz}}\right) \cdot \boldsymbol{\omega}+\sum_i \mathbf{r}_i \times \mathbf{N}_i-\mathbf{I} \cdot \dot{\boldsymbol{\omega}}-\boldsymbol{\omega} \times(\mathbf{I} \cdot \boldsymbol{\omega})-\mathbf{I}^{\prime} \cdot \boldsymbol{\omega}\right)}{\boldsymbol{m}}-\mathbf{r}_{\mathrm{CM}} \times g \hat{\mathbf{a}}_3\]

\[\mathbf{r}_{\mathrm{CM}} \times \dot{\mathbf{v}}=\boldsymbol{b}\]

Reorganized EOM to aid execution speed

Linear

\[\begin{split}\begin{aligned} m \dot{\mathbf{v}}+\left[m \mathbf{r}_{\mathrm{CM}}\right]_{\times}^T & \cdot \dot{\boldsymbol{\omega}} \\ & =-\omega \times\left(\omega \times m \mathbf{r}_{\mathrm{CM}}\right)+\omega \times\left(2 \dot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)-2 m \mathbf{r}_{\mathrm{CM}}^{\prime}\right)+\mathrm{T}-m \mathbf{r}_{\mathrm{CM}}^{\prime \prime}-2 \dot{m} \mathbf{r}_{\mathrm{CM}}^{\prime} \\ & +\ddot{m}\left(\mathbf{r}_{\mathrm{noz}}-\mathbf{r}_{\mathrm{CM}}\right)-m g \hat{\mathbf{a}}_3+\mathbf{A}+\sum_i \mathbf{N}_i \end{aligned}\end{split}\]

Angular

\[\mathbf{I} \cdot \dot{\boldsymbol{\omega}}+\left[m \mathbf{r}_{\mathrm{CM}}\right]_{\times} \cdot \dot{\mathbf{v}}=-\omega \times(\mathbf{I} \cdot \omega)+\left(\dot{m} \mathbf{S}_{\mathrm{noz}}-\mathbf{I}^{\prime}\right) \cdot \omega-\mathbf{r}_{\mathrm{CM}} \times m g \hat{\mathbf{a}}_3+\sum_i \mathbf{r}_i \times \mathbf{N}_i\]

Available terms that must be interpolated in time/altitude

\(m\): mass
\(𝑚'\): time derivative of \(m\)
\(𝑚''\): time derivative of \(𝑚'\)
\(r_{CM}\):
\(r_{CM}'\):
\(r_{CM}''\):
\(T\): thrust
\(I\): inertia tensor
\(I'\): time derivative of \(I\)
\(g\): gravity acceleration

Pre-computed terms that optimize interpolations needed

\(m\): mass
\(\mathrm{r}_{CM}\): position vector of the center of mass
\(\mathbf{T}_{03}\): \(2\dot{m} \left( r_{noz} - r_{CM} \right) - 2 \cdot m \cdot r_{CM}\)
\(\mathbf{T}_{04}\): \(T - m \cdot r_{CM}' - 2 \cdot 𝑚̇ \cdot r_{CM} + 𝑚̈ \cdot (r_{noz} - r_{CM})\)
\(\mathbf{T}_{05}\): \(\dot{m} \cdot S_{noz} - I'\)
\(g\): gravity acceleration
\(\mathbf{I}\): inertia tensor

Pre-computed terms

\(\mathbf{T}_{00}\): \(m \cdot \mathrm{r}_{\mathrm{CM}}\)
\(\mathbf{T}_{01}\): \([m \cdot \mathrm{r}_{\mathrm{CM}}] \times\)
\(\mathbf{T}_{02}\): \([m \cdot \mathrm{r}_{\mathrm{CM}}] \times \mathbf{T}'\)
\(\mathbf{T}_{03}\): \(2\cdot \dot{m} (\mathrm{r}_{noz} - \mathrm{r}_{\mathrm{CM}}) - 2 \cdot m \mathrm{r}_{\mathrm{CM}}\)
\(\mathbf{T}_{04}\): \(\mathbf{T} - m \cdot \mathrm{r}_{\mathrm{CM}}'' - 2 \cdot \dot{m} \cdot \mathrm{r}_{\mathrm{CM}} + \ddot{m} (\mathrm{r}_{noz} - \mathrm{r}_{\mathrm{CM}})\)
\(\mathbf{T}_{05}\): \(\dot{m} \cdot S_{noz} - \mathbf{I}'\)
\(\mathbf{T}_{20}\): \(-\omega \times (\omega \times \mathbf{T}_{00}) + \omega \times (\mathbf{T}_{03}) + \mathbf{T}_{04} - m \cdot g \hat{a}_3 + \mathbf{A} + \sum \mathbf{N}_{i}\)
\(\mathbf{T}_{21}\): \(-\omega \times (\mathbf{I} \cdot \omega) + (T_{05}) \cdot \omega + \mathrm{r}_{\mathrm{CM}} \times m \cdot g \hat{a}_3 + \sum r_{i} \times \mathbf{N}_{i}\)

Final system of equations

\[ \begin{align}\begin{aligned}\mathrm{M} \cdot \dot{\mathbf{v}}+\left[m \mathbf{r}_{\mathrm{CM}}\right]_{\times}^T \cdot \dot{\boldsymbol{\omega}}=T_{20}\\\mathbf{I} \cdot \dot{\boldsymbol{\omega}}+\left[\mathrm{mr}_{\mathrm{CM}}\right]_x \cdot \dot{\mathbf{v}}=T_{21}\end{aligned}\end{align} \]

Solution to system of equations

\[\dot{\boldsymbol{\omega}}=\left(\left(\mathrm{I}-\left[m \mathbf{r}_{\mathrm{CM}}\right]_X \cdot \mathrm{M}^{-1} \cdot\left[m \mathbf{r}_{\mathrm{CM}}\right]_X^T\right)\right)^{-1} \cdot\left(T_{21}-\left[m \mathbf{r}_{\mathrm{CM}}\right]_X \cdot \mathrm{M}^{-1} \cdot T_{20}\right)\]

\[\dot{\mathbf{v}}=\mathrm{M}^{-1} \cdot\left(T_{20}-\left[m \mathbf{r}_{\mathrm{CM}}\right]_{\times}^T \cdot \dot{\boldsymbol{\omega}}\right)\]

Taking a closer look at the matrix inversion:

\[\mathbf{H}=\left[m \mathbf{r}_{\mathrm{CM}}\right]_{\times} \cdot \mathbf{M}^{-1} \cdot\left[m \mathbf{r}_{\mathrm{CM}}\right]_{\times}^T\]

\[\mathbf{H}=-m\left[\mathrm{r}_{\mathrm{CM}}\right]_{\times}^2\]

\[\begin{split}\mathbf{H}=-m\left[\begin{array}{ccc} 0 & -r_{\mathrm{CM}_3} & r_{\mathrm{CM}_2} \\ r_{\mathrm{CM}_3} & 0 & -r_{\mathrm{CM}_1} \\ -r_{\mathrm{CM}_2} & r_{\mathrm{CM}_1} & 0 \end{array}\right]^2\end{split}\]

\[\begin{split}\mathbf{H} = -m \left[ \begin{array}{ccc} 0 & -r_{\mathrm{CM}_3} & r_{\mathrm{CM}_2} \\ r_{\mathrm{CM}_3} & 0 & -r_{\mathrm{CM}_1} \\ -r_{\mathrm{CM}_2} & r_{\mathrm{CM}_1} & 0 \end{array}\right]\left[\begin{array}{ccc} 0 & -r_{\mathrm{CM}_3} & r_{\mathrm{CM}_2} \\ r_{\mathrm{CM}_3} & 0 & -r_{\mathrm{CM}_1} \\ -r_{\mathrm{CM}_2} & r_{\mathrm{CM}_1} & 0 \end{array} \right]\end{split}\]

\[\begin{split}\mathbf{H}=-m\left[\begin{array}{ccc} -r_{\mathrm{CM}_3}^2-r_{\mathrm{CM}_2}^2 & r_{\mathrm{CM}_2} r_{\mathrm{CM}_1} & r_{\mathrm{CM}_3} r_{\mathrm{CM}_1} \\ r_{\mathrm{CM}_2} r_{\mathrm{CM}_1} & -r_{\mathrm{CM}_3}^2-r_{\mathrm{CM}_1^2} & r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} \\ r_{\mathrm{CM}_3} r_{\mathrm{CM}_1} & r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} & -r_{\mathrm{CM}_2}-r_{\mathrm{CM}_1}^2 \end{array}\right]\end{split}\]

\[\begin{split}\mathbf{H} = m \left[ \begin{array}{ccc} r_{\mathrm{CM}_3}^2+r_{\mathrm{CM}_2}^2 & -r_{\mathrm{CM}_2} r_{\mathrm{CM}_1} & -r_{\mathrm{CM}_3} r_{\mathrm{CM}_1} \\ -r_{\mathrm{CM}_2} r_{\mathrm{CM}_1} & r_{\mathrm{CM}_3}^2+r_{\mathrm{CM}_1}^2 & -r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} \\ -r_{\mathrm{CM}_3} r_{\mathrm{CM}_1} & -r_{\mathrm{CM}_3} r_{\mathrm{CM}_2} & r_{\mathrm{CM}_2}^2+r_{\mathrm{CM}_1}^2 \end{array}\right]\end{split}\]

Consider \(I_{CM}\) as the inertia tensor relative to the true center of mass. Then:

\[ \begin{align}\begin{aligned}\mathbf{I}_{\mathrm{CM}}+\mathbf{H}=\mathbf{I}\\\mathbf{I}_{\mathrm{CM}}=\mathbf{I}-\mathbf{H}\end{aligned}\end{align} \]

New simplified equations:

\[ \begin{align}\begin{aligned}\dot{\omega} = \mathbf{I}_{\mathrm{CM}}{ }^{-1} \cdot\left(T_{21}-\left[\mathrm{r}_{\mathrm{CM}}\right]_{\times} \cdot T_{20} \right)\\\dot{\mathbf{v}} = \mathrm{M}^{-1} \cdot \left( T_{20}-\left[m \mathrm{r}_{\mathrm{CM}}\right]_{\mathrm{x}}^T \cdot \dot{\omega} \right)\end{aligned}\end{align} \]