Earth-fixed frame mechanization

Attitude

Propagation of direction cosine matrix with time

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-DCM} \mathbf{\dotup{C}}_b^e = \mathbf{C}_b^e \boldsymbol{\Omega}_{eb}^b \end{equation}$

where

$\boldsymbol{\Omega}_{eb}^b$ is the skew symmetric form of $\boldsymbol{\omega}_{eb}^b$ , the body rate with respect to the Earth-fixed frame

see

[47] Titterton, ch. 3.5.2, eq. 3.22, p. 29
[24] Jekeli, ch. 4.3.3, eq. 4.83, p. 126
[17] Groves, ch. 5.3.3, eq. 5.35, p. 175

The body rate with respect to the Earth-fixed frame $\boldsymbol{\omega}_{eb}^b$ can be expressed as

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-omega-eb} \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} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-skew-symmetric-matrix-transform} \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 $\eqref{eq:eq-ImuIntegrator-Mechanization-e-Attitude-DCM}$

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-DCM-full} \mathbf{\dotup{C}}_b^e = \mathbf{C}_b^e \boldsymbol{\Omega}_{ib}^b - \boldsymbol{\Omega}_{ie}^e \mathbf{C}_b^e \end{equation}$

Propagation of quaternion with time

The quaternion $\mathbf{q} = a + \mathbf{i} b + \mathbf{j} c + \mathbf{k} d$ propagates as (see [47] Titterton, ch. 3.6.4.3, eq. 3.60-3.62, p. 44)

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-Quaternion-base} \mathbf{\dotup{q}}_b^e = 0.5 \mathbf{q}_b^e \cdot \mathbf{p}_{eb}^b \end{equation}$

where $\mathbf{p}_{eb}^b = \begin{pmatrix} \boldsymbol{\omega}_{eb}^b & 0 \end{pmatrix}^T = \begin{pmatrix} \omega_{eb,x}^b & \omega_{eb,y}^b & \omega_{eb,z}^b & 0 \end{pmatrix}^T$

This can be written in matrix form as

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-Quaternion-base-matrix} \mathbf{\dotup{q}}_b^e = \begin{bmatrix} \dotup{b} \\ \dotup{c} \\ \dotup{d} \\ \dotup{a} \end{bmatrix} = 0.5 \begin{bmatrix} a & -d & c & b \\ d & a & -b & c \\ -c & b & a & d \\ -b & -c & -d & a \end{bmatrix} \begin{bmatrix} \omega_{eb,x}^b \\ \omega_{eb,y}^b \\ \omega_{eb,z}^b \\ 0 \end{bmatrix} \end{equation}$

that is

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-Quaternion-components} \begin{aligned} \dotup{b} &= 0.5 (a \omega_{eb,x}^b - d \omega_{eb,y}^b + c \omega_{eb,z}^b) &= 0.5 (+ 0 \cdot b + \omega_{eb,z}^b c - \omega_{eb,y}^b d + \omega_{eb,x}^b a) \\ \dotup{c} &= 0.5 (d \omega_{eb,x}^b + a \omega_{eb,y}^b - b \omega_{eb,z}^b) &= 0.5 (- \omega_{eb,z}^b b + 0 \cdot c + \omega_{eb,x}^b d + \omega_{eb,y}^b a) \\ \dotup{d} &= -0.5 (c \omega_{eb,x}^b - b \omega_{eb,y}^b - a \omega_{eb,z}^b) &= 0.5 (+ \omega_{eb,y}^b b - \omega_{eb,x}^b c + 0 \cdot d + \omega_{eb,z}^b a) \\ \dotup{a} &= -0.5 (b \omega_{eb,x}^b + c \omega_{eb,y}^b + d \omega_{eb,z}^b) &= 0.5 (- \omega_{eb,x}^b b - \omega_{eb,y}^b c - \omega_{eb,z}^b d + 0 \cdot a) \\ \end{aligned} \end{equation}$

and this can be written in matrix form again as

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Attitude-Quaternion-matrix} \mathbf{\dotup{q}}_b^e = \begin{bmatrix} \dotup{b} \\ \dotup{c} \\ \dotup{d} \\ \dotup{a} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & \omega_{eb,z}^b & -\omega_{eb,y}^b & \omega_{eb,x}^b \\ -\omega_{eb,z}^b & 0 & \omega_{eb,x}^b & \omega_{eb,y}^b \\ \omega_{eb,y}^b & -\omega_{eb,x}^b & 0 & \omega_{eb,z}^b \\ -\omega_{eb,x}^b & -\omega_{eb,y}^b & -\omega_{eb,z}^b & 0 \end{bmatrix} \begin{bmatrix} b \\ c \\ d \\ a \end{bmatrix} \end{equation}$

see

[47] Titterton, ch. 11.2.5, eq. 11.33-11.35, p. 319 (e instead of n system)

Velocity

The derivative of the velocity in Earth frame coordinates can be expressed as

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Velocity} \boldsymbol{\dotup{v}}^e = \mathbf{C}_b^e \cdot \overbrace{\boldsymbol{f}_{ib}^b}^{\hidewidth\text{measured}\hidewidth} -\ \underbrace{2 \boldsymbol{\omega}_{ie}^e \times \boldsymbol{v}^e}_{\hidewidth\text{coriolis acceleration}\hidewidth} +\ \overbrace{\boldsymbol{\gamma}^e}^{\hidewidth\text{gravitation}\hidewidth} -\ \underbrace{\left(\boldsymbol{\omega}_{ie}^e \times [ \boldsymbol{\omega}_{ie}^e \times \mathbf{x}^e ] \right)}_{\hidewidth\text{centrifugal acceleration}\hidewidth} \end{equation}$

where

$\boldsymbol{v}^e$ is the velocity with respect to the Earth in Earth-fixed frame coordinates,
$\boldsymbol{f}_{ib}^b$ is the specific force vector as measured by a triad of accelerometers and resolved into body frame coordinates
$\boldsymbol{\omega}_{ie}^e = \begin{bmatrix} 0 & 0 & \omega_{ie} \end{bmatrix}^T$ is the turn rate of the Earth expressed in Earth-fixed frame coordinates
$\boldsymbol{\gamma}^e$ the local gravitation vector in Earth-fixed frame coordinates (caused by effects of mass attraction)

see

[47] Titterton, ch. 3.5.2, eq. 3.21, p. 29
[24] Jekeli, ch. 4.3.3, eq. 4.82, p. 126
[17] Groves, ch. 5.3.3, eq. 5.35, p. 175

Position

$\begin{equation} \label{eq:eq-ImuIntegrator-Mechanization-e-Position} \boldsymbol{\dotup{x}}^e = \boldsymbol{v}^e \end{equation}$

see

[47] Titterton, ch. 3.5.2, p. 28ff
[24] Jekeli, ch. 4.3.3, eq. 4.82, p. 126
[17] Groves, ch. 5.3.4, eq. 5.37, p. 175

Numerical Integration

Runge Kutta 4th order

The universal Runge Kutta 4th order equations are

$\begin{equation} \label{eq:eq-ImuIntegrator-e-RK4} \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} \label{eq:eq-ImuIntegrator-e-ODE} f =\\ \begin{pmatrix} \dotup{x} \\ \dotup{y} \\ \dotup{z} \\ \dotup{v}_x \\ \dotup{v}_y \\ \dotup{v}_z \\ \dotup{b} \\ \dotup{c} \\ \dotup{d} \\ \dotup{a} \end{pmatrix} = \begin{pmatrix} v_x \\ v_y \\ v_z \\ \\ \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) \\ \\ \frac{1}{2} \begin{bmatrix} 0 & \omega_{eb,z}^b & -\omega_{eb,y}^b & \omega_{eb,x}^b \\ -\omega_{eb,z}^b & 0 & \omega_{eb,x}^b & \omega_{eb,y}^b \\ \omega_{eb,y}^b & -\omega_{eb,x}^b & 0 & \omega_{eb,z}^b \\ -\omega_{eb,x}^b & -\omega_{eb,y}^b & -\omega_{eb,z}^b & 0 \end{bmatrix} \begin{bmatrix} b \\ c \\ d \\ a \end{bmatrix} \\ \end{pmatrix} \end{equation}$

Taking only the position and velocity terms and simplifying we get

$\begin{equation} \label{eq:eq-ImuIntegrator-e-ODE-posvel} f = \begin{pmatrix} \boldsymbol{\dotup{x}} \\ \boldsymbol{\dotup{v}} \end{pmatrix} = \begin{pmatrix} \boldsymbol{v} \\ \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) \\ \end{pmatrix} = \begin{pmatrix} \boldsymbol{v} \\ \boldsymbol{a} (\mathbf{C}_b^e, \boldsymbol{v}^e, \mathbf{x}^e) \\ \end{pmatrix} \approx \begin{pmatrix} \boldsymbol{v} \\ \boldsymbol{a} (\mathbf{x}^e) \\ \end{pmatrix} \end{equation}$

Which leads to the terms

$\begin{equation} \label{eq:eq-ImuIntegrator-e-RK4-ODE} \begin{aligned} k_1 &= \begin{pmatrix} k_{1_{x}} \\ k_{1_{v}} \end{pmatrix} = f(y_{i-1}, t_{i-1}) = \begin{pmatrix} \boldsymbol{v}_{i-1} \boldsymbol{a} (\mathbf{x}^e_{i-1}) \\ \end{pmatrix} \\ k_2 &= \begin{pmatrix} k_{2_{x}} \\ k_{2_{v}} \end{pmatrix} = f(y_{i-1} + \frac{h}{2} k_1, t_{i-1} + \frac{h}{2}) = \begin{pmatrix} \boldsymbol{v}_{i-1} + \frac{h}{2} k_{1_{v}} \boldsymbol{a} (\mathbf{x}^e_{i-1} + \frac{h}{2} k_{1_{x}}) \\ \end{pmatrix} \\ k_3 &= \dots \end{aligned} \end{equation}$

These terms coincide with [49] Voesenek, eq. 9,10

Table of Contents