 |
0.2.0
|
|
0.2.0
|
\begin{equation} \mathbf{\dot{C}}_b^e = \mathbf{C}_b^e \boldsymbol{\Omega}_{eb}^b \end{equation}
where
see
The body rate with respect to the Earth-fixed frame \( \boldsymbol{\omega}_{eb}^b \) can be expressed as
\begin{equation} \boldsymbol{\omega}_{eb}^b = \boldsymbol{\omega}_{ib}^b - \mathbf{C}_e^b \boldsymbol{\omega}_{ie}^e \end{equation}
this together with the skew-symmetric matrix transformation (see [17] Groves, ch. 2.3.1, eq. 2.51, p. 45)
\begin{equation} \mathbf{\Omega}_{\beta\alpha}^\delta = \mathbf{C}_\gamma^\delta \mathbf{\Omega}_{\beta\alpha}^\gamma \mathbf{C}_\delta^\gamma \end{equation}
leads to the skew symmetric form of eq-ImuIntegrator-Mechanization-e-Attitude-DCM
\begin{equation} \mathbf{\dot{C}}_b^e = \mathbf{C}_b^e \boldsymbol{\Omega}_{ib}^b - \boldsymbol{\Omega}_{ie}^e \mathbf{C}_b^e \end{equation}
The quaternion \( \mathbf{q} = a + \mathbf{i} b + \mathbf{j} c + \mathbf{k} d \) propagates as (see [45] Titterton, ch. 3.6.4.3, eq. 3.60-3.62, p. 44)
\begin{equation} \mathbf{\dot{q}}_b^e = 0.5 \mathbf{q}_b^e \cdot \mathbf{p}_{eb}^b \end{equation}
where \( \mathbf{p}_{eb}^b = \begin{pmatrix} 0 & \boldsymbol{\omega}_{eb}^b \end{pmatrix}^T = \begin{pmatrix} 0 & \omega_{eb,x}^b & \omega_{eb,y}^b & \omega_{eb,z}^b \end{pmatrix}^T \)
This can be written in matrix form as
\begin{equation} \mathbf{\dot{q}}_b^e = \begin{bmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \end{bmatrix} = 0.5 \begin{bmatrix} a & -b & -c & -d \\ b & a & -d & c \\ c & d & a & -b \\ d & -c & b & a \end{bmatrix} \begin{bmatrix} 0 \\ \omega_{eb,x}^b \\ \omega_{eb,y}^b \\ \omega_{eb,z}^b \end{bmatrix} \end{equation}
\begin{equation} \begin{aligned} \dot{a} &= -0.5 (b \omega_{eb,x}^b + c \omega_{eb,y}^b + d \omega_{eb,z}^b) &= 0.5 ( 0 \cdot a - \omega_{eb,x}^b b - \omega_{eb,y}^b c - \omega_{eb,z}^b d) \\ \dot{b} &= 0.5 (a \omega_{eb,x}^b - d \omega_{eb,y}^b + c \omega_{eb,z}^b) &= 0.5 (\omega_{eb,x}^b a + 0 \cdot b + \omega_{eb,z}^b c - \omega_{eb,y}^b d) \\ \dot{c} &= 0.5 (d \omega_{eb,x}^b + a \omega_{eb,y}^b - b \omega_{eb,z}^b) &= 0.5 (\omega_{eb,y}^b a - \omega_{eb,z}^b b + 0 \cdot c + \omega_{eb,x}^b d) \\ \dot{d} &= -0.5 (c \omega_{eb,x}^b - b \omega_{eb,y}^b - a \omega_{eb,z}^b) &= 0.5 (\omega_{eb,z}^b a + \omega_{eb,y}^b b - \omega_{eb,x}^b c + 0 \cdot d) \\ \end{aligned} \end{equation}
and this can be written in matrix form again as
\begin{equation} \mathbf{\dot{q}}_b^e = \begin{bmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{eb,x}^b & -\omega_{eb,y}^b & -\omega_{eb,z}^b \\ \omega_{eb,x}^b & 0 & \omega_{eb,z}^b & -\omega_{eb,y}^b \\ \omega_{eb,y}^b & -\omega_{eb,z}^b & 0 & \omega_{eb,x}^b \\ \omega_{eb,z}^b & \omega_{eb,y}^b & -\omega_{eb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \\ d \end{bmatrix} \end{equation}
see
The derivative of the velocity in Earth frame coordinates can be expressed as
\begin{equation} \boldsymbol{\dot{v}}^e = \mathbf{C}_b^e \cdot \overbrace{\boldsymbol{f}_{ib}^b}^{\text{measured}} -\ \underbrace{2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e}_{\text{coriolis acceleration}} +\ \overbrace{\boldsymbol{\gamma}^e}^{\text{gravitation}} -\ \underbrace{\left(\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] \right)}_{\text{centrifugal acceleration}} \end{equation}
where
see
\begin{equation} \boldsymbol{\dot{x}}^e = \boldsymbol{v}^e \end{equation}
see
The universal Runge Kutta 4th order equations are
\begin{equation} \begin{aligned} y_{i} &= y_{i-1} + \frac{h}{6} \left( k_1 + 2 k_2 + 2 k_3 + k_4 \right) \\ k_1 &= f(y_{i-1}, t_{i-1}) \\ k_2 &= f(y_{i-1} + \frac{h}{2} k_1, t_{i-1} + \frac{h}{2}) \\ k_3 &= f(y_{i-1} + \frac{h}{2} k_2, t_{i-1} + \frac{h}{2}) \\ k_4 &= f(y_{i-1} + h \cdot k_2, t_{i-1} + h) \\ \end{aligned} \end{equation}
As the position, velocity and attitude depend on each other, the equations have to be solved simultaneously and we should not first advance \( \boldsymbol{v} \) and then advance \( \boldsymbol{x} \).
\begin{equation} f =\\ \begin{pmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \\ \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \\ \dot{x} \\ \dot{y} \\ \dot{z} \end{pmatrix} = \begin{pmatrix} \frac{1}{2} \begin{bmatrix} 0 & -\omega_{eb,x}^b & -\omega_{eb,y}^b & -\omega_{eb,z}^b \\ \omega_{eb,x}^b & 0 & \omega_{eb,z}^b & -\omega_{eb,y}^b \\ \omega_{eb,y}^b & -\omega_{eb,z}^b & 0 & \omega_{eb,x}^b \\ \omega_{eb,z}^b & \omega_{eb,y}^b & -\omega_{eb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \\ d \end{bmatrix} \\ \\ \mathbf{C}_b^e \cdot \boldsymbol{f}_{ib}^b -\ 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e +\ \boldsymbol{\gamma}^e -\ \left(\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] \right) \\ \\ v_x \\ v_y \\ v_z \end{pmatrix} \end{equation}
Taking only the position and velocity terms and simplifying we get
\begin{equation} f = \begin{pmatrix} \boldsymbol{\dot{v}} \\ \boldsymbol{\dot{x}} \end{pmatrix} = \begin{pmatrix} \mathbf{C}_b^e \cdot \boldsymbol{f}_{ib}^b -\ 2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e +\ \boldsymbol{\gamma}^e (\mathbf{x}^e) -\ \left(\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] \right) \\ \boldsymbol{v} \end{pmatrix} = \begin{pmatrix} \boldsymbol{a} (\mathbf{C}_b^e, \boldsymbol{v}^e, \mathbf{x}^e) \\ \boldsymbol{v} \end{pmatrix} \approx \begin{pmatrix} \boldsymbol{a} (\mathbf{x}^e) \\ \boldsymbol{v} \end{pmatrix} \end{equation}
Which leads to the terms
\begin{equation} \begin{aligned} k_1 &= \begin{pmatrix} k_{1_{v}} \\ k_{1_{x}} \end{pmatrix} = f(y_{i-1}, t_{i-1}) = \begin{pmatrix} \boldsymbol{a} (\mathbf{x}^e_{i-1}) \\ \boldsymbol{v}_{i-1} \end{pmatrix} \\ k_2 &= \begin{pmatrix} k_{2_{v}} \\ k_{2_{x}} \end{pmatrix} = f(y_{i-1} + \frac{h}{2} k_1, t_{i-1} + \frac{h}{2}) = \begin{pmatrix} \boldsymbol{a} (\mathbf{x}^e_{i-1} + \frac{h}{2} k_{1_{x}}) \\ \boldsymbol{v}_{i-1} + \frac{h}{2} k_{1_{v}} \end{pmatrix} \\ k_3 &= \dots \end{aligned} \end{equation}
These terms coincide with [47] Voesenek, eq. 9,10
1.10.0