2. Quaternion

A quaternion is a composition of four elements, which are used for the representation of attitude between two frames. Sometimes it is represented by a $4 \times 1$ unit vector (noncommutative) as given below (Jekeli, Reference Jekeli2012):

(1)\begin{equation}\boldsymbol{q} = \left[ {\begin{array}{{c}} s\\ \boldsymbol{V} \end{array}} \right]\end{equation}

In this representation method, s is a scalar, and $\boldsymbol{V}$ is a $3 \times 1$ vector. The conjugate of a quaternion is defined as:

(2)\begin{equation}{\boldsymbol{q}^\ast } = \left[ {\begin{array}{{c}} s\\ { - \boldsymbol{V}} \end{array}} \right]\end{equation}

The multiplication of the two quaternions $ {\boldsymbol{q}_1}$ and ${\boldsymbol{q}_2}$ is represented by $ {\boldsymbol{q}_1} \otimes {\boldsymbol{q}_2}$, and in the matrix multiplication form, it is written as:

(3)\begin{equation}{\boldsymbol{q}_1} \otimes {\boldsymbol{q}_2} = {\boldsymbol{q}_2} \odot {\boldsymbol{q}_1} = {[{{\boldsymbol{q}_1}} ]_L}{\boldsymbol{q}_2} = {[{{\boldsymbol{q}_2}} ]_R}{\boldsymbol{q}_1}\end{equation}

where ${[{{\boldsymbol{q}_1}} ]_L}$ and ${[{{\boldsymbol{q}_2}} ]_R}$ are $4 \times 4$ matrices, as shown below:

(4)\begin{equation} {{{[{{\boldsymbol{q}_1}} ]}_L} = \left[ {\begin{matrix} {{s_1}}&{ - \boldsymbol{V}_1^T}\\ {{\boldsymbol{V}_1}}&{{s_1}\boldsymbol{I} + [{{\boldsymbol{V}_1} \times } ]} \end{matrix}} \right], \quad {{[{{\boldsymbol{q}_2}} ]}_R} = \left[ {\begin{matrix} {{s_2}}&{ - \boldsymbol{V}_2^T}\\ {{\boldsymbol{V}_2}}&{{s_2}\boldsymbol{I} - [{{\boldsymbol{V}_2} \times } ]} \end{matrix}} \right]} \end{equation}

where $[{\boldsymbol{V} \times } ]$ is skew-symmetric $3 \times 3$ and is named cross product matrix of vector $\boldsymbol{V}$. The indices i and b are used for representation of inertial and body frames. The angular velocity of the body frame relative to the inertial frame, which is expressed in the body frame, is denoted by $ {\boldsymbol{\omega }^b}$. For the representation of a vector as a quaternion, we use the notation $ {\boldsymbol{\omega }^{b^{\prime}}} = \left[ {\begin{smallmatrix} 0\\ {{\boldsymbol{\omega }^b}} \end{smallmatrix}} \right]$. The projection of that vector onto the inertial frame using quaternion is represented by (Jekeli, Reference Jekeli2012):

(5)\begin{equation} {{\boldsymbol{\omega }^{i^{\prime}}} = \boldsymbol{q} \otimes {\boldsymbol{\omega }^{b^{\prime}}} \otimes {\boldsymbol{q}^\ast } = \left[ {\begin{matrix} 0\\ {\boldsymbol{C}_b^i{\boldsymbol{\omega }^b}} \end{matrix}} \right]} \end{equation}

where $\boldsymbol{C}_b^i$ is the rotation matrix from $b$-frame to $i$-frame while the rotation matrix is a function of the quaternion, as given below (Jekeli, Reference Jekeli2012):

(6)\begin{equation} \left[ \begin{matrix} 1&{{\textbf{0}^T}}\\ \textbf{0}&{\boldsymbol{C}_b^i} \end{matrix}\right] = {{[\boldsymbol{q} ]}_L}{{[{{\boldsymbol{q}^\ast }} ]}_R} \end{equation}

The differential equation of quaternion for attitude dynamics is (Savage, Reference Savage1998a, Reference Savage1998b):

(7)\begin{equation}{\dot{\boldsymbol{q}} = \dfrac{1}{2}\boldsymbol{q} \otimes {\boldsymbol{\omega }^{b^{\prime}}}}\end{equation}

And the equivalent equation for the rotation matrix is (Savage, Reference Savage1998a, Reference Savage1998b):

(8)\begin{equation}\dot{\boldsymbol{C}}_b^i = \boldsymbol{C}_b^i[{{\boldsymbol{\omega }^b} \times } ]\end{equation}

The quaternion as a function of rotation vector is (Jekeli, Reference Jekeli2012):

(9)\begin{equation} {\boldsymbol{q} = \left[ {\begin{matrix} {\textrm{cos}({\varphi /2} )}\\ {\textrm{sin}({\varphi /2} )\hat{\boldsymbol{n}}} \end{matrix}} \right]} \end{equation}

where $\varphi $ is rotation angle, and $\hat{\boldsymbol{n}}$ is a unit vector that shows the direction of rotation vector.

In real applications, the angular velocity is measured with high sampling frequency as much as possible and an approximated polynomial is fitted to it. In some cases, the output of sensors is incremental angles (integral of the angular velocity is measured in the incremental time) (Wu and Yan, Reference Wu and Yan2019). Fitting a polynomial on high frequency sampled data has some benefits because it is able to reduce the white noise of sensors and describes the angular velocity as a continuous function of time. Suppose the given polynomial describes the angular velocity between times zero and $ t$ (Wu and Yan, Reference Wu and Yan2019).

(10)\begin{equation}{\boldsymbol{\omega }^b} = 2({{\boldsymbol{w}_0} + {\boldsymbol{w}_1}t + \ldots + {\boldsymbol{w}_n}{t^n}} )\end{equation}

where t is time and ${\boldsymbol{w}_0}, \ldots ,{\boldsymbol{w}_n}$ are the coefficient vectors of the fitted polynomial. When there are more than $m \ge n + 1$ data, the least square (LS) estimation method helps us to calculate the unknown parameters ${\boldsymbol{w}_0}, \ldots ,{\boldsymbol{w}_n} $ as given below (Wu and Litmanovich, Reference Wu and Litmanovich2019; Wu and Yan, Reference Wu and Yan2019):

(11)\begin{equation}\left[ {\begin{matrix} {{\omega^b}{{({{t_0}} )}^T}}\\ {{\omega^b}{{({{t_1}} )}^T}}\\ \ldots \\ {{\omega^b}{{({{t_m}} )}^T}} \end{matrix}} \right] = 2\left[ \begin{matrix} 1&{{t_0}}&{\ldots }&{t_0^n}\\ 1&{{t_1}}& \ldots &{t_1^n}\\ \ldots & \ldots & \ldots & \ldots \\ 1&{{t_m}}& \ldots &{t_m^n} \end{matrix} \right]\left[ {\begin{matrix} {w_0^T}\\ {w_1^T}\\ \ldots \\ {w_n^T} \end{matrix}} \right]\end{equation}

where, ${\boldsymbol{\omega }^b}({{t_0}} ), {\boldsymbol{\omega }^b}({{t_1}} ),\ldots ,{\boldsymbol{\omega }^b}({{t_m}} )$ are sample data in times $ {t_0},{t_1}, \ldots ,{t_m}$. When n is high, the coefficient matrix in Equation (11) will be singular, then QR-factorisation or singular value decomposition are useful methods to solve this problem. When there are correlations between gyros’ noises, the weighted LS of errors as a goal function is more efficient. In that condition, the weighting is selected as an inverse of the covariance matrix. Here we avoid discussing this issue because it has been discussed in many classic texts.

3. Attitude reconstruction using power series

Using notation Equation (4) in Equation (7), the differential equation of quaternions in the matrix form is:

(12)\begin{equation}\dot{\boldsymbol{q}} = {[{0.5{\boldsymbol{\omega }^{b^{\prime}}}} ]_R}\boldsymbol{q}\end{equation}

Substituting Equation (10) in Equation (12), we obtain the following results:

(13)\begin{equation}\dot{\boldsymbol{q}} = \boldsymbol{\Omega }(t )\boldsymbol{q}, \boldsymbol{\Omega }(t )= ({{\boldsymbol{\Omega }_0} + {\boldsymbol{\Omega }_1}t + \ldots + {\boldsymbol{\Omega }_n}{t^n}} ),\boldsymbol{q}(0 )= {\boldsymbol{q}_0}\end{equation}

where n is the degree of angular velocity's polynomial and the matrices $ {\boldsymbol{\Omega }_0}$, ${\boldsymbol{\Omega }_1}, \ldots , {\boldsymbol{\Omega }_n}$ are constant and known matrices with $4 \times 4\ \textrm{dimensions}$, and they are defined as follows:

(14)\begin{equation}{\boldsymbol{\Omega }_i} = {[{{{\boldsymbol{w^{\prime}}}_i}} ]_R}, i = 0,1, \ldots ,n\end{equation}

Now, we examine a power series solution for Equation (13) with a degree of the infinite as:

(15)\begin{equation}\boldsymbol{q}(t )= {\textbf{a}_0} + {\textbf{a}_1}t + \ldots + {\textbf{a}_i}{t^i} + \ldots \end{equation}

where ${\textbf{a}_0}, {\textbf{a}_1}, \ldots , {\textbf{a}_i}, \ldots $ are constant and unknown vectors with the dimensions $ 4 \times 1$. After substituting Equation (15) in Equation (13) and rearranging the coefficient $ {t^i}, (i = 0,1, \ldots $), the following equations are extracted:

(16)\begin{equation}{\textbf{a}_i} = \left|\begin{array}{@{}ll} \dfrac{1}{i}\left(\displaystyle\sum_{j = 0}^{i - 1} \boldsymbol{\Omega }_j{\textbf{a}_{i - j - 1}}\right), & 1 \le i \le n\\ \dfrac{1}{i}\left(\displaystyle\sum_{j = 0}^n \boldsymbol{\Omega }_{j}{\textbf{a}_{i - j - 1}}\right), & i > n \end{array}\right.\end{equation}

In the above equation, one can determine the coefficient vectors $ {\textbf{a}_i}$, recursively. The constant vector ${\textbf{a}_0}$ is equal to initial condition ${\boldsymbol{q}_0}$. When $ i \le n$, by using the first n Equation (16), the coefficients ${\textbf{a}_1}, \ldots , {\textbf{a}_n}$ are determined. When $i > n,$ the following sequence can be extracted:

(17)\begin{equation}\left[ \begin{matrix} {{\textbf{a}_{i - n}}}\\ {{\textbf{a}_{i - n + 1}}}\\ {{\textbf{a}_{i - n + 2}}}\\ \ldots \\ {{\textbf{a}_{i - 1}}}\\ {{\textbf{a}_i}} \end{matrix} \right] = \left[\begin{matrix} \textbf{0}&\boldsymbol{I}&\textbf{0}& \ldots &\textbf{0}&\textbf{0}\\ \textbf{0}&\textbf{0}&\boldsymbol{I}& \ldots &\textbf{0}&\textbf{0}\\ \textbf{0}&\textbf{0}&\textbf{0}& \ldots &\textbf{0}&\textbf{0}\\ \ldots & \ldots & \ldots & \ldots & \ldots & \ldots \\ \textbf{0}&\textbf{0}&\textbf{0}& \ldots &\textbf{0}&\boldsymbol{I}\\ {\dfrac{1}{i}{\boldsymbol{\Omega }_n}}&{\dfrac{1}{i}{\boldsymbol{\Omega }_{n - 1}}}&{\dfrac{1}{i}{\boldsymbol{\Omega }_{n - 2}}}& \ldots &{\dfrac{1}{i}{\boldsymbol{\Omega }_1}}&{\dfrac{1}{i}{\boldsymbol{\Omega }_0}} \end{matrix}\right]\left[\begin{matrix} {{\textbf{a}_{i - n - 1}}}\\ {{\textbf{a}_{i - n}}}\\ {{\textbf{a}_{i - n + 1}}}\\ \ldots \\ {{\textbf{a}_{i - 2}}}\\ {{\textbf{a}_{i - 1}}} \end{matrix}\right]\end{equation}

The above system is a linear iteration varying discrete system and the state vector of the system is $ {\boldsymbol{\chi }_i} = {[{\boldsymbol{a}_{i - n}^T, \ldots ,\boldsymbol{a}_i^T} ]^T}$ which contains $n + 1$ coefficient vectors of the quaternions’ power series. If the system matrix of the above equation is represented by $ {\boldsymbol{{\cal M}}_{i - 1}}$, then Equation (17) is summarised by:

(18)\begin{equation}{\boldsymbol{\chi }_i} = {\boldsymbol{{\cal M}}_{i - 1}}{\boldsymbol{\chi }_{i - 1}}, \textrm{ }{\boldsymbol{\chi }_n} = {[{\boldsymbol{a}_0^T, \ldots ,\boldsymbol{a}_n^T} ]^T}, i > n\end{equation}

All blocks inside the matrix ${\boldsymbol{{\cal M}}_{i - 1}}$ are $4 \times 4$ matrices. The dimension of the matrix ${\boldsymbol{{\cal M}}_{i - 1}}$ is $4({n + 1} )\times 4({n + 1} )$. Again, we write Equation (15) as a sequence:

(19)\begin{equation}{\tilde{\boldsymbol{q}}_i} = {\tilde{\boldsymbol{q}}_{i - 1}} + {\boldsymbol{a}_i}{t^i}, {\tilde{\boldsymbol{q}}_n} = {\boldsymbol{a}_0} + \ldots {\boldsymbol{a}_n}{t^n} , i > n\end{equation}

Here ${\tilde{\boldsymbol{q}}_i}$ is an approximation of $\boldsymbol{q}(t )$ up to the $i$th term. Substituting ${\boldsymbol{a}_i}$ from the last equation of (17) into Equation (19), one can write:

(20)\begin{equation}{\tilde{\boldsymbol{q}}_i} = {\tilde{\boldsymbol{q}}_{i - 1}} + {\boldsymbol{H}_{i - 1}}{\boldsymbol{\chi }_{i - 1}}, \boldsymbol{ }{\boldsymbol{H}_{i - 1}} = {t^i}\left[ {\begin{matrix} {\dfrac{1}{i}{\boldsymbol{\Omega }_n}}& \ldots &{\dfrac{1}{i}{\boldsymbol{\Omega }_0}} \end{matrix}} \right], i > n \end{equation}

where ${\tilde{\boldsymbol{q}}_i}$ is the approximation of quaternion by power series when the truncation order is i. It is obvious that for a fixed time $ t < 1$ and initial condition $ {\tilde{\boldsymbol{q}}_n}$, sequence 20 will converge when:

(21)\begin{equation}\mathop {\lim }\limits_{i \to \infty } \boldsymbol{||}{\boldsymbol{H}_{i - 1}}{\boldsymbol{\chi }_{i - 1}}||= 0 \end{equation}

Equation (21) shows that when i approaches infinity, ${\boldsymbol{H}_\infty }$ will approach zero matrix because $\mathop {\textrm{ lim}}\limits_{ i \to \infty } {\raise0.7ex\hbox{${{t^i}}$} \!\mathord{/ {\vphantom {{{t^i}} i}} }\!\lower0.7ex\hbox{$i$}} = 0$. Furthermore, the norm of ${\boldsymbol{\chi }_\infty }$ is zero too (see theorem 1), then the stability of Equation (20) will be guaranteed.

Proof: The iteration varying linear system (18) is termed asymptotically stable with equilibrium point zero, if for any bounded initial condition $\textrm{ }{\boldsymbol{\chi }_n}$, we have $\mathop {\textrm{ lim}}\limits_{i \to \infty } \boldsymbol{||}{\boldsymbol{\chi }_i}\boldsymbol{||} = \boldsymbol{||}{\boldsymbol{\chi }_\infty }\boldsymbol{||} = 0$ then $ {\boldsymbol{\chi }_\infty } = \mathop {\textrm{ lim}}\limits_{i \to \infty } {\boldsymbol{\chi }_i} = 0$. Using Equation (18) one can write,

(22)\begin{equation}{\boldsymbol{\chi }_\infty } = ({\boldsymbol{{\cal M}}_\infty } \ldots {\boldsymbol{{\cal M}}_\infty } \ldots {\boldsymbol{{\cal M}}_{n + 1}}{\boldsymbol{{\cal M}}_n}){\boldsymbol{\chi }_n}\end{equation}

For asymptotical stability, it needs $ ||{\boldsymbol{{\cal M}}_\infty } \ldots {\boldsymbol{{\cal M}}_\infty } \ldots {\boldsymbol{{\cal M}}_{n + 1}}{\boldsymbol{{\cal M}}_n}||\to 0$ as $i \to \infty $. Here we consider norm two $||.|{|_2}$ of a matrix, the maximum singular value has to be zero when the number of iterations approaches infinity. When the norm of ${\boldsymbol{{\cal M}}_\infty }$ is zero (see Appendix A), then the norm of ${\boldsymbol{\chi }_\infty }$ for any initial condition ${\boldsymbol{\chi }_n}$ is zero, consequently $ {\boldsymbol{\chi }_\infty } = 0$.□

As a result, the state Equation (18) is stable and it will converge to zero for any bounded initial condition. It is important to mention that the maximum value of $||{\boldsymbol{\chi }_i}||$ can be large but it will remain limited in this case. It can be easily checked that Equation (20) satisfies Equation (13) when i approaches infinity, $(d/dt){\tilde{\boldsymbol{q}}_\infty }(t )= \boldsymbol{\Omega }(t ){\tilde{\boldsymbol{q}}_\infty }(t )$ when there is no error in the initial condition $\textrm{ }{\boldsymbol{\chi }_n}$, that means ${\tilde{\boldsymbol{q}}_\infty }$ converges to true quaternion. It means ${\boldsymbol{a}_0}$ is known and other n−1 coefficients ${\boldsymbol{a}_1}, \ldots , {\boldsymbol{a}_{n - 1}}$ are calculated by the first equation of Equation (16) recursively. In the same way, we consider a power series for DCM as given below:

(23)\begin{equation}\boldsymbol{C}_b^i = {\boldsymbol{C}_0} + {\boldsymbol{C}_1}t + \ldots + {\boldsymbol{C}_i}{t^i} + \ldots \end{equation}

where ${\boldsymbol{C}_0}$ is the rotation matrix at time zero and ${\boldsymbol{C}_i}, i = 1,2, \ldots $ are constant matrices. Substituting Equation (23) into Equation (8), and comparing equivalent terms, it is possible to determine the coefficient matrices sequentially, which gives the following results:

(24)\begin{equation}{\boldsymbol{C}_i} = \left|{\begin{array}{{cc}} {\dfrac{1}{i}\left( {\mathop \sum \limits_{j = 0}^{i - 1} {\boldsymbol{C}_{i - j - 1}}{\boldsymbol{{\cal W}}_j}} \right),}&{1 \le i \le n}\\ {\dfrac{1}{i}\left( {\mathop \sum \limits_{j = 0}^n {\boldsymbol{C}_{i - j - 1}}{\boldsymbol{{\cal W}}_j}} \right),}&{i > n} \end{array}} \right.\end{equation}

where ${\boldsymbol{{\cal W}}_j} = \frac{1}{2}[{{\boldsymbol{w}_0} \times } ], i = 1, \ldots ,n$ are constant matrices, and the coefficient matrices ${\boldsymbol{C}_i}, i = 1,2, \ldots $ are determined by using Equation (24) recursively. In the above equation, when i goes to infinity, ${\boldsymbol{C}_i}$ will go to zero and Equation (24) will converge into a true DCM. Here, we are avoiding the repetition of the same results that are derived for quaternion expansion.

4. Attitude reconstruction using Taylor series

The well-known Taylor series has been used for approximation of an infinitely differentiable function for centuries. Some researchers have used this method for reconstructing quaternion form rate gyro output. Unfortunately, the Taylor series requires calculating the derivative of the quaternions infinite times, but due to the structure of quaternion dynamics, we can derive an explicit method for this purpose. Here we consider the Taylor expansion of the quaternion as a function of time as (Hairer et al., Reference Hairer, Nørsett and Wanner1993; Kunovský, Reference Kunovský2015):

(25)\begin{equation}\boldsymbol{q}(t )= {\boldsymbol{q}_0} + \mathop \sum \limits_{i = 1}^\infty \frac{{{t^i}}}{{i!}}{ {{\boldsymbol{q}^{(i )}}} |_{t = 0}}\end{equation}

where ${ {{\boldsymbol{q}^{(i )}}} |_{t = 0}}$ is the $i$th derivative of quaternion that evaluated at time zero. Using Equation (13) and the Leibniz formula we can write (Hairer et al., Reference Hairer, Nørsett and Wanner1993):

(26)\begin{equation}{\boldsymbol{q}^{(i )}} = \mathop \sum \limits_{j = 0}^{i - 1} { {\boldsymbol{C}_j^{i - 1}{\boldsymbol{\Omega }^{(j )}}} |_{t = 0}} { {{\boldsymbol{q}^{({i - j - 1} )}}} |_{t = 0}}\end{equation}

where $\boldsymbol{C}_j^{i - 1} = ((({i - 1} )!)/(j!({i - j - 1} )!))$ denotes the number of j combinations of $i - 1$ elements and ${ {{\boldsymbol{\Omega }^{(j )}}} |_{t = 0}}$ represents the $j$th derivative of $\boldsymbol{\Omega }(t )$ at time zero. Using the power series Equation (10), we can calculate the derivatives of angular velocity as:

(27)\begin{equation}\left\{ {\begin{array}{{c}} {{{ {{\boldsymbol{\Omega }^{(j )}}} |}_{t = 0}} = j!{\boldsymbol{\Omega }_j} j \le n}\\ {{{ {{\boldsymbol{\Omega }^{(j )}}} |}_{t = 0}} = 0 j > n} \end{array}} \right.\end{equation}

Substituting Equation (27) into Equation (26), we can write,

(28)\begin{equation}{ {{\boldsymbol{q}^{(i )}}} |_{t = 0}} = \left|\begin{array}{cc} \displaystyle{\mathop \sum \limits_{j = 0}^{i - 1} P_j^{i - 1}{\boldsymbol{\Omega }_j}{{ {{\boldsymbol{q}^{({i - j - 1} )}}} |}_{t = 0}}}&{1 \le i \le n}\\ \displaystyle{\mathop \sum \limits_{j = 0}^n P_j^{i - 1}{\boldsymbol{\Omega }_j}{{ {{\boldsymbol{q}^{({i - j - 1} )}}} |}_{t = 0}}}&{i < n} \end{array} \right.\end{equation}

where $P_j^{i - 1} = (((i - 1)!)/((i - j - 1)!))$ is the number of permutations of $i - 1$ objects taken $j $ at a time. Equation (28) is used to calculate all the derivatives of quaternion recursively at time zero. For simplicity, we will neglect ${ {} |_{t = 0}}$ in $ { {{q^{(i )}}} |_{t = 0}}$, as all the derivatives are evaluated at time zero. Then Equation (28) becomes,

(29)\begin{equation}{\boldsymbol{q}^{(i )}} = \left|\begin{array}{cc} \displaystyle{\mathop \sum \limits_{j = 0}^{i - 1} \boldsymbol{P}_j^{i - 1}{\boldsymbol{\Omega }_j}{\boldsymbol{q}^{({i - 1 - j} )}}}&{1 \le i \le n}\\ \displaystyle{\mathop \sum \limits_{j = 0}^n \boldsymbol{P}_j^{i - 1}{\boldsymbol{\Omega }_j}{\boldsymbol{q}^{({i - 1 - j} )}}}&{i < n} \end{array} \right.\end{equation}

Rearranging Equation (29) in the recursive form, we obtain:

(30)\begin{equation}\begin{array}{{c}} {\begin{array}{{cc}} {{\boldsymbol{\Upsilon }_i} = {{\boldsymbol{\mathcal N}}_{i - 1}}{\boldsymbol{\Upsilon }_{i - 1}},}&{{\boldsymbol{\Upsilon }_i} = {{\left[ {\begin{array}{{ccc}} {{{({{\boldsymbol{q}^{({i - n} )}}} )}^T}}& \ldots &{{{({{\boldsymbol{q}^{(i )}}} )}^T}} \end{array}} \right]}^T}} \end{array}}\\ {{{\boldsymbol{\mathcal N}}_{i - 1}} = \left[ {\begin{array}{{cccccc}} \boldsymbol{0}&\boldsymbol{I}&\boldsymbol{0}& \ldots &\boldsymbol{0}&\boldsymbol{0}\\ \boldsymbol{0}&\boldsymbol{0}&\boldsymbol{I}& \ldots &\boldsymbol{0}&\boldsymbol{0}\\ \boldsymbol{0}&\boldsymbol{0}&\boldsymbol{0}& \ldots &\boldsymbol{0}&\boldsymbol{0}\\ \ldots & \ldots & \ldots & \ldots & \ldots & \ldots \\ 0 & 0 & 0 & \ldots & 0 & I \\ {\boldsymbol{P}_n^{i - 1}{\boldsymbol{\Omega }_n}}&{\boldsymbol{P}_{n - 1}^{i - 1}{\boldsymbol{\Omega }_{n - 1}}}&{\boldsymbol{P}_{n - 2}^{i - 1}{\boldsymbol{\Omega }_{n - 2}}}& \ldots &{\boldsymbol{P}_1^{i - 1}{\boldsymbol{\Omega }_1}}&{\boldsymbol{P}_0^{i - 1}{\boldsymbol{\Omega }_0}} \end{array}} \right]} \end{array}\end{equation}

In the above equation, when $i > n,$ it is possible to calculate the time derivatives of the quaternion in a recursive form. The system matrix ${\boldsymbol{{\cal N}}_{i - 1}}$ is an iteration varying matrix and it is used to calculate the $i$th derivative of quaternion at time zero from $n + 1$ last derivatives. Again, Equation (25) can be written in the recursive form:

(31)\begin{equation}\begin{aligned} {{{\bar{\boldsymbol{q}}}_i}(t )= {{\bar{\boldsymbol{q}}}_{i - 1}}(t )+ {\boldsymbol{L}_{i - 1}}{\boldsymbol{\Upsilon }_{i - 1}}}\\ {{\boldsymbol{L}_{i - 1}} = {t^i}{{\left[ {\begin{array}{{ccc}} {F_n^{i - 1}{\boldsymbol{\Omega }_n}}& \ldots &{F_0^{i - 1}{\boldsymbol{\Omega }_0}} \end{array}} \right]}^T}, i > n} \end{aligned}\end{equation}

where ${\bar{\boldsymbol{q}}_i}(t )$ is an approximation of quaternion at time $ t$, truncated up to i iterations, and $F_n^{i - 1} = ((1)/(i({i - n - 1} )!))$ is a constant scalar. For stability analysis of the system presented by Equation (30), matrices ${\boldsymbol{\Omega }_0}, \ldots , {\boldsymbol{\Omega }_n}$ are constant and skew-symmetric matrices, when i approaches infinity, all the permutation coefficients will go to infinity and then the norm of ${\boldsymbol{{\cal N}}_\infty }$ will become infinite. Consequently, Equation (30) is unstable, because when one element of a matrix goes to infinity, the norm of the matrix approaches infinity too. Then, contrary to the time series method, the norm of the quaternion's derivative for higher degrees grows unboundedly. It can be easily checked that Equation (31) satisfies Equation (13) when i approaches infinity, then $(d/dt){\bar{\boldsymbol{q}}_\infty }(t )= \boldsymbol{\Omega }(t ){\bar{\boldsymbol{q}}_\infty }(t )$ and that means ${\bar{\boldsymbol{q}}_\infty }$ will converge to true quaternion. Although the ${\bar{\boldsymbol{q}}_\infty }$ approaches true quaternion, the norm of ${\boldsymbol{\Upsilon }_{i - 1 }}$ grows unboundedly in Equation (31) but the norm of ${\boldsymbol{L}_{i - 1}}$ approaches zero. This issue will generate a problem in real implementations.

Again, like quaternions, we consider the Taylor series of rotation matrix as given below:

(32)\begin{equation}\boldsymbol{C}(t )= {\boldsymbol{C}_0} + \mathop \sum \limits_{i = 1}^\infty \frac{{{t^i}}}{{i!}}{ {{\boldsymbol{C}^{(i )}}} |_{t = 0}}\end{equation}

where ${ {{\boldsymbol{C}^{(i )}}} |_{t = 0}}$ is $i$th derivative of the rotation matrix at time zero. Using the Leibniz formula:

(33)\begin{equation}{\boldsymbol{C}^{(i )}} = \mathop \sum \limits_{j = 0}^{i - 1} C_j^{i - 1}{ {{\boldsymbol{C}^{({i - j - 1} )}}} |_{t = 0}}[{{{ {{\boldsymbol{\omega }^{(j )}}} |}_{t = 0}} \times } ]\end{equation}

where ${ {{\boldsymbol{C}^{({i - j - 1} )}}} |_{t = 0}}$ is the $({i - j - 1} )\textrm{th}$ derivative of the rotation matrix at time zero and $[{{{ {{\boldsymbol{\omega }^{(j )}}} |}_{t = 0}} \times } ]$ is a skew-symmetric matrix of the $j$th derivative of angular velocity at time zero.

The error of $ i$th order Taylor series of Equation (25) is (Hairer et al., Reference Hairer, Nørsett and Wanner1993; Wu and Yan, Reference Wu and Yan2019).

(34)\begin{equation}\begin{aligned} {{{\bar{\boldsymbol{q}}}_i}(t )= {\boldsymbol{q}_0} + \mathop \sum \limits_{j = 1}^i \dfrac{{{t^j}}}{{j!}}{{ {{\boldsymbol{q}^{(j )}}} |}_{t = 0}} + \boldsymbol{R}(\tau ), }\\ {{\boldsymbol{R}_i}(\tau )= \dfrac{{{\tau ^{i + 1}}}}{{({i + 1} )!}}{{ {{\boldsymbol{q}^{({i + 1} )}}} |}_{t = 0}}, \tau = [{0 t} ]} \end{aligned}\end{equation}

where $ {\boldsymbol{R}_i}(\tau )$ is the remainder terms after the $i$th term, which is the approximation error of the Taylor polynomial and, $\tau $ is between zero and $ t$. The Taylor theorem describes the asymptomatic behaviour of the remainder term, then (Hairer et al., Reference Hairer, Nørsett and Wanner1993):

(35)\begin{equation}\mathop {\lim }\limits_{i \to \infty } {\boldsymbol{R}_i}(\tau )= \boldsymbol{0}\end{equation}

Comparing Equation (31) with Equation (34), we can write:

(36)\begin{equation}{\boldsymbol{R}_i}(\tau )= {\tau ^i}\left[ {\begin{array}{{ccc}} {F_n^{i - 1}{\boldsymbol{\Omega }_n}}& \ldots &{F_0^{i - 1}{\boldsymbol{\Omega }_0}} \end{array}} \right]{\boldsymbol{\Upsilon }_{i - 1}}\end{equation}

Furthermore, it is obvious that:

(37)\begin{equation}\mathop {\lim F_j^{i - 1}}\limits_{i \to \infty } = 0, j = 0, \ldots ,n\end{equation}

Using Equation (35), the limit of all terms of Equation (36) has to be zero.

(38)\begin{equation}\mathop {\lim }\limits_{i \to \infty } F_j^{i - 1}{\boldsymbol{\Omega }_j}{\boldsymbol{q}^{({i - j - 1} )}} = 0, j = 0, \ldots ,n\end{equation}

where ${\boldsymbol{\Omega }_j}$ is a constant matrix and then the limit of $F_j^{i - 1}{q^{({i - j - 1} )}}$ for any j has to be zero, but the third term in Equation (40) goes to infinity while the first term goes to zero, when the i approaches infinity. Then the speed of $F_j^{i - 1}$ becomes zero, which is faster than the norm of ${q^{({i - j - 1} )}}$ going to infinity. Unfortunately, for the Taylor approximation we need to calculate the derivatives of the quaternion at time $ t$. In the case of quaternions of higher-order derivatives, there is unbounded growth due to the instability of Equation (30), when the angular velocity is non-zero. We can say that the implementation of the Taylor series is internally unstable and perhaps there is numerical degeneracy.

If we continue with the Taylor series with more terms to reduce the remainder term i in Equation (34), finally the one element of ${q^{(i )}}$ will be bigger than the $\beta $ or smaller than the $- \beta $. Here $\beta $ is the largest value and $- \beta $ has the lowest value specified to any variable. For example, when using double precision data type, the largest value is $ 1.79769({{{10}^{308}}} )$, but for single precision data it is $3.4028235({{{10}^{38}}} )$ (Muller et al., Reference Muller, Brisebarre, De Dinechin, Jeannerod, Lefevre, Melquiond, Revol, Stehlé and Torres2018). When $ {q^{(i )}} > \beta $, this condition can generate more errors and it needs to cut the Taylor series up to $i - 1$ terms. A simple way to control this degeneration is to consider i terms of Equation (31), in which the norm of ${q^{(i )}} $is lower than the $ \beta $, before adding $i$th term to the Taylor series.

5. Simulation study

To study the performances of algorithms and compare the results with recently published algorithms, we consider the classic coning motion that has been mentioned in the literature (Wu, Reference Wu2018; Wu et al., Reference Wu, Cai and Truong2018; Wu and Yan, Reference Wu and Yan2019). The angular velocity of the classical coning motion is, ${\boldsymbol{\omega }_{\textrm{con}}} = {\omega _0}{\left[ {\begin{array}{{ccc}} { - 2\textrm{si}{\textrm{n}^2}\tfrac{\alpha }{2}}&{ - \textrm{sin}\alpha \textrm{sin}{\omega_0}t}&{\textrm{sin}\alpha \textrm{cos}{\omega_0}t} \end{array}} \right]^T}$, and the error-less quaternion for coning motion is (Wu, Reference Wu2018; Wu et al., Reference Wu, Cai and Truong2018; Wu and Yan, Reference Wu and Yan2019), ${\boldsymbol{q}_{\textrm{con}}} = {\left[ {\begin{array}{{cccc}} {\textrm{cos}\tfrac{\alpha }{2}}&0&{\textrm{sin}\tfrac{\alpha }{2}\textrm{ cos}{\omega_0}t}&{\textrm{sin}\tfrac{\alpha }{2} \textrm{sin}{\omega_0}t} \end{array}} \right]^T}$. Here $\alpha $ is the coning angle, and ${\omega _0} = 2\pi {f_0}$ is the angular velocity of the coning motion, while ${f_0}$ is the coning frequency in Hz. When the true quaternion is $\boldsymbol{q}, $ and $\hat{\boldsymbol{q}}$ is the calculated quaternion, the following equations will be used for the calculation of the error of the rotation angle. Here, we use the last three elements of the quaternion because the first element of the error quaternion ${\boldsymbol{q}_e} $ is close to one, and the cosine function is not sensitive at zero, then

(39)\begin{equation}\varepsilon = 2\textrm{si}{\textrm{n}^{ - 1}}({{\boldsymbol{q}_e} ({2:4} )} ),{\boldsymbol{q}_e} = [{{\boldsymbol{q}^\ast } \otimes \hat{\boldsymbol{q}}} ]\end{equation}

where ${\boldsymbol{q}_e} ({2:4} )$ is vector part of the error's quaternion. When we use the DCM, the attitude error can be calculated as follows:

(40)\begin{equation}\varepsilon = \textrm{co}{\textrm{s}^{ - 1}}[{0.5({\textrm{trace}({{\boldsymbol{C}_e}} )- 1} )} ],{\boldsymbol{C}_e} = \boldsymbol{C}{\hat{\boldsymbol{C}}^T}\end{equation}

Again, due to the non-sensitivity of cosine function in zero, Equation (40) is not effective. Instead, we can approximate the error of rotation angle from ${\boldsymbol{C}_e}$ as:

(41)\begin{equation}\boldsymbol{S} = {\boldsymbol{C}_e} - \boldsymbol{I}, \quad \boldsymbol{S} \cong [{\boldsymbol{s} \times } ], \quad \varepsilon = \boldsymbol{s}\end{equation}

where, when the rotation angle is closer to zero, the matrix $\boldsymbol{S}$ is almost a skew-symmetric matrix. Now, we will compare the performances of the four methods as follows:

• – QuatPow: Quaternion approximation using power series,

• – QuatTay: Quaternion approximation using Taylor series,

• – DCMPow: DCM approximation using power series, and

• – DCMTay: DCM approximation using Taylor series.

We used single and double precision implementations for implementing the above four methods. For any method in real applications, the processor has limited bits for implementation of any variable type. For example, any single precision number is between $1.1755({{{10}^{ - 38}}} )$ and $3.4028({{{10}^{ + 38}}} )$; in case of double precision, it is between $2.2251({{{10}^{ - 308}}} )$ and $1.7977({{{10}^{308}}} )$; therefore, the results for single and double precisions will be different (Muller et al., Reference Muller, Brisebarre, De Dinechin, Jeannerod, Lefevre, Melquiond, Revol, Stehlé and Torres2018).

All the simulations are done with ideal gyros and the sampling frequency of rate gyros is 1000 Hz and we suppose that the attitude updating frequency is 100 Hz (two-speed algorithm). Actually, for updating quaternion, there are 11 data then the maximum degree of the polynomial for angular velocity is 10. There are two parameters for coning motion, we are considering $ {f_0} = 30\,\textrm{Hz}$, and $\alpha = 50\,\textrm{degrees}$ for the first simulation. Simulation parameters are represented in Table 1.

Table 1.

Simulation parameters

When ${f_0}$ is high, we need very high sampling frequencies for attitude reconstruction. The true angular velocity is presented in Figure 1 when $t \in [{0, 0.05}]\,\textrm{sec}$. We know that the amplitude of angular velocity is a function of coning motion's frequency.

Figure 1.

Angular velocity in $t \in [{0,0.05} ]\textrm{sec}$

Here N and ${N_{it}}$ stand for degree of angular velocities’ fitted polynomial and number terms of series (number of iterations), respectively. Figure 2 shows the error between the fitted and real angular velocities when N is 6 and 8. It is obvious that when N is high, the error of angular velocity is low. In this simulation, when N is 8, the order of error is $3.3({{{10}^{ - 7}}} ) \textrm{rad/sec}$ and when N is 6, the error of fitted angular velocity is $ 2.4({{{10}^{ - 4}}} )\textrm{rad/sec}$. Here, we set $ N$ at 8.

Figure 2.

Error of fitted angular velocity with degrees of N = 6 and N = 8

Figure 3 shows the single and double precision algorithm implementation when the fitted polynomial degree is 8, and iteration numbers are 5, 15, 25 and 35. Here the blue colour shows the double precision implementation and red colour stands for single precision implementation. Obviously, when the number of iterations increases, the errors in both implementations will reduce. The double precision implementation always shows better accuracy than the single precision for all methods. Furthermore, when the number of iterations is low, the single and double precision implementations have the same performance, but when the numbers of iterations are high, two implementation methods have different performance.

Figure 3.

Rotation error when $N = 8$ and when iteration numbers are 5, 15, 25 and 35.

Figure 4 compares the effects of degrees of the fitted polynomial in the error of rotation. The red colour stands for $N = 8$, and the blue colour is for $N = 5$. It is obvious that when the error of angular velocity is low (N is 8), the error of all the methods in double or single forms is lower than the case in which $N = 5$. It was obvious because using high degrees of fitted polynomial has fewer errors for angular velocity and then the calculated attitude has low error.

Figure 4.

Rotation error when N is 5, 8 and, ${N_{it}} = 45$ in single and double precision implementation forms

Figure 5 shows that in the double implementation form, for all methods, when the number of iterations approaches infinity, finally, all the algorithms have the same errors, but in limited iterations, the quaternion's performance is always better than the DCM. In high iterations, both quaternion and DCM methods, the Taylor and power series results are close to each other. In the second figure, we can see that the single precision implementations, the QuatPow has the best performance when the number of iterations increases. The power series always shows better results than the Taylor series when the number of iterations is large enough.

Figure 5.

Maximum error of double precision implementation at time t = 0.01 s when N = 8

Figure 6 shows that the norm of the derivatives of the Taylor polynomial and the norm of power series’ coefficients increase when the number of iterations increases. It is clear that, when the iteration number approaches infinity, in the ideal case, there is no error in the calculated attitude. It is also observable that the power series always has a low norm and then we can consider more terms for decreasing the error. In addition, the norm of coefficients of DCM is lower than the quaternion coefficients’ norm. In the quaternion form, we can take more terms into the model, so we will get better performance. The one cycle of computational burden of algorithms are 1, 1⋅02, 1⋅09, 1⋅10 for QuatPow, QuatTay, DCMPow, DCMTay in relation to the computaion burden for QuatPow method.

Figure 6.

The rotation angle error when $N = \textit{8}$

Figure 7 represents the error of rotation angle with two fitted angular velocities’ polynomials using different algorithms in single and double precision forms for a long time (100 s). All the subplots show that the double precision implementations have low errors, but in power series, in single precision implementation form, the error of rotation angle has the lowest value, which is closer to double implementation method and the worst case is using Taylor series of DCM.

Figure 7.

The rotation angle error when $N = \textit{8}$ and N = 5

Figure 8 shows the results for single and double precision implementations when the coning parameters are ${f_0} = 10\,\textrm{Hz}$ and $\alpha = 1\,\textrm{degree}$, and ${N_{it}} = 50$. Here, the number of iterations is high enough for implementation and increasing the ${N_{it}}$ will have the same results. Figure 8 represents the situation in which the frequency and magnitude of the angular velocity for coning motion are low, the performances of algorithms are closer to each other, and double precision implementation always gives better results than the single precision. Figure 9 shows the 1,000 time Monte-Carlo simulations for single and double implementaions for algorithms when the standard deviation (std) of white noise of gyros are $0{\cdot}1\,\textrm{deg/hour}$. All algorithms are using $N = 8$ and ${N_{it}} = 50$, ${f_0} = 10\,\textrm{Hz}, \alpha = 1\,\textrm{deg}.$

Figure 8.

The rotation angle error when $N = 8$, ${f_0} = \textit{10}\,\textrm{Hz}, \alpha = \textit{1}\,\textrm{deg}, N_{it} = \textit{50}$

Figure 9.

Monte-Carlo simulations when $N = \textit{8}$ and ${N_{it}} = \textit{50}$

Table 2 shows that in all methods the mean and std of error for double precision implementation are lower than the single precision implementation. The mean and std of error for DCM are both bigger than the quaternion method and thus we can say the DCM method is more sensitive to sensors’ noises. In addition, mean and std of error in QuatPow are lower than all methods for both single and double precision implementations. Then we can say that the QuatPow method is more robust than the others with respect to the sensors’ noises.

Table 2.

Monte-Carlo simulation results after 100 s