In-Motion Coarse Alignment Method Based on Position Loci and Optimal-REQUEST for SINS

Abstract

In this paper, an improved in-motion coarse alignment method is proposed for a strapdown inertial navigation system (SINS) using position loci obtained from the Global Positioning System (GPS). The difference from the popular coarse alignment methods is that the proposed algorithm uses GPS position loci information to form the vector observation, and does not need velocity information, which expands the application range of in-motion coarse alignment. In addition, this paper utilizes the Optimal-REQUEST algorithm to reduce the influence of random errors contained in the vector observation. The Optimal-REQUEST algorithm is an adaptive iterative updating algorithm, which can adaptively adjust the gain of the filter according to the loss function. Simulation results confirmed that the proposed algorithm can suppress the impact of random errors effectively. The pitch, roll and yaw angles calculated by the proposed algorithm were improved by 51.95%, 53.80% and 63.03% compared with the comparison algorithms.

1. Introduction

A strapdown inertial navigation system (SINS) is an autonomous navigation system that does not rely on any external information. A SINS has the characteristics of excellent concealment because it does not radiate energy to the outside. Thus, it can work in a variety of complex environments, such as air, ground, and underwater [1,2,3]. In practical navigation applications, an integrated navigation system (INS) is usually used to improve navigation accuracy. An INS usually takes a SINS as the main navigation system and the Global Positioning System (GPS) or other systems, where errors do not accumulate with time, are used as auxiliary navigation systems [4,5,6]. Among many INSs, SINS/GPSs have attracted the attention of many researchers because of their wide application range and high efficiency [7]. In SINS/GPS research, initial alignment has been one of the significant focal areas [8].

Initial alignment is the process of calculating the initial attitude, position, and velocity of vehicles [9,10]. In a SINS/GPS integrated system, the initial position and velocity can be easily obtained by GPS. Therefore, the essence of initial alignment is to calculate the initial attitude. In general, initial alignment is divided into two stages: coarse alignment and fine alignment [11]. The coarse alignment period estimates and provides a rough attitude matrix for the fine alignment. Then, the fine alignment uses the liner model and filter technology based on the results of coarse alignment. Thus, the accuracy of coarse alignment greatly affects the accuracy of the whole integrated navigation process [12,13,14].

Several researchers have devoted themselves to devising new coarse alignment algorithms. In [15,16,17], the coarse alignment algorithms are studied based on gravitational apparent motion. The authors of [15] used gravitational apparent motion vectors at three moments to construct an attitude matrix and then to obtain latitude information by calculating the geometric relationship among the apparent motions. However, the method proposed in [15] is easily affected by random noise contained in accelerometer measurement. However, when the measurement time interval is very short, or the measurement quantity is inaccurate, the apparent motion of gravity will be collinear. In order to avoid collinearity among apparent motions, the authors of [16] propose an online sensor data denoising method and study the design of a novel reconstruction method for apparent motion Through the reconstruction of vectors, the method can increase the accuracy of self-alignment. In addition, in [17], a fast self-alignment method with real-time estimated latitude to obtain high accuracy with unknown geographic latitude constraints and limited time is proposed. Unfortunately, the above three coarse alignment methods can only be applied to the static base or swaying base and limit the application range of a SINS. Therefore, researchers have begun to investigate in-motion coarse alignment methods based on an external equipment auxiliary.

When a vehicle is moving, the coarse alignment process cannot be completed using measurement information from inertial sensors alone, and it is necessary to engage the assistance of external equipment. GPS is the most popular auxiliary equipment for land vehicles, and many coarse alignment methods based on GPS have been studied in recent years. The authors of [18] investigated an optimization-based alignment (OBA) algorithm in an innovative way. The OBA algorithm uses infinite vector observations to transform the coarse alignment problem into an attitude determination problem. In [19], the approach reported in [18] is further developed by utilization of devised velocity integration formulae to construct the vector observation. Based on the above two algorithms, the basic problem of in-motion alignment can be solved. Unfortunately, both algorithms cannot suppress the impact of outliers contained in the outputs of GPS and inertial measurement units. Thus, in [20], a robust optimization-based alignment method to detect the outliers and improve the accuracy of in-motion coarse alignment is proposed. It is worth noting that the above in-motion coarse alignment methods are all based on the ground velocity. However, not all GPS receivers can export the ground velocity and some of them can only provide the position information of the vehicle. This limits the application of in-motion coarse alignment. Therefore, ref. [21] investigated an in-motion coarse alignment method for the SINS/GPS integrated system using position loci. The vector observation, based on position trajectory, further expands the application range of coarse alignment on the moving base. However, ref. [21] uses a filter-QUEST algorithm to calculate the attitude matrix, which has only limited ability to reduce the errors of vector observation contained in the inertial sensors and GPS measurement. Furthermore, the propagation noises are treated as fixed fading memory values in the filter-QUEST algorithm, which is similar to the REQUEST algorithm, and this makes both of the algorithms suboptimal.

To improve the performance of in-motion coarse alignment, this paper extends the research of [21] and proposes a new alignment method based on an Optimal-REQUEST algorithm and position loci (OP-PL). This paper employs a vector observation calculation method based on position loci. Then, the Optimal-REQUEST is utilized to suppress the noise in the measurement vectors and to filter the measurement noises by adaptively adjusting the fading memory value. Finally, the objective of improving the accuracy of coarse alignment is achieved. The contributions of the proposed in-motion coarse alignment are summarized as follows:

• The proposed OP-PL utilizes position information to construct the observation vectors, which can expand the application range of the in-motion coarse alignment algorithm, because the proposed method only utilizes the position information which is available from every GPS receiver.
• The outliers contained in the GPS positioning outputs can be suppressed as the outliers can be smoothed by GPS position loci; this can reduce the noise impact of outliers on observation vectors.
• The proposed algorithm employs the Optimal-REQUEST algorithm to further suppress random noise contained in the observation vectors. The Optimal-REQUEST overcomes the defect of the REQUEST algorithm of the use of an empirical and constant gain to filter the random noise. Therefore, the Optimal-REQUEST algorithm can optimally filter random noise using a cost function, and can improve the stability and accuracy of the coarse alignment.

The remainder of the paper is organized as follows: Section 2 introduces the traditional method of in-motion coarse alignment and defines the problem. Next, Section 3 elaborates the principle of vector observation based on the position loci, and Section 4 introduces the proposed Optimal-REQUEST algorithm. The simulation experiments conducted are described in Section 5. Finally, the conclusions and suggestions for future work are presented in Section 6. See Appendix A for the proof of some formulas.

2. Traditional Method of In-Motion Coarse Alignment and Related Work

In this section, the traditional method of in-motion coarse alignment is briefly described. Then, the limits and practical issues of constructing a vector using velocity measurement are introduced.

2.1. Traditional Construction Method Using Velocity Measurement

The core issue for in-motion coarse alignment is to find the initial attitude matrix $C_b^n$. According to the chain rule of the attitude matrix, the initial attitude matrix $C_b^n$ can be divided into three matrices [11]:

$$C_b^n(t)=C_{n(0)}^{n(t)}{C}_{b(0)}^{n(0)}{C}_{b(t)}^{b(0)}$$

(1)

where n-frame denotes the navigation frame aligned with East-North-Up (ENU) geodetic axes, and b-frame denotes the body frame aligned with inertial measurement unit axes, namely right-forward-upper. $C_{n(t)}^{n(0)}$ and $C_{b(t)}^{b(0)}$ encode the attitude changes of the navigation frame and the body frame from the initial time to the current time, respectively. Using the attitude matrix differential equation, $C_{n(t)}^{n(0)}$ and $C_{b(t)}^{b(0)}$ can be calculated as follows:

$${\dot{C}}_{n(t)}^{n(0)}=C_{n(t)}^{n(0)}{\omega }_{in}^n\times$$

(2)

$${\dot{C}}_{b(t)}^{b(0)}=C_{b(t)}^{b(0)}{\omega }_{ib}^b\times$$

(3)

where i-frame denotes the Earth-center-fixed orthogonal reference frame, which does not rotate with the alignment process, ${\omega }_{in}^n$ denotes the rotation of n-frame relative to the i-frame, and ${\omega }_{ib}^b$ denotes the rotation of the b-frame relative to the i-frame. After the above calculation, the determination of $C_b^n$ is translated into the determination of $C_{b(0)}^{n(0)}$, which is a constant matrix during the in-motion coarse alignment.

2.2. Related Work

To calculate the matrix $C_{b(0)}^{n(0)}$, ref. [18] proposes an algorithm using vector observation. Using the traditional vector observation method, we can acquire the measurement vectors and the reference vectors based on the velocity measurement. The specific force equation under the navigation frame is given by [18]:

$${\dot{v}}^n=C_b^n{f}^b-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times v^n+g^n$$

(4)

Combining Equations (1)–(3), the following equation can be obtained:

$$C_{b(0)}^{n(0)}{C}_{b(t)}^{b(0)}{f}^b=C_{n(t)}^{n(0)}{\dot{v}}^n+C_{n(t)}^{n(0)}(2{\omega }_{ie}^n+{\omega }_{en}^n)\times v^n-C_{n(t)}^{n(0)}{g}^n$$

(5)

Then, the reference vectors and the measurement vectors can be acquired, and the initial alignment problem is transformed into an attitude determination problem.

$$C_{b(0)}^{n(0)}{\alpha }_v={\beta }_v$$

(6)

where

$${\alpha }_v={\displaystyle {\int }_0^t{C}_{b(\tau )}^{b(0)}}{f}^{b}d\tau$$

(7)

$${\beta }_v=C_{n(t)}^{n(0)}{v}^n-v^n(0)+{\displaystyle {\int }_0^t{C}_{n(\tau )}^{n(0)}{\omega }_{ie}^n\times v^{n}d\tau -{\displaystyle {\int }_0^t{C}_{n(\tau )}^{n(0)}{g}^{n}d\tau }}$$

(8)

From Equation (8), it can be seen that the measurement vector ${\beta }_v$ is calculated based on the ground velocity $v^n$; this requires that the GPS receiver can output the ground velocity. This construction method for the observation vector is the method reported in many current alignment studies [12,13,14]. However, some GPS receivers can only output the position of the vehicle, and the traditional construction method of vector observation is not suitable for in-motion coarse alignment in this situation. Furthermore, the noise contained in the GPS and inertial sensor measurements inevitably affects the accuracy of observation. Thus, it is necessary to change the construction method of vector observation and expand the application scope of the traditional OBA algorithms.

3. The Principle of Vector Observation Based on Position Loci

In this section, a construction method of vector observation based on position loci is derived, and the detailed recursive formulae are also investigated.

3.1. Position Integration Formula

Based on the principles of inertial navigation, the position and the velocity under the navigation frame are shown as:

$${\dot{p}}^n=R_c{v}^n$$

(9)

where $p^n={\left[\begin{array}{ccc}L& \lambda & h\end{array}\right]}^{\mathrm{T}}$; $L$, $\lambda$, $h$ denote the vehicle’s latitude, longitude, and height, respectively. The local curvature matrix $R_c$ is defined below.

$$R_c=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right]$$

(10)

where $R_M$ and $R_N$ denote the meridian radius and the transverse radius of curvature based on the WGS-84 reference ellipsoid, respectively.

As is well-known, the expression of position integration can be obtained by integrating the velocity. Combining with Equation (9), the specific position integration can be calculated as:

$$r^n={\displaystyle {\int }_0^t{v}^{n}d\tau }={\displaystyle {\int }_0^t{R}_c^{-1}{\dot{p}}^{n}d}\tau$$

(11)

where $r^n(0)=0$.

Based on the relation between position and velocity, Equations (6)–(8) can be reconstructed through integral calculation, as follows:

$$C_{b0}^{n0}{\displaystyle {\int }_0^t{\alpha }_v}d\tau ={\displaystyle {\int }_0^t{\beta }_v}d\tau$$

(12)

The integral calculation formulae on both sides of the equation are shown below [22]:

$${\alpha }_p={\displaystyle {\int }_0^t{\alpha }_v}d\tau ={\displaystyle {\int }_0^t{\displaystyle {\int }_0^{\tau }{C}_{b(\sigma )}^{b0}{f}^{b}d}}\sigma d\tau$$

(13)

$${\beta }_p={\displaystyle {\int }_0^t{C}_{n(\tau )}^{n0}}{\dot{r}}^{n}d\tau -t{v}^n(0)+{\displaystyle {\int }_0^t{\displaystyle {\int }_0^{\tau }{C}_{n(\sigma )}^{n0}}}({\omega }_{ie}^n\times ){\dot{r}}^{n}d\sigma d\tau -{\displaystyle {\int }_0^t{\displaystyle {\int }_0^{\tau }{C}_{n(\sigma )}^{n0}}}{g}^{n}d\sigma d\tau$$

(14)

Comparing Equation (14) with Equation (8), we notice that the measurement vectors are constructed by the GPS position information of the vehicle. Moreover, the effects of outliers, coming from the GPS position information, can be weakened through position-loci integration. Thus, the proposed algorithm is more robust than the traditional algorithms.

3.2. Discrete Recursive Formulae

In the previous subsection, we obtained the vector construction method based on position information. However, all the formulae are continuous and cannot be calculated in practical experiments. Thus, the discrete recursive formulae are studied in this subsection [21].

The reference vector ${\alpha }_p$ can be approximated as below:

$${\alpha }_p(M)={\displaystyle \sum _{k=0}^{M-1}\Delta t_G}{\displaystyle \sum _{m=0}^{k-1}{C}_{b(t_m)}^{b0}}{\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{b(\sigma )}^{b(t_m)}}{f}^{b}d\sigma +{\displaystyle \sum _{k=0}^{M-1}{C}_{b(t_k)}^{b0}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{b(\sigma )}^{b(t_k)}{f}^b}}d\sigma d\tau$$

(15)

where $\Delta t_G$ and $M$ represent the sampling time of GPS and the current time, respectively. Specifically, $t=t_M=M\Delta t_G$, $t_k=k\Delta t_G$ and $t_m=m\Delta t_G$. The integral term can be calculated by the two-sample method, which is as follows:

$${\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{b(\sigma )}^{b(t_m)}}{f}^{b}d\sigma =\Delta {\nu }_1+\Delta {\nu }_2+\frac{1}{2}(\Delta {\theta }_1+\Delta {\theta }_2)\times (\Delta {\nu }_1+\Delta {\nu }_2)+\frac{2}{3}(\Delta {\theta }_1\times \Delta {\nu }_2+\Delta {\nu }_1\times \Delta {\theta }_2)$$

(16)

$${\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{b(\sigma )}^{b(t_k)}{f}^b}}d\sigma d\tau =\frac{\Delta t_G}{30}(25\Delta {\nu }_1+5\Delta {\nu }_2+12\Delta {\theta }_1\times \Delta {\nu }_1+8\Delta {\theta }_1\times \Delta {\nu }_2+2\Delta {\nu }_1\times \Delta {\theta }_1+2\Delta {\theta }_2\times \Delta {\nu }_2)$$

(17)

where

$$\left\{\begin{array}{c}\Delta v_1={\displaystyle \sum _{l=1}^{\frac{D}{2}}{f}_l^b\Delta t_S\begin{array}{cc}& \hspace{1em}\Delta v_2={\displaystyle \sum _{l=\frac{D}{2}+1}^D{f}_l^b\Delta t_S}\end{array}}\\ \Delta {\theta }_1={\displaystyle \sum _{l=1}^{\frac{D}{2}}{\omega }_{ib,l}^b\Delta t_S\hspace{1em}\begin{array}{cc}& \Delta {\theta }_2={\displaystyle \sum _{l=\frac{D}{2}+1}^D{\omega }_{ib,l}^b\Delta t_S}\end{array}}\end{array}\right.$$

(18)

where D denotes the half sampling time of GPS, and $\Delta t_s$ denotes the sampling time of the accelerometers and gyroscopes.

After acquiring the recursive formulae of the reference vector, the measurement vector is derived in detail. The first integration term of Equation (14) is shown below:

$${\displaystyle {\int }_0^t{C}_{n(\tau )}^{n0}}{\dot{r}}^{n}d\tau =C_{n(t)}^{n0}{r}^n-{\displaystyle {\int }_0^t{C}_{n(\tau )}^{n0}}({\omega }_{in}^n\times )r^{n}d\tau =C_{n(t_M)}^{n0}{r}^n-{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{C}_{n(\tau )}^{n(t_k)}}({\omega }_{in}^n\times )r^{n}d\tau$$

(19)

The second integration term can be given by:

$$\begin{array}{c}{\displaystyle {\int }_0^t{\displaystyle {\int }_0^{\tau }{C}_{n(\sigma )}^{n0}}}({\omega }_{ie}^n\times ){\dot{r}}^{n}d\sigma d\tau ={\displaystyle {\int }_0^t{C}_{n(\tau )}^{n0}}({\omega }_{ie}^n\times )r^{n}d\tau -{\displaystyle {\int }_0^t{\displaystyle {\int }_0^{\tau }{C}_{n(\sigma )}^{n0}}}({\omega }_{in}^n\times )({\omega }_{ie}^n\times )r^{n}d\sigma d\tau \hfill \\ ={\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{C}_{n(\tau )}^{n(t_k)}}({\omega }_{ie}^n\times )r^{n}d\tau -{\displaystyle \sum _{k=0}^{M-1}\Delta t_G}{\displaystyle \sum _{m=0}^{k-1}{C}_{n(t_m)}^{n0}}{\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{n(\tau )}^{n(t_m)}}({\omega }_{in}^n\times )({\omega }_{ie}^n\times )r^{n}d\tau \hfill \\ -{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n(t_k)}}}}({\omega }_{in}^n\times )({\omega }_{ie}^n\times )r^{n}d\sigma d\tau \hfill \end{array}$$

(20)

The last integration term is calculated as follows:

$$\begin{array}{c}{\displaystyle {\int }_0^t{\displaystyle {\int }_0^{\tau }{C}_{n(\sigma )}^{n0}}}{g}^{n}d\sigma d\tau ={\displaystyle \sum _{k=0}^{M-1}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_0^{\tau }{C}_{n(\sigma )}^{n0}{g}^{n}d\sigma d\tau }}}={\displaystyle \sum _{k=0}^{M-1}{\displaystyle {\int }_{t_k}^{t_{k+1}}\left({\displaystyle {\int }_0^{t_k}{C}_{n(\sigma )}^{n0}{g}^{n}d\sigma }+{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n0}{g}^{n}d\sigma }\right)d\tau }}\hfill \\ ={\displaystyle \sum _{k=0}^{M-1}\Delta t_G{\displaystyle {\int }_0^{t_k}{C}_{n(\sigma )}^{n0}{g}^{n}d\sigma }+}{\displaystyle \sum _{k=0}^{M-1}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n0}{g}^{n}d\sigma d\tau }}}\hfill \\ ={\displaystyle \sum _{k=0}^{M-1}\Delta t_G{\displaystyle \sum _{m=0}^{k-1}{C}_{n(t_m)}^{n0}{\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{n(\sigma )}^{n(t_m)}}}{g}^{n}d\sigma +{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n(t_k)}{g}^{n}d\sigma d\tau }}\hfill \end{array}$$

(21)

Substituting Equations (19)–(21) into Equation (14), the complete formula for the measurement vector is shown below:

$$\begin{array}{c}{\beta }_p=C_{n(t_M)}^{n0}{r}^n-{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{C}_{n(\tau )}^{n(t_k)}}({\omega }_{in}^n\times )r^{n}d\tau -t{v}^n(0)+{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{C}_{n(\tau )}^{n(t_k)}}({\omega }_{ie}^n\times )r^{n}d\tau \hfill \\ -{\displaystyle \sum _{k=0}^{M-1}\Delta t_G}{\displaystyle \sum _{m=0}^{k-1}{C}_{n(t_m)}^{n0}}{\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{n(\tau )}^{n(t_m)}}({\omega }_{in}^n\times )({\omega }_{ie}^n\times )r^{n}d\tau -{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n(t_k)}}}}({\omega }_{in}^n\times )({\omega }_{ie}^n\times )r^{n}d\sigma d\tau \hfill \\ -{\displaystyle \sum _{k=0}^{M-1}\Delta t_G{\displaystyle \sum _{m=0}^{k-1}{C}_{n(t_m)}^0{\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{n(\sigma )}^{n(t_m)}}}{g}^{n}d\sigma -{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^0}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n(t_k)}{g}^{n}d\sigma d\tau }}\hfill \end{array}$$

(22)

Assuming that the terms related to $({\omega }_{ie}^n\times )$ and $({\omega }_{in}^n\times )$ are so small compared with other terms in Equation (22) that they can be ignored during calculation, then the measurement vectors can be approximately determined as:

$${\beta }_p=C_{n(t_M)}^{n0}{r}^n-t{v}^n(0)-{\displaystyle \sum _{k=0}^{M-1}\Delta t_G{\displaystyle \sum _{m=0}^{k-1}{C}_{n(t_m)}^{n0}{\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{n(\sigma )}^{n(t_m)}{g}^{n}d\sigma }}-{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}}}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n(t_k)}{g}^{n}d\sigma d\tau }}$$

(23)

Meanwhile, the attitude matrix $C_{n(t)}^{n(t_k)}$ can be determined approximately as follows:

$$C_{n(t)}^{n(t_k)}=I+\frac{\sin ‖{\phi }_n‖}{‖{\phi }_n‖}\left({\phi }_n\times \right)+\frac{1-\cos ‖{\phi }_n‖}{{‖{\phi }_n‖}^2}{\left({\phi }_n\times \right)}^2\approx I+\left({\phi }_n\times \right)$$

(24)

where ${\phi }_n\approx {\displaystyle {\int }_{t_k}^t{\omega }_{ie}^{n}d\tau }\approx (t-t_k){\omega }_{in}^n$. The reason for this approximation is that ${\omega }_{in}^n$ slowly changes during the coarse alignment. Further discretizing the Equation (23), we obtain the following formulae.

$$\begin{array}{cc}{\displaystyle {\int }_{t_m}^{t_{m+1}}{C}_{n(\sigma )}^{n(t_m)}{g}^{n}d\sigma }\hfill & \approx {\displaystyle {\int }_{t_m}^{t_{m+1}}\left[I+({\phi }_n\times )\right]g^{n}d\sigma }={\displaystyle {\int }_{t_m}^{t_{m+1}}\left[I+(\sigma -t_m)({\omega }_{in}^n\times )\right]g^{n}d\sigma }\hfill \\ & \approx \left(\Delta t_{G}I+\frac{\Delta t_G^2}{2}({\omega }_{in}^n\times )\right)g^n\hfill \end{array}$$

(25)

$$\begin{array}{cc}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }{C}_{n(\sigma )}^{n(t_k)}{g}^{n}d\sigma d\tau }}\hfill & \approx {\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }\left[I+({\phi }_n\times )\right]g^{n}d\sigma d\tau }}={\displaystyle {\int }_{t_k}^{t_{k+1}}{\displaystyle {\int }_{t_k}^{\tau }\left[I+(\sigma -t_m)({\omega }_{in}^n\times )\right]g^{n}d\sigma d\tau }}\hfill \\ & \approx \left(\frac{\Delta t_G^2}{2}I+\frac{\Delta t_G^3}{6}({\omega }_{in}^n\times )\right)g^n\hfill \end{array}$$

(26)

Finally, we acquire the recursive calculation process for the observation vector based on the formulae described:

$${\beta }_p\approx C_{n(t_M)}^{n0}{r}^n-t{v}^n(0)-{\displaystyle \sum _{k=0}^{M-1}\Delta t_G{\displaystyle \sum _{m=0}^{k-1}{C}_{n(t_m)}^{n0}\left(\Delta t_{G}I+\frac{\Delta t_G^2}{2}({\omega }_{in}^n\times )\right)g^n}-{\displaystyle \sum _{k=0}^{M-1}{C}_{n(t_k)}^{n0}}}\left(\frac{\Delta t_G^2}{2}I+\frac{\Delta t_G^3}{6}({\omega }_{in}^n\times )\right)g^n$$

(27)

$$r^n\approx {\displaystyle \sum _{k=1}^M{R}_c^{-1}(k)(p_k^n-p_{k-1}^n)}$$

(28)

where $p_k^n$ represents the position information of the vehicle at time instant k, and $R_c^{-1}(k)$ is the inverse matrix of Equation (10). It is worth noting that the position information can be acquired through GPS directly.

4. Attitude Determination Using the Optimal-REQUEST Algorithm

Based on the above calculations, models of the observation vector are acquired. However, the results of coarse alignment are inevitably affected by GPS measurement errors in the process of constructing vectors. Thus, an Optimal-REQUEST algorithm, based on position loci (OR-PL), is proposed to reduce the impact of errors and to determine the initial attitude matrix $C_{b(0)}^{n(0)}$. The Optimal-REQUEST algorithm utilizes the measurement vectors and reference vectors to construct a K-matrix. Moreover, an iteration algorithm, which can adjust the gain according to the measurement error, is applied to update the K-matrix. The optimal attitude quaternion can then be obtained by calculating the K-matrix eigenvector corresponding to its largest eigenvalue.

4.1. Measurement Update

When the measurement vectors and reference vectors are obtained based on the position loci, the initial attitude matrix $C_{b(0)}^{n(0)}$ can be determined by minimizing the following formula:

$$\frac{1}{2}{\displaystyle \sum _{i=1}^n{a}_i{‖b_i-A{r}_i‖}^2=1-q^{\mathrm{T}}Kq}$$

(29)

where $b_i=\frac{{\beta }_p}{‖{\beta }_p‖}$, $r_i=\frac{{\alpha }_p}{‖{\alpha }_p‖}$ and ${\displaystyle \sum _{i=1}^n{a}_i}=1$. The matrix $A$ is the initial attitude matrix $C_{b(0)}^{n(0)}$. The construction method of the K-matrix is shown below:

$$K_{k/k}=\left[\begin{array}{cc}{S}_k-{\sigma }_k{I}_3& z_k\\ z_k^{\mathrm{T}}& {\sigma }_k\end{array}\right]$$

(30)

where the parameters in Equation (30) are defined as follows:

$$B_k\triangleq {\displaystyle \sum _{i=1}^n{a}_i{b}_i{r}_i^{\mathrm{T}}}, S_k\triangleq B_k+B_k^{\mathrm{T}}$$

(31)

$$z_k\triangleq {\displaystyle \sum _{i=1}^n{a}_i{b}_i\times r_i}, {\sigma }_k\triangleq tr(B_k)$$

(32)

According to the REQUEST algorithm, the propagation formula of the K-matrix is given by:

$$K_{k+1/k}={\Phi }_k{K}_{k/k}{\Phi }_k^{\mathrm{T}}$$

(33)

During the coarse alignment, the initial attitude matrix $C_{b(0)}^{n(0)}$ stays constant. Thus, the propagation coefficient matrix ${\Phi }_k=I_4$.

When the new observation vectors are obtained at time k + 1, we construct the corresponding incremental K-matrix, namely $\delta K_{k+1}$. The specific construction method is the same as Equations (30)–(32), and the only difference is that the vectors acquired at time k + 1 are used to calculate $\delta K_{k+1}$.

Next, the estimated $K_{k+1/k+1}$ can be updated by the combination of prior estimation $K_{k+1/k}$ and the incremental matrix $\delta K_{k+1}$ at time k + 1, namely,

$$K_{k+1/k+1}=(1-{\rho }_{k+1})\frac{m_k}{m_{k+1}}{K}_{k+1/k}+{\rho }_{k+1}\frac{\delta m_{k+1}}{m_{k+1}}\delta K_{k+1}$$

(34)

where $m_k$, $m_{k+1}$ and $\delta m_{k+1}$ are scalar coefficients, and the specific relationship of the three scalars is as follows:

$$m_{k+1}=(1-{\rho }_{k+1})m_{{}_k}+{\rho }_{k+1}\delta m_{k+1}$$

(35)

The fading memory factor ${\rho }_{k+1}$ is the core of the REQUEST algorithm and the Optimal-REQUEST algorithm. The REQUEST algorithm uses the fading memory concept to treat the measurement noise, and this makes the algorithm suboptimal. However, the proposed OP-PL algorithm can optimally filter the measurement noise by calculating the value of the fading memory factor adaptively.

4.2. Stochastic Models

To deduce the error models of the proposed OP-PL algorithm accurately, we assume that the reference vector $r_{k+1}$ is error-free and that the measurement vector $b_{k+1}$ is corrupted by the noisy vector $\delta b_{k+1}$.

$$b_{k+1}=A_{k+1}{r}_{k+1}+\delta b_{k+1}$$

(36)

Define the following noise matrix:

$$V_{k+1}=\left[\begin{array}{cc}{S}_b-{\sigma }_b{I}_3& z_b\\ z_b^{\mathrm{T}}& {\sigma }_b\end{array}\right]$$

(37)

where the parameters in Equation (37) are defined as:

$$B_b\triangleq {\displaystyle \sum _{i=1}^n{a}_i\delta b_{k+1}^{}(r_{k+1}^{}}{)}^{\mathrm{T}}, S_b\triangleq B_b+B_b^{\mathrm{T}}$$

(38)

$$z_b\triangleq {\displaystyle \sum _{i=1}^n{a}_i\delta b_{k+1}^{}\times r_{k+1}^{}}, {\sigma }_b\triangleq tr(B_b)$$

(39)

It is worth noting that $V_{k+1}$ is made up of $\delta b_{k+1}^{}$ and $r_{k+1}^{}$, and $\delta b_{k+1}^{}$ is random. Thus, we conclude that $V_{k+1}$ is also random. The linear relation of $V_{k+1}$ in $\delta b_{k+1}^{}$ is used to deduce a stochastic model for $V_{k+1}$ and $\delta b_{k+1}^{}$. The relationship between $\delta K_{k+1}$ and $V_{k+1}$ is given by:

$$\delta K_{k+1}=\delta K_{k+1}^o+V_{k+1}$$

(40)

where $\delta K_{k+1}^o$ denotes the error-free matrix.

The mean and the covariance of $\delta b_k^{}$ are constructed as follows:

$$E\left[\delta b_k\right]=0$$

(41)

$$E\left[\delta b_k\delta b_{k+i}^{\mathrm{T}}\right]={\mu }_k(I_3-b_k{b}_{k+i}^{\mathrm{T}}){\delta }_{k,k+i}$$

(42)

where ${\delta }_{k,k+i}$ represents the Kronecker delta function and ${\mu }_k$ represents the variance of the component of $\delta b_k^{}$. It is obvious that $\delta b_k^{}$ is white noise with zero-mean. Thus, $V_k$ also has the characteristics of zero-mean white noise, namely,

$$E\left[V_k{V}_{k+i}^{\mathrm{T}}\right]=0$$

(43)

where $i\ne 0$. To further derive the uncertainty of the matrix, we introduce a special form of uncertainty, namely $P_{xx}$. The proof of $P_{xx}$ is shown in Appendix A.

The P-matrix of $V_k$ is given by:

$${\Re }_k=E\left[V_k{V}_k^{\mathrm{T}}\right]=\left[\begin{array}{cc}{\Re }_{11}& {\Re }_{12}\\ {\Re }_{21}& {\Re }_{22}\end{array}\right]$$

(44)

The calculation formulae of each element in the matrix are shown below and the detailed derivation process is shown in [23].

$${\Re }_{11}=\frac{{\mu }_k}{n_k}{\displaystyle \sum _{i=1}^{n_k}\left[3-{(r_i^{\mathrm{T}}{b}_i)}^2\right]I_3}+(b_i^{\mathrm{T}}{r}_i)(b_i{r}_i^{\mathrm{T}}+r_i{b}_i^{\mathrm{T}})+(r_i\times )(b_i{b}_i^{\mathrm{T}}){(r_i\times )}^{\mathrm{T}}$$

(45)

$${\Re }_{12}=0$$

(46)

$${\Re }_{21}=0^{\mathrm{T}}$$

(47)

$${\Re }_{22}=\frac{2{\mu }_k}{n_k}$$

(48)

where ${\mu }_k$ is the variance for all the $n_k$ simultaneous observations calculated at time $t_k$.

4.3. Calculation of Optimal Gain

After the measurement update formulae and error models are deduced, the cost function can be determined through the estimated errors of the K-matrix. Next, the fading memory factor ${\rho }_{k+1}$ can be updated adaptively according to the cost function.

We define the estimation errors of the K-matrix as:

$$\Delta K_{k+1/k}\triangleq K_{k+1/k}^o-K_{k+1/k}$$

(49)

$$\Delta K_{k+1/k+1}\triangleq K_{k+1/k+1}^o-K_{k+1/k+1}$$

(50)

where $K_{k+1/k}^o$ and $K_{k+1/k+1}^o$ denote the a priori and the a posteriori estimation with error-free, respectively. $\Delta K_{k+1/k}$ and $\Delta K_{k+1/k+1}$ denote the errors of the a priori and the a posteriori estimation.

Similarly to Equation (34), reconstruct the equation with the error-free K-matrix, namely:

$$K_{k+1/k+1}^o=(1-{\rho }_{k+1})\frac{m_k}{m_{k+1}}{K}_{k+1/k}^o+{\rho }_{k+1}\frac{\delta m_{k+1}}{m_{k+1}}\delta K_{{}_{k+1}}^o$$

(51)

After subtracting Equations (34) and (51), substitute Equations (40), (49) and (50) into the following formula:

$$\Delta K_{k+1/k+1}=(1-{\rho }_{k+1})\frac{m_k}{m_{k+1}}\Delta K_{k+1/k}+{\rho }_{k+1}\frac{\delta m_{k+1}}{m_{k+1}}{V}_{k+1}$$

(52)

The P matrix of $\Delta K_{k+1/k}$ and $\Delta K_{k+1/k+1}$ are defined as below

$$P_{k+1/k}\triangleq E\left[\Delta K_{k+1/k}\Delta K_{k+1/k}^{\mathrm{T}}\right]$$

(53)

$$P_{k+1/k+1}\triangleq E\left[\Delta K_{k+1/k+1}\Delta K_{k+1/k+1}^{\mathrm{T}}\right]$$

(54)

Considering Equation (52), the following formula can be calculated as:

$$\begin{array}{l}\Delta K_{k+1/k+1}\Delta K_{k+1/k+1}^{\mathrm{T}}={\left[(1-{\rho }_{k+1})\frac{m_k}{m_{k+1}}\right]}^2\Delta K_{k+1/k}\Delta K_{k+1/k}^{\mathrm{T}}\\ -(1-{\rho }_{k+1}){\rho }_{k+1}\frac{m_k\delta m_{k+1}}{m_{k+1}^2}(\Delta K_{k+1/k}{V}_{k+1}^{\mathrm{T}}+V_{k+1}^{}\Delta K_{k+1/k}^{\mathrm{T}})+({\rho }_{k+1}\frac{\delta m_{k+1}}{m_{k+1}}{)}^2{V}_{k+1}^{}{V}_{k+1}^{\mathrm{T}}\end{array}$$

(55)

As mentioned, $\delta b_k^{}$ and $V_k$ represent the white noise process with zero-mean. Moreover, $\Delta K_{k+1/k}$ consists of $V_i$, where i is an integer from 1 to k. Thus, $\Delta K_{k+1/k}$ has no relation with $V_{k+1}^{}$, namely,

$$E\left[\Delta K_{k+1/k}{V}_{k+1}^{\mathrm{T}}\right]=0$$

(56)

Then, we take the expectation of Equation (55), and simplify the following formulae:

$$P_{k+1/k+1}\triangleq E\left[\Delta K_{k+1/k+1}\Delta K_{k+1/k+1}^{\mathrm{T}}\right]={\left[(1-{\rho }_{k+1})\frac{m_k}{m_{k+1}}\right]}^{2}E\left[\Delta K_{k+1/k}\Delta K_{k+1/k}^{\mathrm{T}}\right]+{({\rho }_{k+1}\frac{\delta m_{k+1}}{m_{k+1}})}^{2}E\left[V_{k+1}^{}{V}_{k+1}^{\mathrm{T}}\right]$$

(57)

$$P_{k+1/k+1}={\left[(1-{\rho }_{k+1})\frac{m_k}{m_{k+1}}\right]}^2{P}_{k+1/k}+{({\rho }_{k+1}\frac{\delta m_{k+1}}{m_{k+1}})}^2{\Re }_{k+1}$$

(58)

Based on Equation (58), we define the cost function $J_{k+1}$:

$$J_{k+1}=tr(E\left[\Delta K_{k+1/k+1}\Delta K_{k+1/k+1}^{\mathrm{T}}\right])=tr(P_{k+1/k+1})$$

(59)

The calculation of the fading memory factor is then transformed by solving the minimization problem of Equation (59). We use the derivation to obtain the expression of the fading memory factor.

$$J_{k+1}({\rho }_{k+1})={\left[(1-{\rho }_{k+1})\frac{m_k}{m_{k+1}}\right]}^{2}tr(P_{k+1/k})+{({\rho }_{k+1}\frac{\delta m_{k+1}}{m_{k+1}})}^{2}tr({\Re }_{k+1})$$

(60)

$$\frac{d{J}_{k+1}}{d{\rho }_{k+1}}=2\left[{\left(\frac{m_k}{m_{k+1}}\right)}^{2}tr(P_{k+1/k})+{\left(\frac{\delta m_{k+1}}{m_{k+1}}\right)}^{2}tr({\Re }_{k+1})\right]{\rho }_{k+1}-2{\left(\frac{m_k}{m_{k+1}}\right)}^{2}tr(P_{k+1/k})=0$$

(61)

Finally, the stationary point ${\rho }_{{}_{k+1}}^{*}$ can be determined using the extremum of $J_{k+1}$.

$${\rho }_{{}_{k+1}}^{*}=\frac{m_k^{2}tr(P_{k/k})}{m_k^{2}tr(P_{k/k})+\delta m_{k+1}^{2}tr({\Re }_{k+1})}$$

(62)

4.4. Algorithm Summary

Compared with the REQUEST algorithm, the OP-PL algorithm uses the updated estimation error to define the special cost function and calculates the scalar gain ${\rho }_{{}_{k+1}}^{*}$. Thus, the OP-PL can better reduce the impact of GPS measurement errors which are contained in the observation vectors. For clear explanation, the whole in-motion coarse alignment procedure is shown in

Table 1. In-motion coarse alignment based on the OP-PL algorithm.

Initialization	$\mathit{k}=0$$, {\mathit{m}}_0=\mathit{\delta }{\mathit{m}}_0$$, {\mathit{\rho }}_0^{*}=1$$, {\mathbf{C}}_{\mathit{n}(0)}^{\mathit{n}0}={\mathbf{C}}_{\mathit{b}(0)}^{\mathit{b}0}={\mathbf{I}}_3$$, {\mathbf{K}}_{0/0}=\mathit{\delta }{\mathbf{K}}_0$$, {\mathbf{P}}_{0/0}={\mathbf{\Re }}_0$
Step 1	$k=k+1$
Step 2	$\mathrm{Update} C_{n(t)}^{n(0)}$ and $C_{b(t)}^{b(0)}$ using Equations (2) and (3)
Step 3	Calculate and normalize ${\alpha }_p$ using Equations (15)–(18)
Step 4	Calculate and normalize ${\beta }_p$ using Equations (27) and (28)
Step 5	Calculate $\delta K_k$ and ${\Re }_k$ using Equations (30)–(32) and (44)–(48)
Step 6	Calculate ${\rho }_{{}_k}^{*}$ and $m_k$ using Equations (62) and (35)
Step 7	Update $K_{k/k}$ using Equation (34)
Step 8	Calculate the attitude matrix $C_{b(0)}^{n(0)}$
Step 9	Calculate the attitude matrix $C_{b(k)}^{n(k)}$
Step 10	Back to the Step 1 until the end

and Figure 1.

5. Simulation

The simulation tests carried out to verify the accuracy and rapidity of the proposed OP-PL algorithm are described in this section. The trajectory of the vehicle was designed to follow a zigzag movement, and the actual information of the vehicle was known. Thus, the actual angles were used as reference angles to calculate the errors of three angles. The zigzag trajectory and the reference information are shown in Figure 2 and Figure 3, respectively. It can be seen that the outliers, which corrupt the measurement of GPS, were added to the measurement. The output frequencies of GPS and the simulated inertial measurement unit (IMU) were set as 5 Hz and 200 Hz, respectively. The constant bias and random noise of the gyroscopes and accelerometers are listed in

Table 2. Parameter setting of inertial measurement units.

Sensors	Constant Bias	Random Noise
Gyroscopes	0.01 deg/h	$0.005 \mathrm{deg}/\sqrt{\mathrm{h}}$
Accelerometer	${100}_{} \mathsf{\mu }\mathrm{g}$	${100}_{} \mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}$

. The initial latitude and longitude were set as 32.00° and 118.00°, respectively. Throughout the in-motion coarse alignment process, it was assumed that the vehicle was moving on the horizontal plane; the initial values of the attitude angles were all set as 0°. The simulation was conducted using Matlab 2016b with an alignment time of 300 s. The proposed OP-PL algorithm was compared with the QUEST algorithm and the REQUEST algorithm to verify performance.

Figure 4, Figure 5 and Figure 6 show the different performances of the REQUEST algorithm and the proposed OP-PL algorithm. We chose three different values of the fading memory factor, which were 0.1, 0.01 and 0.001, to compare with the optimal adaptive gain ${\rho }^{*}$. Figure 7 shows the optimal gain of the proposed algorithm in the simulation. Considering the results from three angles, we found that the REQUEST algorithm was unstable during simulation. From Figure 4 and Figure 5, it is evident that the REQUEST algorithm showed similar performance to the proposed algorithm when 0.1 was chosen as the value of the fading memory factor. The ‘MEAN’ and ‘RMSE’ represent the mean and the root mean square error of the attitude errors. The MEAN and RMSE errors of the blue and black curves were calculated by collecting data from 150 s to 300 s, where the blue curve represents $\rho =0.1$ and the black curve represents the optimal adaptively gain. The pitch MEAN errors of the blue and black curves were 0.0269° and −0.0760°, respectively. The pitch RMSE errors of the blue and black curves were 0.0425° and 0.0824°, respectively. Although the accuracy of the pitch angle calculated by the proposed OP-PL algorithm was lower than the REQUEST, the convergence rate of the pitch angle calculated by the proposed OP-PL algorithm was half that calculated by the REQUEST algorithm.

Moreover, the roll MEAN errors of the blue and black curves were 0.1875° and 0.1159°, respectively. The roll RMSE errors of the blue and black curves were 0.2114° and 0.1210°, respectively. From the RMSE data, it is evident that the proposed OP-PL algorithm performed better than the REQUEST algorithm for the calculation of the roll angle. It can also be seen from Figure 5 that the proposed OP-PL algorithm was very stable and that the convergence speed was still very fast.

However, when $\rho =0.1$, the performance of the yaw angle was very poor. Although this value can produce excellent results in calculating the roll angle and the pitch angle, it was completely wrong for calculating the yaw angle. This indicates that the fixed gain was unreliable during the coarse alignment. When the value of $\rho$ was selected as 0.1, the new K-matrix ($\delta K_{k+1}$) accounted for a relatively large proportion during the update of the K-matrix. This made it impossible to calculate the yaw angle correctly. If the value of $\rho$ was decreased to 0.001, more error effects were accumulated because the old K-matrix ($K_{k+1/k}$) accounted for a relatively large proportion during the update of the K-matrix. This made it difficult to calculate the pitch and roll angles correctly.

To overcome the drawbacks of the REQUEST algorithm, the proposed algorithm uses a cost function to adjust the fading memory value adaptively. From Figure 7, we can see that, the value is selected as one at the beginning of the filter. Thus, more parts of the new measurements can be used during the update stage to speed up convergence. With increase in alignment time, a relatively low weight is assigned to the new measurement to ensure that the alignment process is steady.

To further verify the superiority of the proposed OP-PL algorithm, we chose a third algorithm for comparison. Based on the obtained simulation results, we chose $\rho =0.001$ as the fading memory factor in the REQUEST algorithm. Figure 8, Figure 9 and Figure 10 show the calculation results of the REQUEST, QUEST and the proposed OP-PL algorithm, which are represented by the yellow, green and brown curves, respectively. To better compare the errors among the three algorithms, we list the mean values and root mean square errors of the angle error values in

Table 3. Error statistics of three algorithms in simulation test.

Items			REQUEST	QUEST	OP-PL
Pitch (deg)	1~150 s	MEAN	−14.7921	−1.7156	−2.3169
		RMSE	16.4321	3.5995	4.1962
	150~300 s	MEAN	−36.1971	0.1662	−0.0760
		RMSE	37.5654	0.1715	0.0824
Roll (deg)	1~150 s	MEAN	−11.6315	1.8145	−0.3420
		RMSE	14.3065	12.6798	1.5843
	150~300 s	MEAN	−29.8454	−0.2526	0.1159
		RMSE	31.4920	0.2619	0.1210
Yaw (deg)	1~150 s	MEAN	2.0683	5.7750	10.8685
		RMSE	2.6878	63.6506	57.2915
	150~300 s	MEAN	8.9417	3.5183	−0.9181
		RMSE	9.4477	3.7871	1.4000

, and present a box plot which is shown in Figure 11.

In Figure 8 and Figure 9, the performance of the REQUEST algorithm can be seen to be still poor compared with the QUEST and the proposed OP-PL algorithm. The reason for this situation is that the REQUEST is a suboptimal algorithm, and it is hard to find an appropriate value to keep the algorithm stable. Figure 11 confirms the poor performance of REQUEST, where the three angle errors were obviously larger than for the other two algorithms. The MEANs and RMSEs of the pitch and roll angles, which were calculated by the REQUEST algorithm, were also the worst among the three algorithms.

Moreover, the OP-PL algorithm retains the merits of the REQUEST algorithm, as it is a time-varying recursive attitude quaternion estimator which preserves the quaternion unit-norm property. Compared with the QUEST algorithm, the OP-PL algorithm has an enhanced ability to reduce the impact of the measurement noise contained in the measurement vectors.

From Figure 8, Figure 9 and Figure 10, we can see that, although the convergence speed of the OP-PL algorithm was the same as that of the QUEST algorithm, the proposed algorithm had higher accuracy and the accuracy of the three angles were all improved. Moreover, as can be seen from Figure 11, although the outliers of the yaw angle calculated by OP-PL were similar to that calculated by the QUEST algorithm, the proposed algorithm was relatively stable, based on the calculation results from the three angles. The main cause of the outliers in the yaw angle was unstable performance from 0 to 50 s. After the algorithm became stable, the advantages of the proposed algorithm were still evident. The pitch angle, roll angle and yaw angle calculated by the OP-PL algorithm were reduced by approximately 51.95%, 53.80% and 63.03%, respectively, compared with the QUEST algorithm.

Therefore, the above analyses confirm that the OP-PL algorithm had better alignment accuracy than the comparison algorithms.

6. Conclusions and Future Work

This paper proposes an improved in-motion coarse alignment method based on position loci and an Optimal-REQUEST algorithm. The proposed algorithm only uses position information to construct the observation vector, which greatly expands the application scope of the coarse alignment algorithm because all GPS receivers can output the position information. Moreover, the effects of random noise contained in the vector observation are suppressed by the Optimal-REQUEST algorithm which can choose the filter gain adaptively. The results of simulation tests showed that the proposed OP-PL algorithm converged faster and had better accuracy than the comparison algorithms. The pitch angle, roll angle and yaw angle calculated by the OP-PL algorithm were reduced by approximately 51.95%, 53.80% and 63.03%, respectively. However, our work still has some limitations and areas that can be improved. In future work, we plan to verify our proposed algorithm in real vehicle experiments and to extend the application scope of the algorithm to underwater environments.