 |
0.2.0
|
|
0.2.0
|
\begin{equation} \mathbf{\dot{C}}_b^n = \mathbf{C}_b^n \boldsymbol{\Omega}_{nb}^b \end{equation}
where
see
The body rate with respect to the navigation frame \( \boldsymbol{\omega}_{nb}^b \) can be expressed as
\begin{equation} \boldsymbol{\omega}_{nb}^b = \boldsymbol{\omega}_{ib}^b - \mathbf{C}_n^b [\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n] \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-n-Attitude-DCM
\begin{equation} \mathbf{\dot{C}}_b^n = \mathbf{C}_b^n \boldsymbol{\Omega}_{ib}^b - (\boldsymbol{\Omega}_{ie}^n + \boldsymbol{\Omega}_{en}^n) \mathbf{C}_b^n \end{equation}
The gimbal angles \( R \) (roll), \( P \) (pitch), \( Y \) (yaw) are related to the body rates as follows:
\begin{equation} \begin{bmatrix} \omega_{nb,x}^b \\ \omega_{nb,y}^b \\ \omega_{nb,z}^b \end{bmatrix} = \begin{bmatrix} \dot{R} \\ 0 \\ 0 \end{bmatrix} + \mathbf{C_3} \begin{bmatrix} 0 \\ \dot{P} \\ 0 \end{bmatrix} + \mathbf{C_3} \mathbf{C_2} \begin{bmatrix} 0 \\ 0 \\ \dot{Y} \end{bmatrix} \end{equation}
where
\begin{equation} \begin{aligned} \dot{R} &= (\omega_{nb,y}^b \sin{R} + \omega_{nb,z}^b \cos{R}) \tan{P} + \omega_{nb,x}^b \\ \dot{P} &= \omega_{nb,y}^b \cos{R} - \omega_{nb,z}^b \sin{R} \\ \dot{Y} &= (\omega_{nb,y}^b \sin{R} + \omega_{nb,z}^b \cos{R}) \sec{P} \\ \end{aligned} \end{equation}
see
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^n = 0.5 \mathbf{q}_b^n \cdot \mathbf{p}_{nb}^b \end{equation}
where \( \mathbf{p}_{nb}^b = \begin{pmatrix} 0 & \boldsymbol{\omega}_{nb}^b \end{pmatrix}^T = \begin{pmatrix} 0 & \omega_{nb,x}^b & \omega_{nb,y}^b & \omega_{nb,z}^b \end{pmatrix}^T \)
This can be written in matrix form as
\begin{equation} \mathbf{\dot{q}}_b^n = \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_{nb,x}^b \\ \omega_{nb,y}^b \\ \omega_{nb,z}^b \end{bmatrix} \end{equation}
\begin{equation} \begin{aligned} \dot{a} &= -0.5 (b \omega_{nb,x}^b + c \omega_{nb,y}^b + d \omega_{nb,z}^b) &= 0.5 ( 0 \cdot a - \omega_{nb,x}^b b - \omega_{nb,y}^b c - \omega_{nb,z}^b d) \\ \dot{b} &= 0.5 (a \omega_{nb,x}^b - d \omega_{nb,y}^b + c \omega_{nb,z}^b) &= 0.5 (\omega_{nb,x}^b a + 0 \cdot b + \omega_{nb,z}^b c - \omega_{nb,y}^b d) \\ \dot{c} &= 0.5 (d \omega_{nb,x}^b + a \omega_{nb,y}^b - b \omega_{nb,z}^b) &= 0.5 (\omega_{nb,y}^b a - \omega_{nb,z}^b b + 0 \cdot c + \omega_{nb,x}^b d) \\ \dot{d} &= -0.5 (c \omega_{nb,x}^b - b \omega_{nb,y}^b - a \omega_{nb,z}^b) &= 0.5 (\omega_{nb,z}^b a + \omega_{nb,y}^b b - \omega_{nb,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^n = \begin{bmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{nb,x}^b & -\omega_{nb,y}^b & -\omega_{nb,z}^b \\ \omega_{nb,x}^b & 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b \\ \omega_{nb,y}^b & -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b \\ \omega_{nb,z}^b & \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \\ d \end{bmatrix} \end{equation}
see
The derivative of the velocity in local-navigation coordinates can be expressed as
\begin{equation} \boldsymbol{\dot{v}}^n = \overbrace{\boldsymbol{f}^n}^{\text{measured}} -\ \underbrace{(2 \boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \boldsymbol{v}^n}_{\text{coriolis acceleration}} +\ \overbrace{\boldsymbol{\gamma}^n}^{\text{gravitation}} -\ \mathbf{C}_e^n \cdot \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} \begin{aligned} \dot{\phi} &= \frac{v_N}{R_N + h} \\ \dot{\lambda} &= \frac{v_E}{(R_E + h) \cos{\phi}} \\ \dot{h} &= -v_D \end{aligned} \end{equation}
where
see
Different books have different forms of the quaternion propagation, therefore this shall be discussed here.
The matrix form for the propagation is (see [45] Titterton, ch. 11.2.5, eq. 11.33-11.35, p. 319)
\begin{equation} \mathbf{\dot{q}}_b^n = \begin{bmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{nb,x}^b & -\omega_{nb,y}^b & -\omega_{nb,z}^b \\ \omega_{nb,x}^b & 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b \\ \omega_{nb,y}^b & -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b \\ \omega_{nb,z}^b & \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \\ d \end{bmatrix} \end{equation}
which can be converted into a direction cosine matrix with
\begin{equation} \mathbf{C}_b^n = \begin{bmatrix} (a^2 + b^2 - c^2 - d^2) & 2(bc - ad) & 2(bd + ac) \\ 2(bc + ad) & (a^2 - b^2 + c^2 - d^2) & 2(cd - ab) \\ 2(bd - ac) & 2(cd + ab) & (a^2 - b^2 - c^2 + d^2) \end{bmatrix} \end{equation}
the quaternion is defined as (see [45] Titterton, ch. 3.6.4.2, eq. 3.57, p. 43)
\begin{equation} \mathbf{r}^n = \mathbf{q} \mathbf{r}^b \mathbf{q}^* \Rightarrow \mathbf{q} = \mathbf{q}_b^n \end{equation}
Jekeli defines the equations from the body frame to an arbitrary frame, which in our case is the n-frame. Therefore his equations become ([22] Jekeli, ch. 4.2.3.1, eq. 4.20, p. 114)
\begin{equation} \begin{aligned} \mathbf{\dot{C}}_b^a &= \mathbf{C}_b^a \boldsymbol{\Omega}_{ab}^b \\ \mathbf{\dot{C}}_b^n &= \mathbf{C}_b^n \boldsymbol{\Omega}_{nb}^b \end{aligned} \end{equation}
The quaternion is defined by Jekeli over the direction cosine matrix which is ([22] Jekeli, ch. 4.2.3.1, eq. 4.22, p. 114)
\begin{equation} \begin{aligned} \mathbf{C}_b^n &= \begin{bmatrix} (a^2 + b^2 - c^2 - d^2) & 2(bc + ad) & 2(bd - ac) \\ 2(bc - ad) & (a^2 - b^2 + c^2 - d^2) & 2(cd + ab) \\ 2(bd + ac) & 2(cd - ab) & (a^2 - b^2 - c^2 + d^2) \end{bmatrix} \\ &= \begin{bmatrix} ((a^*)^2 + (-b^*)^2 - (-c^*)^2 - (-d^*)^2) & 2((-b^*)(-c^*) + (a^*)(-d^*)) & 2((-b^*)(-d^*) - (a^*)(-c^*)) \\ 2((-b^*)(-c^*) - (a^*)(-d^*)) & ((a^*)^2 - (-b^*)^2 + (-c^*)^2 - (-d^*)^2) & 2((-c^*)(-d^*) + (a^*)(-b^*)) \\ 2((-b^*)(-d^*) + (a^*)(-c^*)) & 2((-c^*)(-d^*) - (a^*)(-b^*)) & ((a^*)^2 - (-b^*)^2 - (-c^*)^2 + (-d^*)^2) \end{bmatrix} \\ &= \begin{bmatrix} ((a^*)^2 + (b^*)^2 - (c^*)^2 - (d^*)^2) & 2(b^* c^* - a^* d^*) & 2(b^* d^* + a^* c^*) \\ 2(b^* c^* + a^* d^*) & ((a^*)^2 - (b^*)^2 + (c^*)^2 - (d^*)^2) & 2(c^* d^* - a^* b^*) \\ 2(b^* d^* - a^* c^*) & 2(c^* d^* + a^* b^*) & ((a^*)^2 - (b^*)^2 - (c^*)^2 + (d^*)^2) \end{bmatrix} \\ \end{aligned} \end{equation}
Comparing Jekeli's DCM eq-ImuIntegrator-Mechanization-n-Attitude-Quaternion-DCM-Jekeli with Titterton's eq-ImuIntegrator-Mechanization-n-Attitude-Quaternion-DCM-Titterton, it can easily be shown, that the quaternion is the complex conjugate (see [22] Jekeli, ch. 4.2.3.1, eq. 4.21,4.23, p. 114f)
\begin{equation} \mathbf{\dot{q}} = \begin{bmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & \omega_{nb,x}^b & \omega_{nb,y}^b & \omega_{nb,z}^b \\ -\omega_{nb,x}^b & 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b \\ -\omega_{nb,y}^b & -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b \\ -\omega_{nb,z}^b & \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \\ d \end{bmatrix} \end{equation}
Replacing the complex conjugated quaternion leads to the same equation as Titterton's eq-ImuIntegrator-Mechanization-n-Attitude-Quaternion-matrix-Titterton-2
\begin{equation} \begin{aligned} \mathbf{\dot{q}}_b^n &= \begin{bmatrix} \dot{a}^* \\ -\dot{b}^* \\ -\dot{c}^* \\ -\dot{d}^* \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & \omega_{nb,x}^b & \omega_{nb,y}^b & \omega_{nb,z}^b \\ -\omega_{nb,x}^b & 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b \\ -\omega_{nb,y}^b & -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b \\ -\omega_{nb,z}^b & \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a^* \\ -b^* \\ -c^* \\ -d^* \end{bmatrix} \\ &= \begin{bmatrix} \dot{a}^* \\ -\dot{b}^* \\ -\dot{c}^* \\ -\dot{d}^* \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{nb,x}^b & -\omega_{nb,y}^b & -\omega_{nb,z}^b \\ -\omega_{nb,x}^b & 0 & -\omega_{nb,z}^b & \omega_{nb,y}^b \\ -\omega_{nb,y}^b & \omega_{nb,z}^b & 0 & -\omega_{nb,x}^b \\ -\omega_{nb,z}^b & -\omega_{nb,y}^b & \omega_{nb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a^* \\ b^* \\ c^* \\ d^* \end{bmatrix} \\ &= \begin{bmatrix} \dot{a}^* \\ \dot{b}^* \\ \dot{c}^* \\ \dot{d}^* \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{nb,x}^b & -\omega_{nb,y}^b & -\omega_{nb,z}^b \\ \omega_{nb,x}^b & 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b \\ \omega_{nb,y}^b & -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b \\ \omega_{nb,z}^b & \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 \end{bmatrix} \begin{bmatrix} a^* \\ b^* \\ c^* \\ d^* \end{bmatrix} \end{aligned} \end{equation}
Groves defines his Quaternion update in two arbitrary frames ([17] Groves, Appendix E, ch. E.6.2, eq. E.32, p. E-12)
\begin{equation} \mathbf{\dot{q}}_\beta^\alpha = \begin{bmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{\beta\alpha,x}^\alpha & -\omega_{\beta\alpha,y}^\alpha & -\omega_{\beta\alpha,z}^\alpha \\ \omega_{\beta\alpha,x}^\alpha & 0 & \omega_{\beta\alpha,z}^\alpha & -\omega_{\beta\alpha,y}^\alpha \\ \omega_{\beta\alpha,y}^\alpha & -\omega_{\beta\alpha,z}^\alpha & 0 & \omega_{\beta\alpha,x}^\alpha \\ \omega_{\beta\alpha,z}^\alpha & \omega_{\beta\alpha,y}^\alpha & -\omega_{\beta\alpha,x}^\alpha & 0 \end{bmatrix} \mathbf{q}_\beta^\alpha = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{\beta\alpha,x}^\beta & -\omega_{\beta\alpha,y}^\beta & -\omega_{\beta\alpha,z}^\beta \\ \omega_{\beta\alpha,x}^\beta & 0 & -\omega_{\beta\alpha,z}^\beta & \omega_{\beta\alpha,y}^\beta \\ \omega_{\beta\alpha,y}^\beta & \omega_{\beta\alpha,z}^\beta & 0 & -\omega_{\beta\alpha,x}^\beta \\ \omega_{\beta\alpha,z}^\beta & -\omega_{\beta\alpha,y}^\beta & \omega_{\beta\alpha,x}^\beta & 0 \end{bmatrix} \mathbf{q}_\beta^\alpha \end{equation}
This can be expresed for the body rate with respect to the navigation frame in body frame coordinates \( \boldsymbol{\omega}_{nb}^b \)
\begin{equation} \mathbf{\dot{q}}_n^b = \begin{bmatrix} \dot{a} \\ \dot{b} \\ \dot{c} \\ \dot{d} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_{nb,x}^b & -\omega_{nb,y}^b & -\omega_{nb,z}^b \\ \omega_{nb,x}^b & 0 & \omega_{nb,z}^b & -\omega_{nb,y}^b \\ \omega_{nb,y}^b & -\omega_{nb,z}^b & 0 & \omega_{nb,x}^b \\ \omega_{nb,z}^b & \omega_{nb,y}^b & -\omega_{nb,x}^b & 0 \end{bmatrix} \mathbf{q}_n^b \end{equation}
which is the same as Titterton and Jekeli
1.10.0