An Accurate Numerical Algorithm for Attitude Updating Based on High-Order Polynomial Iteration

To effectively improve the accuracy of attitude reconstruction under highly dynamic environments, a new numerical attitude updating algorithm is designed in this paper based on the high-order polynomial iteration according to the differential equation for quaternion. In this algorithm, a high-order polynomial is introduced to fit the angular rate accurately without increasing the number of gyro outputs during per attitude updating interval. This algorithm can provide an exact high-order polynomial solution for quaternion and the process of attitude reconstruction can be implemented efficiently. The algorithm's performance is evaluated as compared with optimal coning algorithm, attitude quaternion updating algorithm based on Picard iteration (QPI), and higher-order rotation vector attitude updating algorithm (Fourth4Rot) under coning motion. The simulation results show that this algorithm can improve the accuracy of attitude computation and clearly outperform the optimal coning algorithm, QPI, and Fourth4Rot in high dynamic environment.

SECTION I. Introduction

Unlike gimbaled inertial navigation system (GINS), strapdown inertial navigation system (SINS) uses a digital platform as an inertial navigation platform, in which the attitude matrix is applied to describe the orientation of the digital platform with respect to the body coordinate frame [1]. During navigation operations, the navigation solution is carried out in SINS after strapdown inertial sensors data are transformed from the body coordinate frame into the navigation coordinate frame by using the attitude matrix. Attitude updating algorithms are the core technology deciding the precision of the inertial navigation system. The key to improving the accuracy of attitude updating algorithms is to accurately compensate the non-commutability error which is inevitable for attitude updating [2], [3].

Numerous great endeavors have been devoted to the study on the non-commutability error correction algorithms and the researches are fruitful in the inertial navigation community over the past decades. At present, multi-sample rotation vector and its improved algorithms are the most popular attitude updating algorithms in the inertial navigation filed, which have been widely used in engineering practice [4]. In 1971, Bortz first proposed the concept of rotation vector and its differential equation (Bortz equation), which provided a theoretical basis for the study of rotation vector algorithms [5]. Based on this, Miller first introduced the concept of optimizing the coning compensation coefficients and proposed a coning algorithm [6]. In 1999, Park proposed a formalized approach to obtaining optimal coefficients for coning algorithms [7]. In 2010, Savage presented an optimum approach named explicit frequency shaping (EFS) for strapdown coning algorithm design, which achieved optimization through least-squares estimation over a user selected design frequency range [8]. In 2015, Wang proposed higher-order attitude updating algorithm named Third3Rot after considering the third-order Picard component solutions of rotation vector and then presented its improved algorithm named Fourth4Rot based on the fourth-order Picard component solutions of the rotation vector, which performed better accuracy and had high adaptability in large angular rate maneuvering environments [4], [9].

Unlike the traditional rotation vector algorithms, in which the non-commutability error compensation terms are directly determined by the gyro angular increment measurements (samples) and fixed coning algorithm coefficients, some numerical methods such as the Runge-Kutta can also be used to solve the attitude kinematic equation and complete attitude computation, in which the process of compensating non-commutability error is included in solving differential equations of attitude parameters. Compared with multi-sample rotation vector algorithm, the Runge-Kutta and other numerical algorithms implement the attitude integration with lower accuracy, and do not attract enough attention [10]. In 2018, Yan proposed and demonstrated the attitude quaternion updating algorithm based on Picard iteration (QPI), which showed better performance than the mainstream rotation vector algorithm [11]. Wu raised the functional iteration integration approach, combined with Chebyshev polynomial approximation, for attitude computation by way of the Rodrigues vector and then afterwards applied the approach to quaternion [12]–​[14].

The above mentioned attitude updating algorithms mainly calculate the non-commutability error compensation terms directly or indirectly based on polynomial motion model or coning motion model. In engineering applications, substituting a certain model such as polynomial motion model for rigid body angular motion which has the characteristics of randomness and uncertainty will lead to the fitting error inevitably. In theory, the fitting error can be reduced by increasing the number of samples but the sample frequency is subject to gyro technology and SINS update requirements [15]. Therefore, it is an attractive topic to improve the accuracy of the non-commutability error compensation by increasing the angular rate polynomial fitting order under certain samples. However, from the point of view of mathematical analysis, the angular rate can only be fitted by a polynomial in time up to the order of $N-1$ by using $N$ angular increment measurements. If we want to establish a high-order polynomial to describe the rigid body angular motion, more constraints need to be introduced. Based on this, a high-order polynomial model is proposed in this paper, which takes coning motion excitation as an extra constraint. Then this paper presents an accurate numerical method for quaternion differential equation based on the combination of the high-order polynomial model and QPI, in which the polynomial fitting order is increased to $N$ or $N+1$ up from $N-1$ . In this algorithm, the exact polynomial solution for the quaternion can be iteratively obtained.

The remainder of this paper is organized as follows: Section II briefly discusses the multi-sample rotation vector algorithm and QPI. Section III is devoted to demonstrating the design of the high-order polynomial. Section IV develops the new method based on high-order polynomial iteration and then analyzes its computational complexity and convergence property. Section V evaluates the performance of the proposed algorithm through coning motion. Finally, a brief summary is given in Section VI.

SECTION II. Attitude Updating Algorithm

A. Multi-Sample Rotation Vector Algorithm

In modern days, the most widely used attitude updating algorithm is multi-sample rotation vector algorithm. The main idea of this algorithm is to construct the rotation vector with the gyro samples and fixed coefficients, and then acquire the attitude information based on rotation vector. The differential equation for rotation vector is given as [5], [16]

$$\begin{equation*} \dot {\boldsymbol \phi }={\boldsymbol { \omega }}+\frac {1}{2}\boldsymbol \phi \times {\boldsymbol { \omega }}+\frac {1}{ \phi ^{2}}\left ({{1-\frac { \phi }{2}\cot \frac { \phi }{2}} }\right)(\boldsymbol \phi \times)^{2}{\boldsymbol { \omega }}\tag{1}\end{equation*}$$ where $\boldsymbol \phi$ is the rotation vector, $\phi$ is the norm of $\boldsymbol \phi$ , $\boldsymbol {\omega }$ is the angular rate vector expressed in the body frame and the operator $\times$ means the vector cross product.

Substituting the gyro samples $\boldsymbol {\Delta \theta }$ for the right-sided rotation vector and discarding the third term on the right in (1) which is small enough and can be neglected under most environments, then we have the commonly accepted form of (1):

$$\begin{equation*} \dot {\boldsymbol \phi }={\boldsymbol { \omega }}+\frac {1}{2}\boldsymbol {\Delta \theta }\times {\boldsymbol { \omega }}\tag{2}\end{equation*}$$

Donating the gyro sampling time interval and samples by $h$ and $N$ respectively. During the time interval $\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &~ {Nh} \\ \end{array}}} }\right]$ , let the incremental gyro samples at time points $t=0,h,\cdots, Nh$ be $\Delta \boldsymbol {\theta }_{1}$ , $\Delta \boldsymbol {\theta }_{2},\cdots, ~\Delta \boldsymbol {\theta }_{N}$ respectively. Under the condition that the angular motion can be described by a polynomial motion model, classical multi-sample algorithm use Taylor series expansions to solve (2). The classical 2-4-sample algorithm can calculate the rotation vector when the angular motion is described by a straight line, a parabola and a cubic parabola respectively. The estimation of rotation vector $\boldsymbol \phi \left ({{Nh} }\right)$ calculated by classical multi-sample algorithm is tabulated in Table 1 [17].

TABLE 1 The Estimation of Rotation Vector of Classical Multi-Sample Algorithm

The pure coning motion which is considered as the worst working condition is often selected as a particular input for designing the optimal coning algorithm. The optimal coning algorithm optimize $\boldsymbol \phi \left ({{Nh} }\right)$ by minimizing the error of the nonperiodic component in classical coning environment. The estimation of $\boldsymbol \phi \left ({{Nh} }\right)$ calculated by the optimal coning algorithm is listed in Table 2 [18].

TABLE 2 The Estimation of Rotation Vector of Optimal Coning Algorithm

1) QPI Algorithm

Similar to a complex number expressing the rigid body rotation in 2-D space, quaternion can clearly represent the rigid body rotation in 3-D space as it is singularity-free. The differential equation for quaternion is given by [19]

$$\begin{equation*} { \dot {{\boldsymbol {Q}}}}\left ({t }\right)=\frac {1}{2}{\boldsymbol {Q}}\left ({t }\right)\circ {\boldsymbol { \omega }}\left ({t }\right)\tag{3}\end{equation*}$$ where $\boldsymbol {Q}\left ({t }\right)$ is attitude quaternion at time points $t$ and the operator $\circ$ means the multiplication of quaternions. Over the considered attitude updating interval $\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &~ {Nh} \\ \end{array}}} }\right]$ , the Picard component solutions to quaternion can be calculated as follows: $$\begin{align*} {\boldsymbol {Q}}(Nh)=&{\boldsymbol {Q}}(0)\circ \boldsymbol {q}\left ({{Nh,0} }\right) \\[-3pt] s.t.~{\boldsymbol {Q}}(0)=&\mathbf {1}=\left [{ {{\begin{array}{cccccccccccccccccccc} 1 & ~0 &~ 0 &~ 0 \\ \end{array}}} }\right]^{T}\tag{4}\end{align*}$$ where $$\begin{align*}&\hspace{-1.2pc} \boldsymbol {q}\left ({{Nh,0} }\right) \\[-3pt]=&\mathbf {1}+\frac {1}{2}\int _{0}^{Nh} {\boldsymbol {\omega }\left ({{t_{1}} }\right)} dt_{1} +\frac {1}{2^{2}}\int _{0}^{Nh} {\int _{0}^{t_{2}} {\boldsymbol {\omega }\left ({{t_{1}} }\right)}} dt_{1} \circ \boldsymbol {\omega }\left ({{t_{2}} }\right)dt_{2} \\[-2pt]&+ \, \frac {1}{2^{3}}\!\int _{0}^{Nh} {\int _{0}^{t_{3}} \!{\int _{0}^{t_{2}} {\boldsymbol {\omega }\left ({{t_{1}} }\right)}}} dt_{1} \circ \boldsymbol {\omega }\left ({{t_{2}} }\right)dt_{2} \circ \boldsymbol {\omega }\left ({{t_{3}} }\right)dt_{3} \!+\!\ldots \\[-3pt]\tag{5}\end{align*}$$ where $\boldsymbol {q}\left ({{Nh,0} }\right)$ is an updating quaternion during the time interval $\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &~ {Nh} \\ \end{array}}} }\right]$ .

Using $N$ gyro samples over the time interval $\left [{ {{\begin{array}{cccccccccccccccccccc} 0 & ~{Nh} \\ \end{array}}} }\right]$ , $\boldsymbol {\omega }\left ({t }\right)$ can be fitted by a polynomial in time up to the order of $N-1$ , given by

$$\begin{equation*} \hat {\boldsymbol {\omega }}\left ({t }\right)=\sum \limits _{i=0}^{k} {\boldsymbol {c}{}_{\boldsymbol {i}}} t^{i},\quad k\le \left ({{N-1} }\right)\tag{6}\end{equation*}$$ where $\hat {\boldsymbol {\omega }}\left ({t }\right)$ represents the fitted angular rate vector and $\boldsymbol {c}{}_{\boldsymbol {i}}$ are the coefficients vector of the fitted polynomial. According to the numerical relationship between angular increments and angular rate, the coefficients $\boldsymbol {c}{}_{\boldsymbol {i}}$ can be determined by solving the equation as follows: $$\begin{equation*} \left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {c}_{0}} &~ {\boldsymbol {c}_{1}} &~ \cdots &~ {\boldsymbol {c}_{N-1}} \\ \end{array}}} }\right]=\left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {\Delta \theta }_{\boldsymbol {1}}} &~ {\boldsymbol {\Delta \theta }_{2}} &~ \cdots &~ {\boldsymbol {\Delta \theta }_{N}} \\ \end{array}}} }\right]\boldsymbol {\Gamma }^{-1}\tag{7}\end{equation*}$$ where $$\begin{align*} \boldsymbol {\Gamma }=&\left [{ {{\begin{array}{cccccccccccccccccccc} {\dfrac {t_{1}^{N}}{N}} &\quad {\dfrac {t_{2}^{N} -t_{1}^{N}}{N}} &\quad \cdots &\quad {\dfrac {t_{N}^{N} -t_{N-1}^{N}}{N}} \\ {\dfrac {t_{1}^{N-1}}{N-1}} &\quad {\dfrac {t_{2}^{N-1} -t_{1}^{N-1}}{N-1}} &\quad \cdots &\quad {\dfrac {t_{N}^{N-1} -t_{N-1}^{N-1}}{N-1}} \\ \vdots &\quad \vdots &\quad \ddots &\quad \vdots \\ {t_{1}} &\quad {t_{1}} &\quad \cdots &\quad {t_{1}} \\ \end{array}}} }\right] \\ s.t.~t_{j}=&jh\left ({{j=1,2,\cdots, N} }\right)\tag{8}\end{align*}$$

For the convenience of description, denote $\hat {\boldsymbol {\omega }}\left ({t }\right)=2\hat {\boldsymbol {W}}\left ({t }\right)$ . Replacing $\hat {\boldsymbol {\omega }}\left ({t }\right)$ in (5) with $2\hat {\boldsymbol {W}}\left ({t }\right)$ , it can be derived that

$$\begin{align*}&\hspace{-1.2pc}\boldsymbol {q}\left ({{Nh,0} }\right) \\[-3pt]=&\mathbf {1}+\int _{0}^{Nh} {\hat {\boldsymbol {W}}\left ({{t_{1}} }\right)} dt_{1} +\int _{0}^{Nh} {\int _{0}^{t_{2} } {\hat {\boldsymbol {W}}\left ({{t_{1}} }\right)}} dt_{1} \circ \hat {\boldsymbol {W}}\left ({{t_{2}} }\right)dt_{2} \\[-2pt]&+ \, \int _{0}^{Nh} {\int _{0}^{t_{3}} {\int _{0}^{t_{2}} {\hat {\boldsymbol {W}}\left ({{t_{1} } }\right)}}} dt_{1} \circ \hat {\boldsymbol {W}}\left ({{t_{2}} }\right)dt_{2} \circ \hat {\boldsymbol {W}}\left ({{t_{3}} }\right)dt_{3} +\ldots \\\tag{9}\end{align*}$$

Obviously, $\hat {\boldsymbol {W}}\left ({t }\right)$ is expressed in the polynomial form, which lays the foundation for converting the product of the quaternion in (9) to the convolution operation of the angular velocity polynomial coefficients. Thus,

$$\begin{align*} \begin{cases} {\int _{0}^{Nh} {\hat {\boldsymbol {W}}\left ({{t_{1}} }\right)} dt_{1} =\left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {U}_{0}^{\left ({1 }\right)}} \\ {\boldsymbol {U}_{1}^{\left ({1 }\right)}} \\ {\boldsymbol {U}_{2}^{\left ({1 }\right)}} \\ {\boldsymbol {U}_{3}^{\left ({1 }\right)}} \\ \end{array}}} }\right]\left [{ {{\begin{array}{cccccccccccccccccccc} {(Nh)^{N}} \\[0.4pc] {(Nh)^{N-1}} \\ \vdots \\ Nh \\[5pt] \end{array}}} }\right]} \\ {\int _{0}^{Nh} {\int _{0}^{t_{2}} {\hat {\boldsymbol {W}}\left ({{t_{1}} }\right)}} dt_{1} \circ \hat {\boldsymbol {W}}\left ({{t_{2}} }\right)dt_{2} =\left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {U}_{0}^{\left ({2 }\right)}} \\ {\boldsymbol {U}_{1}^{\left ({2 }\right)}} \\ {\boldsymbol {U}_{2}^{\left ({2 }\right)}} \\ {\boldsymbol {U}_{3}^{\left ({2 }\right)}} \\ \end{array}}} }\right]\left [{ {{\begin{array}{cccccccccccccccccccc} {(Nh)^{2N}} \\[0.4pc] {(Nh)^{2N-1}}\\ \vdots \\ {(Nh)^{2}} \\ \end{array}}} }\right]} \\[5pt] {\begin{array}{l} \int _{0}^{Nh} {\int _{0}^{t_{3}} {\int _{0}^{t_{2}} {\hat {\boldsymbol {W}}\left ({{t_{1} } }\right)}}} dt_{1} \circ \hat {\boldsymbol {W}}\left ({{t_{2}} }\right)dt_{2} \circ \hat {\boldsymbol {W}}\left ({{t_{3}} }\right)dt_{3} \\[5pt] \quad =\left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {U}_{0}^{\left ({3 }\right)}} \\ {\boldsymbol {U}_{1}^{\left ({3 }\right)}} \\ {\boldsymbol {U}_{2}^{\left ({3 }\right)}} \\ {\boldsymbol {U}_{3}^{\left ({3 }\right)}} \\ \end{array}}} }\right]\left [{ {{\begin{array}{cccccccccccccccccccc} {(Nh)^{3N}} \\[0.4pc] {(Nh)^{3N-1}} \\ \vdots \\ {(Nh)^{3}} \\ \end{array}}} }\right] \\[5pt] \vdots \\ \end{array}} \\ \end{cases}\!\!\!\!\!\!\! \\\tag{10}\end{align*}$$ where $\boldsymbol {U}_{j}^{\left ({k }\right)}$ is the $j$ -th row of the k-integral polynomials coefficients matrix. At the same time, $\boldsymbol {U}_{j}^{\left ({{k+1} }\right)}$ and $\boldsymbol {U}_{j}^{\left ({k }\right)}$ satisfy the following equation [11]: $$\begin{align*} \left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {U}_{0}^{\left ({{k+1} }\right)}} \\ {\boldsymbol {U}_{1}^{\left ({{k+1} }\right)}} \\ {\boldsymbol {U}_{2}^{\left ({{k+1} }\right)}} \\ {\boldsymbol {U}_{3}^{\left ({{k+1} }\right)}} \\ \end{array}}} }\right]&=\left [{ {{\begin{array}{cccccccccccccccccccc} {-\boldsymbol {U}_{1}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{x} -\boldsymbol {U}_{2}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{y} -\boldsymbol {U}_{3}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{z}} \\ {\boldsymbol {U}_{0}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{x} +\boldsymbol {U}_{2}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{z} -\boldsymbol {U}_{3}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{y}} \\ {\boldsymbol {U}_{0}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{y} +\boldsymbol {U}_{3}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{x} -\boldsymbol {U}_{1}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{z}} \\ {\boldsymbol {U}_{0}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{z} +\boldsymbol {U}_{1}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{y} -\boldsymbol {U}_{2}^{\left ({k }\right)} \ast \hat {\boldsymbol {W}}_{x}} \\ \end{array}}} }\right] \\ \hspace {1pc}&diag\left ({\frac {1}{(k+1)N}\frac {1}{(k+1)N-1}\ldots \frac {1}{k+1}}\right) \\\tag{11}\end{align*}$$ Substituting (10) into (9) yields that $$\begin{align*}&\hspace {-2pc} \boldsymbol {q}\left ({{Nh,0} }\right) \\=&\mathbf {1}+\left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {U}_{0}^{\left ({1 }\right)}} \\ {\boldsymbol {U}_{1}^{\left ({1 }\right)}} \\ {\boldsymbol {U}_{2}^{\left ({1 }\right)}} \\ {\boldsymbol {U}_{3}^{\left ({1 }\right)}} \\ \end{array}}} }\right]\left [{ {{\begin{array}{cccccccccccccccccccc} {(Nh)^{N}} \\[0.4pc] {(Nh)^{N-1}} \\ \vdots \\ Nh \\ \end{array}}} }\right]+\left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {U}_{0}^{\left ({2 }\right)}} \\ {\boldsymbol {U}_{1}^{\left ({2 }\right)}} \\ {\boldsymbol {U}_{2}^{\left ({2 }\right)}} \\ {\boldsymbol {U}_{3}^{\left ({2 }\right)}} \\ \end{array}}} }\right]\left [{ {{\begin{array}{cccccccccccccccccccc} {(Nh)^{2N}} \\[0.4pc] {(Nh)^{2N-1}} \\ \vdots \\ {(Nh)^{2}} \\ \end{array}}} }\right] \\&+\,\left [{ {{\begin{array}{cccccccccccccccccccc} {\boldsymbol {U}_{0}^{\left ({3 }\right)}} \\ {\boldsymbol {U}_{1}^{\left ({3 }\right)}} \\ {\boldsymbol {U}_{2}^{\left ({3 }\right)}} \\ {\boldsymbol {U}_{3}^{\left ({3 }\right)}} \\ \end{array}}} }\right]\left [{ {{\begin{array}{cccccccccccccccccccc} {(Nh)^{3N}} \\[0.4pc] {(Nh)^{3N-1}} \\ \vdots \\ {(Nh)^{3}} \\ \end{array}}} }\right]+\cdots\tag{12}\end{align*}$$

Obviously, (11) provides a basis for solving (9) and it is easy to calculate ${\boldsymbol {Q}}(Nh)$ after substituting (12) into (4).

The above process that solves the updating quaternion $\boldsymbol {q}\left ({{Nh,0} }\right)$ indicates that the QPI errors are mainly dominated by the angular rate fitting error and the last truncation error which is small enough when calculating enough terms on the right of (12).

SECTION III. High-Order Polynomial Motion Model Based on Coning Motion Constraints

A. High-Order Polynomial Motion Model

Unlike (6), let $\boldsymbol {\omega }\left ({t }\right)$ be described by a polynomial of high order over the time interval $\left [{ {{\begin{array}{cccccccccccccccccccc} {0} &~ {Nh} \\ \end{array}}} }\right]$ , namely,

$$\begin{equation*} {\hat {\boldsymbol \omega }}'\left ({t }\right)=\sum \limits _{i=0}^{k} {\boldsymbol {c}'_{i}} t^{i},\quad k\ge N\tag{13}\end{equation*}$$ where $\hat {\boldsymbol \omega }'\left ({t }\right)$ represents the fitted angular rate vector and $\boldsymbol {c}'_{i}$ are the coefficients vector of the high-order fitted polynomial. Obviously, the coefficients $\boldsymbol {c}_{0}$ , $\boldsymbol {c}_{1}$ , $\boldsymbol {c}_{2}, \cdots, \boldsymbol {c}_{N-1}$ can be calculated via $N$ gyro samples according to (7). Substituting $\boldsymbol {c}_{0}$ , $\boldsymbol {c}_{1}$ , $\boldsymbol {c}_{2}$ , $\cdots$ , $\boldsymbol {c}_{N-1}$ into (13) and $\boldsymbol {c}'_{0}$ , $\boldsymbol {c}'_{1}$ , $\cdots$ , $\boldsymbol {c}'_{k}$ can be calculated by solving the function $\boldsymbol {c}'_{i} =f_{i} \left ({{\boldsymbol {c}_{0}, \boldsymbol {c}_{1}, \cdots \boldsymbol {c}_{N-1}, \boldsymbol {c}'_{N},\boldsymbol {c}'_{N+1} \cdots, \boldsymbol {c}'_{k}} }\right)$ , that is $$\begin{equation*} \begin{cases} \boldsymbol {c}'_{0} =f_{0} \left ({{\boldsymbol {c}_{0}, \boldsymbol {c}_{1}, \cdots \boldsymbol {c}_{N-1}, \boldsymbol {c}'_{N}, \boldsymbol {c}'_{N+1} \cdots,\boldsymbol {c}'_{k}} }\right) \\ \boldsymbol {c}'_{1} =f_{1} \left ({{\boldsymbol {c}_{0}, \boldsymbol {c}_{1}, \cdots \boldsymbol {c}_{N-1}, \boldsymbol {c}'_{N}, \boldsymbol {c}'_{N+1} \cdots,\boldsymbol {c}'_{k}} }\right) \\ \vdots \\ \boldsymbol {c}'_{k} =f_{k} \left ({{\boldsymbol {c}_{0}, \boldsymbol {c}_{1}, \cdots \boldsymbol {c}_{N-1}, \boldsymbol {c}'_{N}, \boldsymbol {c}'_{N+1} \cdots,\boldsymbol {c}'_{k}} }\right) \\ \end{cases}\tag{14}\end{equation*}$$

Consequently, the key to calculating $\boldsymbol {c}'_{0}$ , $\boldsymbol {c}'_{1}$ , $\cdots$ , $\boldsymbol {c}'_{k}$ is to introduce additional constraints to determine parameters $\boldsymbol {c}'_{N}$ , $\boldsymbol {c}'_{N+1}$ , $\cdots$ , and $\boldsymbol {c}'_{k}$ .

B. The Relation Between the Polynomial Coefficients in Coning Motion Environment

The angular rate $\boldsymbol {\omega }\left ({t }\right)$ for classical coning motion is defined as [6]:

$$\begin{equation*} \boldsymbol {\omega }\left ({t }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} {-2\omega _{0} \sin ^{2}\left ({{\alpha /2} }\right)} \\ {-\omega _{0} \sin \left ({\alpha }\right)\sin \left ({{\omega _{0} t} }\right)} \\ {\omega _{0} \sin \left ({\alpha }\right)\cos \left ({{\omega _{0} t} }\right)} \\ \end{array}}} }\right]\tag{15}\end{equation*}$$ where $\alpha$ is the cone half-angle and $\omega _{0}$ is the coning frequency.

Differentiating (13) and (15) repeatedly with respect to time $t$ yields the following results at the time point $t=0$ :

$$\begin{align*} \begin{cases} \boldsymbol {\omega }^{\left ({0 }\right)}\left ({0 }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} {-2\omega _{0} \sin ^{2}\left ({{\alpha /2} }\right)} &\quad 0 &\quad {\omega _{0} \sin \left ({\alpha }\right)} \\ \end{array}}} }\right]^{T}=\boldsymbol {c}'_{0} \\ \boldsymbol {\omega }^{\left ({1 }\right)}\left ({0 }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad {-\omega _{0}^{2} \sin \left ({\alpha }\right)} &\quad 0 \\ \end{array}}} }\right]^{T}=\boldsymbol {c}'_{1} \\ \boldsymbol {\omega }^{\left ({2 }\right)}\left ({0 }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad 0 &\quad {-\omega _{0}^{3} \sin \left ({\alpha }\right)} \\ \end{array}}} }\right]^{T}=2\boldsymbol {c}'_{2} \\ \boldsymbol {\omega }^{\left ({3 }\right)}\left ({0 }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad {\omega _{0}^{4} \sin \left ({\alpha }\right)} &\quad 0 \\ \end{array}}} }\right]^{T}=6\boldsymbol {c}'_{3} \\ \qquad \quad ~~\,\boldsymbol {\vdots } \\ \boldsymbol {\omega }^{\left ({k }\right)}\left ({0 }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad 0 &\quad {\left ({{-1} }\right)^{k \mathord {\left /{ {\vphantom {k 2}} }\right. } 2}\omega _{0}^{k+1} \sin \left ({\alpha }\right)} \\ \end{array}}} }\right]^{T} \\ \qquad \quad \,\,=k!\boldsymbol {c}'_{k}, \quad k=2n,~n=1,2,\cdots \\ \boldsymbol {\omega }^{\left ({k }\right)}\left ({0 }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad {\left ({{-1} }\right)^{{\left ({{k+1} }\right)} \mathord {\left /{ {\vphantom {{\left ({{k+1} }\right)} 2}} }\right. } 2}\omega _{0}^{k+1} \sin \left ({\alpha }\right)} &\quad 0 \\ \end{array}}} }\right]^{T} \\ \qquad \quad \,\,=k!\boldsymbol {c}'_{k}, \quad k=2n+1 \end{cases}\!\!\!\!\!\!\!\! \\\tag{16}\end{align*}$$

Considering that the computational drift appears only on the coning axis(x) and the periodic error appears on the non-coning axis(y) and axis(z) for classical coning motion described in (15), we mainly discuss the relation between the polynomial coefficients on axis(x) [20]. In view of (16), it can be derived that

$$\begin{align*} \begin{cases} {\begin{array}{l} \left ({{\boldsymbol {\omega }^{\left ({0 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({1 }\right)}\left ({0 }\right)} }\right)_{x} =\left ({{\boldsymbol {c}'_{0} \times \boldsymbol {c}'_{1}} }\right)_{x} =\omega _{0}^{3} \sin ^{2}\left ({\alpha }\right) \\ \left ({{\boldsymbol {\omega }^{\left ({0 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({2 }\right)}\left ({0 }\right)} }\right)_{x} =\left ({{\boldsymbol {c}'_{0} \times 2\boldsymbol {c}'_{2}} }\right)_{x} =0 \\ \left ({{\boldsymbol {\omega }^{\left ({1 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({2 }\right)}\left ({0 }\right)} }\right)_{x} =\left ({{\boldsymbol {c}'_{1} \times 2\boldsymbol {c}'_{2}} }\right)_{x} =-\omega _{0}^{5} \sin ^{2}\left ({\alpha }\right) \\ \end{array}} \\ {\left ({{\boldsymbol {\omega }^{\left ({0 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({3 }\right)}\left ({0 }\right)} }\right)_{x} =\left ({{\boldsymbol {c}'_{0} \times 6\boldsymbol {c}'_{3}} }\right)_{x} =\omega _{0}^{5} \sin ^{2}\left ({\alpha }\right)} \\ \vdots \\ \left ({{\boldsymbol {\omega }^{\left ({i }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({j }\right)}\left ({0 }\right)} }\right)_{x} =\left ({{\left ({{i+1} }\right)\mathbf {!{c}'}_{i+1} \times \left ({{j+1} }\right)\mathbf {!{c}'}_{j+1}} }\right)_{x} \\ \qquad \qquad \qquad \qquad ~~\, =0,\quad i+j=2n,~n=1,2,\cdots \\ \left ({{\boldsymbol {\omega }^{\left ({i }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({j }\right)}\left ({0 }\right)} }\right)_{x} =l\omega _{0}^{i+j+2} \sin ^{2}\left ({\alpha }\right),\quad {l=-1,1} \end{cases}\!\!\!\!\!\!\!\!\!\! \\\tag{17}\end{align*}$$

In order to facilitate the analysis, the value of the cross product of the polynomial coefficients on axis(x) is shown in Fig. 1. The value of the cross product terms on the same dotted oblique line have the same power of 0 or $\omega _{0}^{m} \sin ^{2}\alpha$ , $m=3,5,\cdots$ , in Fig. 1.

FIGURE 1.

The value of the cross product of the polynomial coefficients on axis(x).

As shown in Fig. 1, the value of one cross product term on axis(x) is certain times as many as that of other cross product terms when these cross product terms are on the same dotted oblique line. According to the numerical relationship between the cross product terms on the same dotted oblique line, more constraints could be introduced to determine parameters $\boldsymbol {c}'_{N}$ , $\boldsymbol {c}'_{N+1}$ , $\cdots$ , and $\boldsymbol {c}'_{k}$ .

C. Third-Order Polynomial Based on Three Samples

If $N=3$ , four independent equations can be constructed based on gyro samples $\Delta \boldsymbol {\theta }_{1}$ , $\Delta \boldsymbol {\theta }_{2}$ , $\Delta \boldsymbol {\theta }_{3}$ and the numerical relationship between $\boldsymbol {c}'_{1} \times \boldsymbol {c}'_{2}$ and $\boldsymbol {c}'_{0} \times \boldsymbol {c}'_{3}$ on axis(x). Consequently, $\boldsymbol {\omega }\left ({t }\right)$ can be fitted by a polynomial in time up to the order of 3 and can be expressed as:

$$\begin{equation*} {\hat {\boldsymbol {\omega }}'}\left ({t }\right)=\boldsymbol {c}'_{0} +\boldsymbol {c}'_{1} t+\boldsymbol {c}'_{2} t^{2}+\boldsymbol {c}'_{3} t^{3},\quad t\in \left [{ {0~~3h} }\right]\tag{18}\end{equation*}$$

Taking $\boldsymbol {c}'_{3}$ as an unknown parameter, the coefficients $\boldsymbol {c}'_{0}$ , $\boldsymbol {c}'_{1}$ , and $\boldsymbol {c}'_{2}$ can be computed as:

$$\begin{align*} \begin{cases} \boldsymbol {c}'_{0} =\boldsymbol {c}_{0} -\frac {3}{2} h^{3}\boldsymbol {c}'_{3} \\[0.4pc] \boldsymbol {c}'_{1} =\boldsymbol {c}_{1} +\frac {11}{2} h^{2}\boldsymbol {c}'_{3} \\[0.4pc] \boldsymbol {c}'_{2} =\boldsymbol {c}_{2} -\frac {9}{2} h\boldsymbol {c}'_{3} \\ \end{cases}\tag{19}\end{align*}$$ where the coefficients $\boldsymbol {c}_{1}$ , $\boldsymbol {c}_{2}$ , and $\boldsymbol {c}_{3}$ are computed by $\Delta \boldsymbol {\theta }_{1}$ , $\Delta \boldsymbol {\theta }_{2}$ , and $\Delta \boldsymbol {\theta }_{3}$ according to (7).

In view of (17), it can be found that

$$\begin{align*} \boldsymbol {c}'_{0} \times \boldsymbol {c}'_{3}=&\frac {1}{6}\boldsymbol {\omega }^{\left ({0 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({3 }\right)}\left ({0 }\right) \\=&\frac {1}{6}\left [{ {{\begin{array}{cccccccccccccccccccc} {-2\omega _{0} \sin ^{2}\left ({{\alpha /2} }\right)} &\quad 0 &\quad {\omega _{0} \sin \left ({\alpha }\right)} \\ \end{array}}} }\right]^{T} \\&\times \, \left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad {\omega _{0}^{4} \sin \left ({\alpha }\right)} &\quad 0\\ \end{array}}} }\right]^{T} \\=&\frac {1}{6}\left [{ {{\begin{array}{cccccccccccccccccccc} {-\omega _{0}^{5} \sin ^{2}\left ({\alpha }\right)} & 0 & {-2\omega _{0}^{5} \sin \left ({\alpha }\right)\sin ^{2}\left ({{\alpha /2} }\right)} \\ \end{array}}} }\right] \\ \tag{20}\\ \boldsymbol {c}'_{1} \times \boldsymbol {c}'_{2}=&\frac {1}{2}\boldsymbol {\omega }^{\left ({1 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({2 }\right)}\left ({0 }\right) \\=&\frac {1}{2}\left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad {-\omega _{0}^{2} \sin \left ({\alpha }\right)} &\quad 0 \\ \end{array}}} }\right]^{T} \\&\times \, \left [{ {{\begin{array}{cccccccccccccccccccc} 0 &\quad 0 &\quad {-\omega _{0}^{3} \sin \left ({\alpha }\right)}\\ \end{array}}} }\right]^{T} \\\quad =&\quad \frac {1}{2}\left [{ {{\begin{array}{cccccccccccccccccccc} {\omega _{0}^{5} \sin ^{2}\left ({\alpha }\right)} &\quad 0 &\quad 0 \\ \end{array}}} }\right]^{T}\tag{21}\end{align*}$$

Considering the relation between the cross products of polynomial coefficients on axis(x) and (20), (21), we have

$$\begin{equation*} \boldsymbol {c}'_{0} \times \boldsymbol {c}'_{3} =-\frac {1}{3}\boldsymbol {c}'_{1} \times \boldsymbol {c}'_{2}\tag{22}\end{equation*}$$

In view of (19), it can be found that

$$\begin{equation*} \boldsymbol {c}_{0} \times \boldsymbol {c}'_{3} =-\frac {1}{3}\boldsymbol {c}_{1} \times \boldsymbol {c}_{2} +\frac {3}{2} h\boldsymbol {c}_{1} \times \boldsymbol {c}'_{3} +\frac {11}{6} h^{2}\boldsymbol {c}_{2} \times \boldsymbol {c}'_{3}\tag{23}\end{equation*}$$

Neglecting the higher-order terms in (23) yields $\boldsymbol {c}_{0} \times \boldsymbol {c}'_{3} =-1 \mathord {\left /{ {\vphantom {1 3}} }\right. } 3\boldsymbol {c}_{1} \times \boldsymbol {c}_{2}$ , that is

$$\begin{equation*} \boldsymbol {c}'_{3} =-\frac {1}{3}\left ({{\boldsymbol {c}_{0} \times } }\right)^{+}\left ({{\boldsymbol {c}_{1} \times \boldsymbol {c}_{2}} }\right)\tag{24}\end{equation*}$$ where the operator + means the general inverse of matrix. The coefficients $\boldsymbol {c}'_{0}$ , $\boldsymbol {c}'_{1}$ , and $\boldsymbol {c}'_{2}$ can be calculated after substituting (24) into (19). Consequently, the fitted angular rate third-order polynomial is given by: $$\begin{align*}&\hspace{-0.5pc}{\hat {\boldsymbol {\omega }}'}\left ({t }\right)=\boldsymbol {c}'_{0} +\boldsymbol {c}'_{1} t+\boldsymbol {c}'_{2} t^{2} \\&\qquad\qquad\quad {-\,\frac {1}{3}\left ({{\boldsymbol {c}_{0} \times } }\right)^{+}\left ({{\boldsymbol {c}_{1} \times \boldsymbol {c}_{2}} }\right)t^{3},\quad t\in \left [{ {0\,\,\,\,3h} }\right]} \tag{25}\end{align*}$$

D. Fifth-Order Polynomial Based on Four Sample

As shown in Fig. 1, the value of $\left ({{\boldsymbol {c}'_{1} \times \boldsymbol {c}'_{3}} }\right)_{x}$ and $\left ({{\boldsymbol {c}'_{0} \times \boldsymbol {c}'_{4}} }\right)_{x}$ is zero, which means the numerical relationship between $\boldsymbol {c}'_{1} \times \boldsymbol {c}'_{3}$ and $\boldsymbol {c}'_{0} \times \boldsymbol {c}'_{4}$ on axis(x) cannot construct an independent equation for the coefficients of higher-order polynomial. If $N=4$ , six independent equations can be constructed based on gyro samples $\Delta \boldsymbol {\theta }_{1}$ , $\Delta \boldsymbol {\theta }_{2}$ , $\Delta \boldsymbol {\theta }_{3}$ , $\Delta \boldsymbol {\theta }_{4}$ and the numerical relationship between $\boldsymbol {c}'_{2} \times \boldsymbol {c}'_{3}$ and $\boldsymbol {c}'_{1} \times \boldsymbol {c}'_{4}$ , $\boldsymbol {c}'_{2} \times \boldsymbol {c}'_{3}$ and $\boldsymbol {c}'_{0} \times \boldsymbol {c}'_{5}$ on axis(x). Consequently, $\boldsymbol {\omega }\left ({t }\right)$ can be fitted by a polynomial in time up to the order of 5, which can be expressed as:

$$\begin{align*} {\hat {\boldsymbol {\omega }}'}\left ({t }\right)=\boldsymbol {c}'_{0} +\boldsymbol {c}'_{1} t+\boldsymbol {c}'_{2} t^{2}+\boldsymbol {c}'_{3} t^{3}+\boldsymbol {c}'_{4} t^{4}+\boldsymbol {c}'_{5} t^{5},\quad t\in \left [{ {0~~4h} }\right]\!\!\!\!\! \\\tag{26}\end{align*}$$

Taking $\boldsymbol {c}'_{4}$ and $\boldsymbol {c}'_{5}$ as unknown parameters, the coefficients $\boldsymbol {c}'_{0}$ , $\boldsymbol {c}'_{1}$ , $\boldsymbol {c}'_{2}$ , and $\boldsymbol {c}'_{3}$ can be computed as:

$$\begin{equation*} \begin{cases} \boldsymbol {c}'_{0} =\boldsymbol {c}_{0} +\frac {24}{5} h^{4}\boldsymbol {c}'_{4} +40h^{5}\boldsymbol {c}'_{5} \\ \boldsymbol {c}'_{1} =\boldsymbol {c}_{1} -20h^{3}\boldsymbol {c}'_{4} -\frac {476}{3} h^{4}\boldsymbol {c}'_{5} \\ \boldsymbol {c}'_{2} =\boldsymbol {c}_{2} +21h^{2}\boldsymbol {c}'_{4} +150h^{3}\boldsymbol {c}'_{5} \\ \boldsymbol {c}'_{3} =\boldsymbol {c}_{3} -8h\boldsymbol {c}'_{4} -\frac {130}{3} h^{2}\boldsymbol {c}'_{5} \\ \end{cases}\tag{27}\end{equation*}$$ where the coefficients $\boldsymbol {c}_{0}$ , $\boldsymbol {c}_{1}$ , $\boldsymbol {c}_{2}$ , and $\boldsymbol {c}_{3}$ are computed by $\Delta \boldsymbol {\theta }_{1}$ , $\Delta \boldsymbol {\theta }_{2}$ , $\Delta \boldsymbol {\theta }_{3}$ , and $\Delta \boldsymbol {\theta }_{4}$ according to (7).

In view of (17), it can be found that

$$\begin{align*} \boldsymbol {c}'_{2} \times \boldsymbol {c}'_{3}=&\frac {1}{12}\left ({{\boldsymbol {\omega }^{\left ({2 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({3 }\right)}\left ({0 }\right)} }\right) \\=&\frac {1}{12}\left [{ {{\begin{array}{cccccccccccccccccccc} {\omega _{0}^{7} \sin ^{2}\left ({\alpha }\right)} &\quad 0 & \quad 0 \\ \end{array}}} }\right]^{T}\tag{28}\\ \boldsymbol {c}'_{1} \times \boldsymbol {c}'_{4}=&\frac {1}{24}\left ({{\boldsymbol {\omega }^{\left ({1 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({4 }\right)}\left ({0 }\right)} }\right) \\=&\frac {1}{24}\left [{ {{\begin{array}{cccccccccccccccccccc} {-\omega _{0}^{7} \sin ^{2}\left ({\alpha }\right)} &\quad 0 &\quad 0 \\ \end{array}}} }\right]^{T}\tag{29}\\ \boldsymbol {c}'_{0} \times \boldsymbol {c}'_{5}=&\frac {1}{120}\left ({{\boldsymbol {\omega }^{\left ({0 }\right)}\left ({0 }\right)\times \boldsymbol {\omega }^{\left ({5 }\right)}\left ({0 }\right)} }\right) \\=&\frac {1}{120}\left [{ {{\begin{array}{cccccccccccccccccccc} {\omega _{0}^{7} \sin ^{2}\left ({\alpha }\right)} &\quad 0 &\quad {2\omega _{0}^{7} \sin \left ({\alpha }\right)\sin ^{2}\left ({{\alpha /2} }\right)} \\ \end{array}}} }\right]^{T} \!\! \\\tag{30}\end{align*}$$

Considering the relation between the cross products of polynomial coefficients on axis(x) couple with (28), (29), and (30), we have

$$\begin{align*} \boldsymbol {c}'_{1} \times \boldsymbol {c}'_{4}=&-\frac {1}{2}\boldsymbol {c}'_{2} \times \boldsymbol {c}'_{3}\tag{31}\\ \boldsymbol {c}'_{0} \times \boldsymbol {c}'_{5}=&\frac {1}{10}\boldsymbol {c}'_{2} \times \boldsymbol {c}'_{3}\tag{32}\end{align*}$$ In view of (27), it can be found that $$\begin{align*} \boldsymbol {c}_{1} \times \boldsymbol {c}'_{4}=&-\frac {1}{2}\boldsymbol {c}_{2} \times \boldsymbol {c}_{3} +4h\boldsymbol {c}_{2} \times \boldsymbol {c}'_{4} +\frac {65}{3} h^{2}\boldsymbol {c}_{2} \times \boldsymbol {c}'_{5} \\&+\,\frac {21}{2} h^{2}\boldsymbol {c}_{3} \times \boldsymbol {c}'_{4} +75h^{3}\boldsymbol {c}_{3} \times \boldsymbol {c}'_{5} -\frac {911}{3} h^{4}\boldsymbol {c}'_{4} \times \boldsymbol {c}'_{5} \\ \tag{33}\\ \boldsymbol {c}_{0} \times \boldsymbol {c}'_{5}=&\frac {1}{10}\boldsymbol {c}_{2} \times \boldsymbol {c}_{3} -\frac {4}{5} h\boldsymbol {c}_{2} \times \boldsymbol {c}'_{4} -\frac {13}{3} h^{2}\boldsymbol {c}_{2} \times \boldsymbol {c}'_{5} \\&-\,\frac {21}{10} h^{2}\boldsymbol {c}_{3}\times \boldsymbol {c}'_{4} -15h^{3}\boldsymbol {c}_{3} \times \boldsymbol {c}'_{5} +\frac {121}{5} h^{4}\boldsymbol {c}'_{4} \times \boldsymbol {c}'_{5} \\\tag{34}\end{align*}$$

Neglecting the higher-order terms in (32) and (33) yields $\boldsymbol {c}_{1} \times \boldsymbol {c}'_{4} =-1 \mathord {\left /{ {\vphantom {1 2}} }\right. } 2\boldsymbol {c}_{2} \times \boldsymbol {c}_{3}$ and $\boldsymbol {c}_{0} \times \boldsymbol {c}'_{5} =1 \mathord {\left /{ {\vphantom {1 {10}}} }\right. } {10}\boldsymbol {c}_{2} \times \boldsymbol {c}_{3}$ that is

$$\begin{align*} \boldsymbol {c}'_{4}=&-\frac {1}{2}\left ({{\boldsymbol {c}_{1} \times } }\right)^{+}\left ({{\boldsymbol {c}_{2} \times \boldsymbol {c}_{3}} }\right)\tag{35}\\ \boldsymbol {c}'_{5}=&\frac {1}{10}\left ({{\boldsymbol {c}_{0} \times } }\right)^{+}\left ({{\boldsymbol {c}_{2} \times \boldsymbol {c}_{3}} }\right)\tag{36}\end{align*}$$

The coefficients $\boldsymbol {c}'_{0}$ , $\boldsymbol {c}'_{1}$ , $\boldsymbol {c}'_{2}$ , and $\boldsymbol {c}'_{3}$ can be calculated after substituting (35) and (36) into (27). Consequently, the fitted angular rate fifth-order polynomial is given by:

$$\begin{align*} {\hat {\boldsymbol {\omega }}'}\left ({t }\right)=&\boldsymbol {c}'_{0} +\boldsymbol {c}'_{1} t+\boldsymbol {c}'_{2} t^{2}+\boldsymbol {c}'_{3} t^{3}-\frac {1}{2}\left ({{\boldsymbol {c}_{1} \times } }\right)^{+}\left ({{\boldsymbol {c}_{2} \times \boldsymbol {c}_{3}} }\right)t^{4} \\&+\,\frac {1}{10}\left ({{\boldsymbol {c}_{0} \times } }\right)^{+}\left ({{\boldsymbol {c}_{2} \times \boldsymbol {c}_{3}} }\right)t^{5},\quad t\in \left [{ {0~~4h} }\right]\tag{37}\end{align*}$$

E. High-Order Polynomial Based on Arbitrary Samples

Similar to the above-mentioned process of constructing high-order polynomials based on three and four samples, we can construct high-order polynomials based on other samples. If $N=5$ , six independent equations can be constructed based on gyro samples $\Delta \boldsymbol {\theta }_{1}$ , $\Delta \boldsymbol {\theta }_{2}$ , $\Delta \boldsymbol {\theta }_{3}$ , $\Delta \boldsymbol {\theta }_{4}$ , $\Delta \boldsymbol {\theta }_{5}$ , and the numerical relationship between $\boldsymbol {c}'_{2} \times \boldsymbol {c}'_{3}$ and $\boldsymbol {c}'_{0} \times \boldsymbol {c}'_{5}$ on axis(x). Consequently, $\boldsymbol {\omega }\left ({t }\right)$ can be fitted by a polynomial in time up to the order of 5. If $N=6$ , eight independent equations can be constructed based on gyro samples $\Delta \boldsymbol {\theta }_{1}$ , $\Delta \boldsymbol {\theta }_{2}$ , $\Delta \boldsymbol {\theta }_{3}$ , $\Delta \boldsymbol {\theta }_{4}$ , $\Delta \boldsymbol {\theta }_{5}$ , $\Delta \boldsymbol {\theta }_{6}$ , and the numerical relationship between $\boldsymbol {c}'_{3} \times \boldsymbol {c}'_{4}$ and $\boldsymbol {c}'_{1} \times \boldsymbol {c}'_{6}$ , $\boldsymbol {c}'_{3} \times \boldsymbol {c}'_{4}$ and $\boldsymbol {c}'_{0} \times \boldsymbol {c}'_{7}$ on axis(x). Consequently, $\boldsymbol {\omega }\left ({t }\right)$ can be fitted by a polynomial in time up to the order of 7.

Analogously, $\boldsymbol {\omega }\left ({t }\right)$ can be fitted by a polynomial in time up to the order of $N$ or $N+1$ based on $N$ gyro samples when $N$ is odd or even respectively. The high-order polynomial based on 3 to 6 samples are listed in Table 3.

TABLE 3 High-Order Polynomial Based on 3 to 6 Samples

SECTION IV. An Accurate Solution for Quaternion Based on High-Order Polynomial Iteration

A. Quaternion Reconstruction Based on High-Order Polynomial

For simplicity, denote ${\hat {{\boldsymbol {\omega }}}'}\left ({t }\right)=2{\hat {{\boldsymbol {W}}}'}\left ({t }\right)$ and substitute $2{\hat {{\boldsymbol {W}}}'}\left ({t }\right)$ for $\boldsymbol {\omega }\left ({t }\right)$ in (5) yields

$$\begin{align*}&\hspace{-1.2pc}\boldsymbol {q}\left ({{Nh,0} }\right) \\=&\mathbf {1}+\int _{0}^{Nh} {{\hat {{\boldsymbol {W}}}'}\left ({{t_{1}} }\right)} dt_{1} +\int _{0}^{Nh} {\int _{0}^{t_{2}} {{\hat {{\boldsymbol {W}}}'}\left ({{t_{1}} }\right)}} dt_{1} \circ {\hat {{\boldsymbol {W}}}'}\left ({{t_{2}} }\right)dt_{2} \\&+ \, \int _{0}^{Nh} {\int _{0}^{t_{3}} {\int _{0}^{t_{2}} {{\hat {{\boldsymbol {W}}}'}\left ({{t_{1}} }\right)}}} dt_{1} \circ {\hat {{\boldsymbol {W}}}'}\left ({{t_{2}} }\right)dt_{2} \circ {\hat {{\boldsymbol {W}}}'}\left ({{t_{3}} }\right)dt_{3}\! +\!\ldots \\\tag{38}\end{align*}$$

Similar to the process of calculating $\boldsymbol {q}\left ({{Nh,0} }\right)$ by QPI in section II, $\boldsymbol {q}\left ({{Nh,0} }\right)$ can be calculated as follows (39), as shown at the top of the next page,

where ${\boldsymbol {U}'}_{j}^{\left ({k }\right)}$ is the $j$ -th row of the k- integral polynomials coefficients matrix. At the same time, ${\boldsymbol {U}'}_{j}^{\left ({{k+1} }\right)}$ and ${\boldsymbol {U}'}_{j}^{\left ({k }\right)}$ satisfy the equation (40), as shown at the top of the next page.

Thus, ${\boldsymbol {Q}}(Nh)$ can be obtained by substituting (40) into (4).

B. Convergence Analysis

Obviously, (39) builds on an iterative process and $\boldsymbol {q}\left ({{Nh,0} }\right)$ can be iteratively computed. The convergence analysis of the iterative process presented in (39) is essentially same to that of the Picard series shown in (38). The initial quaternion is given by $\boldsymbol {q}\left ({{Nh,0} }\right)=\mathbf {1}$ . As the number of iterations increases, the Picard sequence constructed by (38) is as follows [21], [22]:

$$\begin{equation*} \begin{cases} {\boldsymbol {q}_{0} \left ({{Nh,0} }\right)=\mathbf {1}} \\ \boldsymbol {q}_{l} \left ({{Nh,0} }\right)=\boldsymbol {q}_{0} \left ({{Nh,0} }\right) +\int _{0}^{t} \boldsymbol {q}_{l-1} \left ({{Nh,0} }\right)\\ \quad \qquad \qquad \quad \circ {\hat {{\boldsymbol {W}}}'}(\tau)d\tau, l=1,2,\cdots \end{cases}\tag{41}\end{equation*}$$ where $\boldsymbol {q}_{l} \left ({{Nh,0} }\right)$ is the value of $\boldsymbol {q}\left ({{Nh,0} }\right)$ at the $l$ -th iteration. Consequently, the sequence of $\left \{{{\boldsymbol {q}_{l} \left ({{Nh,0} }\right)} }\right \}$ can be calculated by: $$\begin{align*} \boldsymbol {q}_{l} \left ({{Nh,0} }\right)=\boldsymbol {q}_{0} \left ({{Nh,0} }\right)+\sum \limits _{l=1}^\infty {\left [{ {\boldsymbol {q}_{l} \left ({{Nh,0} }\right)-\boldsymbol {q}_{l-1} \left ({{Nh,0} }\right)} }\right]} \\\tag{42}\end{align*}$$ where $\left |{ \cdot }\right |$ denotes the vector norm. Denote $M=\max \left |{ {\boldsymbol {q}_{0} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)} }\right |$ , $\tau \in \left [{ {{\begin{array}{cccccccccccccccccccc} 0 & {Nh} \\ \end{array}}} }\right]$ , then, $$\begin{align*}&\hspace{-1.2pc}\left |{ {\boldsymbol {q}_{1} \left ({{Nh,0} }\right)-\boldsymbol {q}_{0} \left ({{Nh,0} }\right)} }\right | \\\le&\int _{0}^{Nh} {\left |{ {\boldsymbol {q}_{0} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)} }\right |} d\tau \le MNh \tag{43}\\&\hspace{-1.2pc}\left |{ {\boldsymbol {q}_{2} \left ({{Nh,0} }\right)-\boldsymbol {q}_{1} \left ({{Nh,0} }\right)} }\right | \\\le&\int _{0}^{Nh} {\left |{ {\boldsymbol {q}_{1} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)-\boldsymbol {q}_{0} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)} }\right |} d\tau\tag{44}\end{align*}$$

It is obvious that the functions $\boldsymbol {q}\left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(t)$ satisfy the Lipschitz condition, so there exists a normal number which satisfy the inequality as follows:

$$\begin{align*}&\hspace{-0.5pc} \left |{ {\boldsymbol {q}_{1} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)-\boldsymbol {q}_{0} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)} }\right | \\&\qquad\qquad\qquad\qquad\quad { \le L\left |{ {\boldsymbol {q}_{1} \left ({{Nh,0} }\right)-\boldsymbol {q}_{0} \left ({{Nh,0} }\right)} }\right | } \tag{45}\end{align*}$$

Substituting (45) into (44), we have

$$\begin{align*}&\hspace{-2.5pc} \left |{ {\boldsymbol {q}_{2} \left ({{Nh,0} }\right)-\boldsymbol {q}_{1} \left ({{Nh,0} }\right)} }\right | \\\le&L\int _{0}^{Nh} {\left |{ {\boldsymbol {q}_{1} \left ({{Nh,0} }\right)-\boldsymbol {q}_{0} \left ({{Nh,0} }\right)} }\right |} d\tau \\\le&L\int _{0}^{Nh} {M\tau } d\tau =\frac {ML}{2}\left ({{Nh} }\right)^{2}\tag{46}\end{align*}$$

Assuming that for positive integers $l$ , the $\left |{ {\boldsymbol {q}_{l} \left ({{Nh,0} }\right) }}\right. \left.{{-\boldsymbol {q}_{l-1} \left ({{Nh,0} }\right)} }\right | \le {ML^{l-1}} /{l!}\left ({{Nh} }\right)^{l}$ holds, then

$$\begin{align*}&\hspace {-2pc} \left |{ {\boldsymbol {q}_{l+1} \left ({{Nh,0} }\right)-\boldsymbol {q}_{l} \left ({{Nh,0} }\right)} }\right | \\\le&\int _{0}^{Nh} {\left |{ {\boldsymbol {q}_{l} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)-\boldsymbol {q}_{l-1} \left ({{Nh,0} }\right)\circ {\hat {{\boldsymbol {W}}}'}(\tau)} }\right |} d\tau \\\le&L\int _{0}^{Nh} {\left |{ {\boldsymbol {q}_{l} \left ({{t,0} }\right)-\boldsymbol {q}_{l-1} \left ({{t,0} }\right)} }\right |} d\tau \\\le&\frac {ML^{l}}{l!}\int _{0}^{Nh} {\tau ^{l}} d\tau =\frac {ML^{l}}{\left ({{l+1} }\right)!}\left ({{Nh} }\right)^{l+1}\tag{47}\end{align*}$$

According to mathematical deduction, $\left |{ {\boldsymbol {q}_{l} \left ({{Nh,0} }\right)-}}\right. \left.{{\boldsymbol {q}_{l-1} \left ({{Nh,0} }\right)} }\right |\le {ML^{l-1}} /{l!}\left ({{Nh} }\right)^{l}$ is always tenable. Based on the fact that the series ${ML^{l-1}} / {l!}\left ({{Nh} }\right)^{l}$ is a positive convergence series, according to Weierstrass discriminant [23], it can be readily checked that the series displayed in (42) converges on time the interval $\left [{ {0~~Nh} }\right]$ , so the sequence $\left \{{{\boldsymbol {q}_{l} \left ({{Nh,0} }\right)} }\right \}$ converges on the time interval $\left [{ {0~~Nh} }\right]$ . Besides, it can also be verified that $\dot {{\boldsymbol {q}}}_{\infty } =\boldsymbol {q}_{\infty } \circ {\hat {{\boldsymbol {W}}}'}\left ({t }\right)$ , which means the iterative process in (39) is convergent to the true attitude quaternion function that corresponds to $2{\hat {{\boldsymbol {W}}}'}\left ({t }\right)$ .

C. Calculation Analysis

Based on the fact that the way of calculating $\boldsymbol {q}\left ({{Nh,0} }\right)$ is similar in (12) and (39), we first analyze the cost of computing involved in (12). In practical application, the numerical solution for $\boldsymbol {q}\left ({{Nh,0} }\right)$ obtained by taking the sum of the first $n_{T} +4$ terms on the right side of (12) has high enough numerical accuracy and the truncation error is up to $O\left ({{t^{N+3}} }\right)$ , $n_{T}$ represents the order of the polynomial in (6). Consequently, there is no need to calculate the coefficients of $t^{m}\left ({{m > n_{T} +3} }\right)$ . (12) mainly deals with convolution operation when calculating $\boldsymbol {U}_{j}^{\left ({k }\right)}$ and we mainly discuss the amount of scalar multiplications included in (12).

On the right side of (12), the first term is constant, and the second term equals the sum of the angular increment measurements of gyroscope and does not include scalar multiplications. Considering that $\boldsymbol {U}_{0}^{\left ({1 }\right)}$ equals $\mathbf {0}$ , there are 9 convolution operations in the third term and 12 convolution operations in other terms. In (12), the angular rate polynomial fitting order is $N-1$ . $\boldsymbol {U}_{i}^{\left ({k }\right)} \left ({{i=1,2,3} }\right)$ and $\boldsymbol {W}_{j} \left ({{j=x,y,z} }\right)$ are $k\left ({{N-1} }\right)+1$ and $N$ dimension vector respectively. Neglecting the calculation of the coefficients of $t^{m}\left ({{m > n_{T} +3} }\right)$ , only the last $\left ({{N+2-k} }\right)$ terms of the sequence determined by $\boldsymbol {U}_{i}^{\left ({k }\right)} \ast \boldsymbol {W}_{j}$ are required to be calculated. According to the properties of convolution, the third, fourth, fifth, and sixth term on the right side of (12) involve 8, 6, 3, and 1 scalar multiplications if $N=3$ . Thus the number of scalar multiplications in (12) is $M_{1} =9\times 8+12\times \left ({{6+3+1} }\right)=192$ for $N=3$ . The third, fourth, fifth, sixth and seventh term involve 13, 10, 6, 3, and 1 scalar multiplications if $N=4$ . Thus the number of scalar multiplications in (12) is $M_{1} =9\times 13+12\times \left ({{10+6+3+1} }\right)=357$ for $N=4$ . Similarly, the number of scalar multiplications $M_{1}$ in (12) is 591, 906, 1314, and 1827 for $N=5,6,7,8$ respectively.

In (39), the angular rate polynomial fitting order is $N$ or $N+1$ rather than $N-1$ , thus the number of scalar multiplications $M_{2}$ for $N=3,4,5,6$ in (39) equals that in (12) for $N=4,6,6,8$ . Table 4 presents the comparison of computation cost for QPI and the new algorithm.

TABLE 4 Comparison of Computation Cost

Table 4 shows that the computation cost of the new algorithm is larger than that of QPI algorithm because the increase of angular rate polynomial fitting order.

SECTION V. Simulation Results and Discussion

In this section, the simulated classical coning motion data is used to evaluate the convergence of the new algorithm and the performance of the new algorithm (New) as compared with the optimal coning algorithm (Optimal), QPI, and Fourth4Rot.

The angular rate $\boldsymbol {\omega }\left ({t }\right)$ of the classical coning motion is expressed in (15) and its corresponding theoretical attitude quaternion is given by

$$\begin{equation*} \boldsymbol {q}\left ({t }\right)=\left [{ {{\begin{array}{cccccccccccccccccccc} {\cos \left ({{\alpha \mathord {\left /{ {\vphantom {\alpha 2}} }\right. } 2} }\right)} \\ 0 \\ {\sin \left ({{\alpha \mathord {\left /{ {\vphantom {\alpha 2}} }\right. } 2} }\right)\cos \left ({{\omega _{0} t} }\right)} \\ {\sin \left ({{\alpha \mathord {\left /{ {\vphantom {\alpha 2}} }\right. } 2} }\right)\sin \left ({{\omega _{0} t} }\right)} \\ \end{array}}} }\right]\tag{48}\end{equation*}$$

The variation of the coning half-angle $\alpha$ ranges from $0.0{5}''$ to 90°, and the coning frequency $\omega _{0}$ is set to $2{\pi rad} \mathord {\left /{ {\vphantom {\pi rad s}} }\right. } s$ . The angular increment measurements from gyros is used as inputs and the sampling frequency is set to $100Hz$ .

Defining the increment function of the updating quaternion at each iteration as:

$$\begin{equation*} \boldsymbol {e}_{l} =\left |{ {\boldsymbol {q}_{l} \left ({{Nh,0} }\right)-\boldsymbol {q}_{l-1} \left ({{Nh,0} }\right)} }\right |,\quad l=1,2,3\cdots\tag{49}\end{equation*}$$ where $\boldsymbol {q}_{l} \left ({{Nh,0} }\right)$ is the updating quaternion which equals to the sum of the first $l+1$ terms in (39) and $\boldsymbol {e}_{l}$ is a 4 dimension vector.

Table 5, 6, and 7 present convergence of the increment of the updating quaternion at $l$ -th iteration at the time points $t=0.03s$ for $N=3$ and $\alpha =0.1^{\circ }$ , $\alpha =1^{\circ }$ , $\alpha =90^{\circ }$ respectively.

TABLE 5 Convergence of the Increment of the Updating Quaternion ( $\alpha=0.1^{\circ}$ , $N=3$ )

TABLE 6 Convergence of the Increment of the Updating Quaternion ( $\alpha=1^{\circ}$ , $N=3$ )

TABLE 7 Convergence of the Increment of the Updating Quaternion ( $\alpha=90^{\circ}$ , $N=3$ )

As shown in Table 5–​7, the updating quaternion $\boldsymbol {q}_{4} \left ({{Nh,0} }\right)$ , $\boldsymbol {q}_{6} \left ({{Nh,0} }\right)$ , $\boldsymbol {q}_{10} \left ({{Nh,0} }\right)$ are equal to $\boldsymbol {q}_{\infty } \left ({{Nh,0} }\right)$ for $N=3$ and $\alpha =0.1^{\circ }$ , $\alpha =1^{\circ }$ , $\alpha =90^{\circ }$ respectively and $\boldsymbol {q}_{6} \left ({{Nh,0} }\right)$ can be used to approximate $q_{\infty } \left ({{Nh,0} }\right)$ for arbitrary $\alpha$ . It means that the iterative process in (39) converges absolutely and the updating quaternion error generally decrease as the iteration times increase but the accuracy cannot be improved generally when the iteration times are larger enough. Analyzing other convergence of the increment of the updating quaternion for different samples and cone half-angle, the updating quaternion $\boldsymbol {q}_{n_{T} +3} \left ({{Nh,0} }\right)$ which equals to the sum of the first $n_{T} +4$ terms in (39) has high enough numerical accuracy for arbitrary $N$ samples and $\alpha$ , where $n_{T}$ is the order of the polynomial.

Iteration times $l=n_{T} +3$ is used for all, and Fig. 2–​5 plot the coning drift error $\varepsilon$ of the New, Optimal, QPI, and Fourth4Rot for $N=3$ , $N=4$ , $N=5$ , and $N=6$ respectively. One thing to note here is that the five-sample and six-sample Fourth4Rot have not been studied in the published literature. Thus, the coning drift error $\varepsilon$ of the Fourth4Rot for $N=5$ , and $N=6$ are not included in the following simulations.

FIGURE 2.

Comparison of coning drift error ($N=3$ ).

FIGURE 3.

Comparison of coning drift error ($N=4$ ).

FIGURE 4.

Comparison of coning drift error ($N=5$ ).

FIGURE 5.

Comparison of coning drift error ($N=6$ ).

As shown in Fig. 2–​5, the new algorithm outperforms QPI throughout the cone half-angle range considered for the same samples and shows better performance than optimal coning algorithm and Fourth4Rot for the case of large half-angle coning motion and the same samples. To be specific, the accuracy improvement of the new algorithm over QPI is in an order of 2 and 1 for small cone half-angle and large cone half-angle, respectively. Comparing Fig. 3 and 4, the new algorithm for $N=4$ can achieve higher accuracy than QPI for $N=5$ , which owes to the accurate angular rate approximation by using the higher order polynomial shown in Table 3. Besides, the accuracy of optimal coning algorithm cannot be further improved by increasing the samples for the case of large half-angle coning motion but the new algorithm has higher accuracy with more samples. For $N=6$ and $\alpha =90^{\circ }$ , the new algorithm improves the attitude accuracy by 8 order over the optimal coning algorithm. Compared with Fourth4Rot, the new algorithm improves the attitude accuracy by 3 order for $N=4$ , $\alpha =90^{\circ }$ . This indicates that the new algorithm can effectively compensate non-community error in high dynamic environment.

Fig. 6 and 7 plot the attitude computation error of the four kinds algorithms on non-coning axis for $N=4$ and $\alpha =90^{\circ }$ , $\alpha =1^{\circ }$ over the first three seconds respectively.

FIGURE 6.

Drift error comparison for non-coning axis ($\alpha =90^{\circ }$ , $N=4$ ).

FIGURE 7.

Drift error comparison for non-coning axis ($\alpha =1^{\circ }$ , $N=4$ ).

Fig. 6 and 7 demonstrate that the drift error of the traditional optimal coning algorithm on non-coning axis i.e. axis($y$ ) and axis($z$ ), have large periodical errors compared with that of QPI, the new algorithm, and Fourth4Rot. For example, the maximum error of optimal coning algorithm and QPI on axis($z$ ) for $\alpha =90^{\circ }$ is $74.8{5}''$ , $0.0002{9}''$ , and $0.008{1}''$ , respectively. However, the new algorithm performs significant performance and the maximum on axis($z$ ) is $0.00004{1}''$ , which is $1.83\times 10^{6}$ times smaller than $74.8{5}''$ , 7 times smaller than $0.0002{9}''$ , and 197 times smaller than $0.008{1}''$ . At the same time, the maximum error of the new algorithm on axis($z$ ) for $\alpha =1^{\circ }$ is $- 3.1\times 10^{-11}$ and it is about $6.3\times 10^{6}$ times, 25 times, and 5 times smaller than the maximum error of the optimal coning algorithm, QPI, and Fourth4Rot respectively. This means that the new algorithm can suppress the attitude error more efficiently than the optimal coning algorithm, QPI and Fourth4Rot when the carrier undergoing non-integer-period coning motion.

SECTION VI. Conclusion

This paper displays a new high-accuracy quaternion updating algorithm in the framework of iterative integration of quaternion. From the point of view of reducing the angular rate fitting error, a high-order polynomial model is introduced in the proposed algorithm for the first time, among which the polynomial fitting order is increased to $N$ or $N+1$ up from $N-1$ using $N$ gyro samples. The performance is evaluated by using the simulated classical coning motion data. It has been verified that the new algorithm is capable of showing better performance than pervious algorithms such as the optimal coning algorithm, QPI and Fourth4Rot especially in high dynamic environment owing to the improvement of angular rate fitting accuracy. Consequently, the new algorithm has better capability of adaptation in complex environment and more engineering use significance. Of course, the idea of the new algorithm can be used to calculate other attitude parameters i.e. rotation vector, attitude matrix etc. The proposed high-accuracy algorithm is applicable to inertial sensors of ultra-high precision such as the cold-atom interference gyros as well.

Although the computation cost of the proposed algorithm is bigger than that of traditional attitude updating algorithms, it is not big problem for current computer. Besides, if there exist other constraints which can be introduced in the process of constructing higher-order polynomial, the attitude error will be further reduced based on the higher-order polynomial.