A Novel Alignment Method for SINS with Large Misalignment Angles Based on EKF2 and AFIS

Abstract

In order to achieve the fine alignment of strapdown inertial navigation (SINS) under large misalignment angles, a novel filtering alignment method is proposed based on the second-order extended Kalman filter (EKF2) and adaptive fuzzy inference system (AFIS). Firstly, the quaternion is employed to represent the attitude errors of SINS. A second-order nonlinear state equation is made based on the nonlinear velocity error model and attitude error model, and the linear measurement equation is based on the velocity outputs from SINS. Then, the filtering scheme is designed based on EKF2 and AFIS. The error estimation and fine alignment can be achieved by using the proposed filtering scheme. The results of Monte Carlo Simulation show that the errors of pitch, roll and yaw misalignment angles quickly decrease to about 14″, 15″ and 7.62′ respectively in 350 s under the condition of any misalignment angles with pitch error from −40° to 40°, roll error from −40° to 40°, and yaw error from −50° to 50°. Even when the initial misalignment angles are all very large such as (80°, 120°, 170°), the proposed nonlinear alignment method still can converge normally by utilizing the adaptive fuzzy inference system (AFIS) to adjust the covariance matrix P_k/k−1. Finally, the turntable experiment was performed, and the effectiveness and superiority of the proposed method were further verified by compared with other nonlinear methods.

1. Introduction

The accuracy of strapdown inertial navigation system (SINS) depends largely on the initial alignment [1]. However, under the conditions of low-accuracy inertial sensors or large environmental disturbance, the accuracy of coarse alignment may be very poor, and the fine alignment based on Kalman filter (KF) are no longer applicable [2]. There are some publications on the subject of inertial navigation system (INS) initial alignment on the swaying base, for example, reference [3] proposed a coarse alignment method of marine strapdown INS based on the trajectory fitting of gravity movement in the inertial space, which could avoid the loss of accuracy caused by rocking disturbances. However, there are still some important special application scenarios, for which only the nonlinear alignment methods with the consideration of large initial misalignment angles are applicable: (1) the time of coarse alignment is too short, even if the inertial sensors are sufficiently accurate, the residual misalignment errors could not meet the requirement of small angles for the usage of linear models. In this case, the fine alignment based on the linear error model is difficult to converge to the ultimate precision. For example, when the missile launch vehicle needs to transfer urgently under the threat of artillery attacks, enough time cannot be obtained for the coarse alignment of SINS on the quasi-stationary base. (2) The working environment requires that the fine alignment without coarse alignment process must be achieved directly under the condition of roughly setting the initial attitude, such as missile-borne SINS. With the development of new theories, such as nonlinear filter, robust filter, adaptive estimation, fuzzy inference, and neural network, some approaches combining these new theories with nonlinear filtering method have been proposed for SINS alignment with large misalignment angles. For SINS initial alignment under large misalignment angles, Some alignment methods based on nonlinear filters were designed to improve the estimation precision and convergence speed, which adopted the multi-resetting Kalman filter [4], extended Kalman filter (EKF) [5], scaled unscented Kalman filter (UKF) [5], UKF [6], particle filter (PF) [7], UKF/KF combined filter [8], transformed unscented quadrature Kalman filter (TUQKF) [9], heuristic Kalman filter [10], respectively. In [11] and [12], the methods of SINS in-motion alignment under large misalignment uncertainty were presented, and the nonlinear filters of EKF, UKF, unscented particle filter (UPF), Gaussian particle filter (GPF), Rao-Blackwellized unscented Kalman filter (RBUKF), Rao-Blackwellized unscented particle filter (RBUPF) and Rao-Blackwellized Gaussian particle filter (RBGPF) were compared and analyzed. Under the condition of large misalignment angles, several novel SINS in-motion initial alignment methods using square root nonlinear filters were proposed to improve the filtering stability, which utilized the square root unscented Kalman filter (SRUKF) [13,14], square root cubature information filter (SR-CIF) [15] and square root simplex unscented quadrature Kalman filter (SR-SUQKF) [16], respectively. The adaptive alignment methods based on the Sage–Husa adaptive filter [17] and simplified Sage–Husa adaptive filter [18] were proposed, which could raise the efficiency and adaptability. Aiming at the dynamic disturbance, adaptive Kalman filter was developed, which could be used for the alignment of marine mooring rotary SINS [19] and airborne Micro-Electro-Mechanical-System (MEMS) SINS [20]. By combining nonlinear filters with adaptive estimation, some approaches based on adaptive nonlinear filters were proposed for SINS fine alignment under the conditions of large initial misalignment angles and unknown noise statistics, of which the adaptive nonlinear filters included the adaptive UKF (AUKF) [21,22,23], adaptive cubature Kalman filter (ACKF) [24,25], adaptive unscented particle filter (AUPF) [26,27], adaptive Lie group filter [28], etc. For SINS initial alignment with the nonlinear error model and disturbance noise uncertainty, the H∞ filter was adopted [29,30,31]; thus, the accuracy and robustness of alignment were improved. For SINS initial alignment under inaccurate system model and non-Gaussian observation noise, various fading nonlinear filters were proposed to improve the filtering robustness and convergence, which included the fading UKF filter [32], robust fading CKF filter [33], robust H-infinity CKF/KF hybrid filter (RHCHF) [34], robust adaptive cubature particle filter [35], robust state-dependent Riccati equation (SDRE) filter [36], etc. Combined with fuzzy inference system (FIS), the initial alignment schemes based on fuzzy adaptive filters, such as the fuzzy adaptive Kalman filter [37], fuzzy simplified UKF [38] and fuzzy strong tracking UKF [39,40], etc., were proposed to improve the performance of SINS initial alignment under large alignment angles and disturbance. In [41,42,43,44,45], the SINS initial alignment methods utilizing neural networks were researched, which could be applied for the application scenarios with disturbances, for example, Shipborne, guided projectiles, and so on. In [46,47], two machine learning methods were proposed to achieve the nonlinear initial alignment of SINS under the condition of large misalignment angles, of which one was based on Gaussian process regression (GPR) [46], the other utilized a combination of Gaussian mixture model (GMM), expectation–maximization (EM), and UKF filter [47]. In order to reduce the effects of nonlinear errors, the nonlinear error modeling technique based fast orthogonal search (FOS) was introduced, which have been applied to the radar (RAD)/reduced inertial sensor system (RISS) integration [48], fine frequency estimation of time and code division-orthogonal frequency division multiplexing (TC-OFDM) receivers [49], INS/global navigation satellite system (GNSS) integrated navigation systems [50] and MEMS inertial sensors in mobile devices [51]. However, almost all of these nonlinear alignment methods mentioned above have some shortcomings such as complex algorithm, heavy computational load, difficulties in parameter optimization, insufficient stability, and poor accuracy, etc.

In this paper, a novel initial alignment algorithm based on the second-order extended Kalman filter (EKF2) via adaptive fuzzy inference system (AFIS) is proposed to solve the problem of SINS initial alignment with large misalignment angles. By utilizing the quaternion representation of attitude, the nonlinear error equations of SINS are derived, and the state equation is obtained. On this basis, the velocity information of SINS is used to establish the measurement equation of the nonlinear alignment with large misalignment angles. The adaptive fuzzy inference system and parameter adaptive estimation are used to assist the second-order EKF filter, also referred to as the second-order EKF filter assisted by adaptive fuzzy inference system (‘AFIS-EKF2’ or ‘EKF2 via AFIS’ for short), and the algorithm of EKF2 via AFIS is designed. The filtering process of EKF2 via AFIS is performed to obtain the estimation of misalignment angles, and the fine alignment of SINS with large misalignment angles is achieved. As a result, the proposed large misalignment angle alignment algorithm is effective in achieving the SINS fine alignment with large alignment angles.

The rest of this paper is organized as follows: Section 2 derives the nonlinear error equations of SINS with large misalignment angles by using the quaternion representation of attitude. In Section 3, the filtering model of SINS nonlinear alignment is established by using the velocity information of SINS as the measurement of nonlinear estimation, and the algorithm of simplified second-order EKF filter is designed. Section 4 presents the strong tracking strategy and fuzzy adaptive parameter adjustment method. In Section 5, Experiment setup and result analysis are provided, which include the simulations of fine alignment on stationary base/swaying base and the experiment on three-axis turntable. In addition, the content of this paper is summarized in Section 6.

2. Nonlinear Error Equations of SINS with Large Misalignment Angles

The reference frames in use are denoted as: the script $i$ denotes the Earth-Centered Inertial (ECI) frame, $n$ denotes the navigation frame (East–North–Up, ENU), $e$ denotes the Earth-Centered Earth-Fixed frame (ECEF), and $b$ denotes the body-fixed frame. In order to distinguish the error-free variable from the corresponding variable with error, different variants of the same script are used to represent the true value, calculated value and measured value, respectively. For example, the script $x$ denotes the error-free true value of any variable, then $x$ hat ($\widehat{x}$) and $x$ tilde ($\tilde{x}$) denote the corresponding calculated value and measured value, respectively.

2.1. Attitude Error Equation

When no error is considered, the differential equation of attitude quaternion is expressed as

$$\dot{\mathit{Q}}=\frac{1}{2}\mathit{Q}\otimes {\mathit{\omega }}_{nb}^b$$

(1)

When considering various error resources, the differential equation of attitude quaternion is expressed as

$$\dot{\widehat{\mathit{Q}}}=\frac{1}{2}\widehat{\mathit{Q}}\otimes {\widehat{\mathit{\omega }}}_{nb}^b$$

(2)

where $\mathit{Q}$ represents the true value of attitude quaternion, $\widehat{\mathit{Q}}$ represents the calculated value of SINS attitude quaternion, $\otimes$ represents the multiplication of quaternions, ${\mathit{\omega }}_{nb}^b$ represents the angular velocity of the b-frame relative to the n-frame expressed in the b-frame, ${\widehat{\mathit{\omega }}}_{nb}^b$ represents the calculated value of ${\mathit{\omega }}_{nb}^b$.

The calculating method of ${\mathit{\omega }}_{nb}^b$ and ${\widehat{\mathit{\omega }}}_{nb}^b$ is as follows:

$${\mathit{\omega }}_{nb}^b={\mathit{\omega }}_{ib}^b-{\mathit{Q}}^{*}\otimes {\mathit{\omega }}_{in}^n\otimes \mathit{Q}$$

(3)

$${\widehat{\mathit{\omega }}}_{nb}^b={\tilde{\mathit{\omega }}}_{ib}^b-{\widehat{\mathit{Q}}}^{*}\otimes {\widehat{\mathit{\omega }}}_{in}^n\otimes \widehat{\mathit{Q}}=\left({\mathit{\omega }}_{ib}^b+\mathit{\delta }{\mathit{\omega }}_{ib}^b\right)-{\widehat{\mathit{Q}}}^{*}\otimes \left({\mathit{\omega }}_{in}^n+\mathit{\delta }{\mathit{\omega }}_{in}^n\right)\otimes \widehat{\mathit{Q}}$$

(4)

where ${\mathit{Q}}^{*}$ and ${\widehat{\mathit{Q}}}^{*}$ represent the conjugates of $\mathit{Q}$ and $\widehat{\mathit{Q}}$, respectively. ${\mathit{\omega }}_{ib}^b$ is the angular velocity of the $b$-frame relative to the $i$-frame expressed in the $b$-frame, ${\mathit{\omega }}_{in}^n$ is the angular velocity of the $n$-frame relative to the $i$-frame expressed in the $n$-frame. ${\tilde{\mathit{\omega }}}_{ib}^b$ is the measured value of ${\mathit{\omega }}_{ib}^b$, which is obtained from the gyro outputs. ${\widehat{\mathit{\omega }}}_{in}^n$ is the calculated value of ${\mathit{\omega }}_{in}^n$. $\mathit{\delta }{\mathit{\omega }}_{ib}^b$ represents the measurement error of ${\mathit{\omega }}_{ib}^b$, which is mainly gyro drift. $\mathit{\delta }{\mathit{\omega }}_{in}^n$ represents the calculation error of ${\mathit{\omega }}_{in}^n$, which is mainly related to the velocity error and position error of the vehicle.

Define the error quaternion $\mathit{\delta }\mathit{Q}$ as

$$\mathit{\delta }\mathit{Q}=\mathit{Q}\otimes {\widehat{\mathit{Q}}}^{*}$$

(5)

where ${\widehat{\mathit{Q}}}^{*}$ is the conjugate of calculated quaternion $\widehat{\mathit{Q}}$.

According to Equation (5), the derivative of $\mathit{\delta }\mathit{Q}$ with respective to $t$ can be expressed as

$$\mathit{\delta }\dot{\mathit{Q}}=\dot{\mathit{Q}}\otimes {\widehat{\mathit{Q}}}^{\ast }+\mathit{Q}\otimes {\dot{\widehat{\mathit{Q}}}}^{\ast }$$

(6)

The differential equation of $\mathit{\delta }\mathit{Q}$ can be obtained by substituting Equations (1) and (2) into Equation (6), as shown in Equation (7):

$$\begin{array}{cc}\hfill \mathit{\delta }\dot{\mathit{Q}}& =-\frac{1}{2}\mathit{Q}\otimes \left({\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\omega }}_{ib}^b\right)\otimes {\widehat{\mathit{Q}}}^{\ast }-\frac{1}{2}\mathit{Q}\otimes {\mathit{\omega }}_{in}^b\otimes {\widehat{\mathit{Q}}}^{\ast }+\frac{1}{2}\mathit{Q}\otimes {\widehat{\mathit{\omega }}}_{in}^b\otimes {\widehat{\mathit{Q}}}^{\ast }\hfill \\ & =-\frac{1}{2}\mathit{Q}\otimes \mathit{\delta }{\tilde{\mathit{\omega }}}_{ib}^b\otimes {\widehat{\mathit{Q}}}^{\ast }-\frac{1}{2}\mathit{Q}\otimes {\mathit{\omega }}_{in}^b\otimes {\widehat{\mathit{Q}}}^{\ast }+\frac{1}{2}\mathit{Q}\otimes {\widehat{\mathit{\omega }}}_{in}^b\otimes {\widehat{\mathit{Q}}}^{\ast }\hfill \\ & =-\frac{1}{2}\mathit{Q}\otimes {\widehat{\mathit{Q}}}^{\ast }\otimes \widehat{\mathit{Q}}\otimes \mathit{\delta }{\tilde{\mathit{\omega }}}_{ib}^b\otimes {\widehat{\mathit{Q}}}^{\ast }-\frac{1}{2}\mathit{Q}\otimes {\mathit{Q}}^{\ast }\otimes ({\widehat{\mathit{\omega }}}_{in}^n-\mathit{\delta }{\mathit{\omega }}_{in}^n)\otimes \mathit{Q}\otimes {\widehat{\mathit{Q}}}^{\ast }+\frac{1}{2}\mathit{Q}\otimes {\widehat{\mathit{Q}}}^{\ast }\otimes {\widehat{\mathit{\omega }}}_{in}^n\otimes \widehat{\mathit{Q}}\otimes {\widehat{\mathit{Q}}}^{\ast }\hfill \\ & =-\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes \mathit{\delta }{\tilde{\mathit{\omega }}}_{ib}^n-\frac{1}{2}({\widehat{\mathit{\omega }}}_{in}^n-\mathit{\delta }{\mathit{\omega }}_{in}^n)\otimes \mathit{\delta }\mathit{Q}+\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes {\widehat{\mathit{\omega }}}_{in}^n\hfill \end{array}$$

(7)

where

$$\left\{\begin{array}{l}{\widehat{\mathit{\omega }}}_{in}^n={\mathit{\omega }}_{in}^n+\mathit{\delta }{\mathit{\omega }}_{in}^n\\ {\widehat{\mathit{\omega }}}_{in}^b={\widehat{\mathit{Q}}}^{\ast }\otimes {\widehat{\mathit{\omega }}}_{in}^n\otimes \widehat{\mathit{Q}}\\ {\mathit{\omega }}_{in}^b={\mathit{Q}}^{\ast }\otimes {\mathit{\omega }}_{in}^n\otimes \mathit{Q}\\ \mathit{\delta }{\tilde{\mathit{\omega }}}_{ib}^b={\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\omega }}_{ib}^b\\ \mathit{\delta }{\tilde{\mathit{\omega }}}_{ib}^n=\widehat{\mathit{Q}}\otimes \mathit{\delta }{\tilde{\mathit{\omega }}}_{ib}^b\otimes {\widehat{\mathit{Q}}}^{\ast }\end{array}\right.$$

Equation (7) is the attitude error equation of SINS under the condition of large misalignment angles.

During the fine alignment on quasi-stationary base, the position information of vehicle can be accurately obtained, and the velocity of vehicle can be approximated as 0 m/s. By utilizing the information of zero-velocity and only considering the correlative bias of inertial sensor error, the attitude error equation is simplified as follows:

$$\begin{array}{ll}\hfill \mathit{\delta }\dot{\mathit{Q}}& =-\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes \mathit{\delta }{\tilde{\mathit{\omega }}}_{ib}^n-\frac{1}{2}{\widehat{\mathit{\omega }}}_{ie}^n\otimes \mathit{\delta }\mathit{Q}+\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes {\widehat{\mathit{\omega }}}_{ie}^n\hfill \\ & =-\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes {\epsilon }_b^n-\frac{1}{2}{\widehat{\mathit{\omega }}}_{ie}^n\otimes \mathit{\delta }\mathit{Q}+\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes {\widehat{\mathit{\omega }}}_{ie}^n-\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes {\epsilon }_w^n\hfill \end{array}$$

(8)

where ${\epsilon }_b^n$ and ${\epsilon }_w^n$ are the random constant bias and white noise of the gyro, respectively; ${\widehat{\mathit{\omega }}}_{ie}^n$ is the calculated angular velocity of earth rotation in the $n$-frame.

2.2. Velocity Error Equation

Without considering any errors, the velocity equation of SINS is expressed as follows:

$${\dot{\mathit{V}}}^n={\mathit{C}}_b^n{\mathit{f}}_{sf}^b-\left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)\times {\mathit{V}}^n+{\mathit{g}}_{}^n$$

(9)

Considering various error resources, the velocity equation of SINS is:

$${\dot{\widehat{\mathit{V}}}}^n={\widehat{\mathit{C}}}_b^n{\tilde{\mathit{f}}}_{sf}^b-\left(2{\widehat{\mathit{\omega }}}_{ie}^n+{\widehat{\mathit{\omega }}}_{en}^n\right)\times {\widehat{\mathit{V}}}^n+{\widehat{\mathit{g}}}_{}^n$$

(10)

where ${\mathit{V}}^n$ represents the true vehicle velocity, ${\widehat{\mathit{V}}}^n$ represents the calculated vehicle velocity of SINS, $\mathit{\delta }{\mathit{V}}^n$ represents the velocity error of SINS, and ${\widehat{\mathit{V}}}^n={\mathit{V}}^n+\mathit{\delta }{\mathit{V}}^n$; ${\mathit{C}}_b^n$ represents the true attitude matrix, ${\widehat{\mathit{C}}}_b^n$ represents the calculated attitude matrix of SINS, $\mathit{\delta }{\mathit{C}}_n^{n^{\prime }}$ represents the attitude error matrix of SINS, and ${\widehat{\mathit{C}}}_b^n=\mathit{\delta }{\mathit{C}}_n^{n^{\prime }}{\mathit{C}}_b^n$; ${\mathit{f}}_{sf}^b$ is the true specific force on the vehicle, ${\tilde{\mathit{f}}}_{sf}^b$ represents the measured specific force, which is obtained by the output of accelerators. $\mathit{\delta }{\mathit{f}}_{sf}^b$ represents the measurement error of ${\mathit{f}}_{sf}^b$, which is mainly accelerator bias, and ${\tilde{\mathit{f}}}_{sf}^b={\mathit{f}}_{sf}^b+\mathit{\delta }{\mathit{f}}_{sf}^b$; ${\mathit{\omega }}_{ie}^n$ represents the angular velocity of earth rotation in the $n$-frame, ${\widehat{\mathit{\omega }}}_{ie}^n$ represents the calculated value of ${\mathit{\omega }}_{ie}^n$, which is related to the position of SINS, $\mathit{\delta }{\mathit{\omega }}_{ie}^n$ represents the calculation error of ${\widehat{\mathit{\omega }}}_{ie}^n$, which is mainly caused by the position error of SINS, and ${\widehat{\mathit{\omega }}}_{ie}^n={\mathit{\omega }}_{ie}^n+\mathit{\delta }{\mathit{\omega }}_{ie}^n$; ${\mathit{\omega }}_{en}^n$ represents the angular velocity of the $n$-frame relative to the $e$-frame coordinated in the $n$-frame, ${\widehat{\mathit{\omega }}}_{en}^n$ represents the calculated value of ${\mathit{\omega }}_{en}^n$; $\mathit{\delta }{\mathit{\omega }}_{en}^n$ represents the calculation error of ${\widehat{\mathit{\omega }}}_{en}^n$, and ${\widehat{\mathit{\omega }}}_{en}^n={\mathit{\omega }}_{en}^n+\mathit{\delta }{\mathit{\omega }}_{en}^n$; ${\mathit{g}}_{}^n$ represents the gravity acceleration expressed in the $n$-frame, ${\widehat{\mathit{g}}}_{}^n$ represents the calculated value of ${\mathit{g}}_{}^n$, $\mathit{\delta }{\mathit{g}}_{}^n$ represents the calculation error of ${\widehat{\mathit{g}}}_{}^n$, and ${\widehat{\mathit{g}}}_{}^n={\mathit{g}}_{}^n+\mathit{\delta }{\mathit{g}}_{}^n$.

By subtracting Equation (9) from Equation (10), and rearranging the error terms, the differential equation of SINS velocity error under large misalignment angles can be obtained, as shown in Equation (11).

$$\mathit{\delta }{\dot{\mathit{V}}}^n=\left({\mathit{I}}_{3\times 3}^{}-{\mathit{C}}_{n^{\prime }}^n\right){\widehat{\mathit{C}}}_b^n{\tilde{\mathit{f}}}_{sf}^b-\left(2{\widehat{\mathit{\omega }}}_{ie}^n+{\widehat{\mathit{\omega }}}_{en}^n\right)\times \mathit{\delta }{\mathit{V}}^n-\left(2\mathit{\delta }{\mathit{\omega }}_{ie}^n+\mathit{\delta }{\mathit{\omega }}_{en}^n\right)\times {\widehat{\mathit{V}}}^n+{\mathit{C}}_{n^{\prime }}^n{\widehat{\mathit{C}}}_b^n{\nabla }_b^b+{\mathit{C}}_{n^{\prime }}^n{\widehat{\mathit{C}}}_b^n{\nabla }_w^b$$

(11)

where ${\nabla }_b^b$ and ${\nabla }_w^b$ are the random constant bias and white noise of the accelerometer, respectively.

Similarly, under the condition of zero-velocity, the velocity error equation is simplified as:

$$\mathit{\delta }{\dot{\mathit{V}}}^n\approx \left({\mathit{I}}_{3\times 3}^{}-{\mathit{C}}_{n^{\prime }}^n\right){\widehat{\mathit{C}}}_b^n{\tilde{\mathit{f}}}_{sf}^b+{\mathit{C}}_{n^{\prime }}^n{\widehat{\mathit{C}}}_b^n{\nabla }_b^b+{\mathit{C}}_{n^{\prime }}^n{\widehat{\mathit{C}}}_b^n{\nabla }_w^b$$

(12)

3. Nonlinear Filtering Model and Second-Order EKF Algorithm

3.1. Filtering Model of SINS Nonlinear Alignment with Large Misalignment Angles

For the errors of inertial sensors, only the random constant bias and white noise are considered. The gyro constant drift and accelerometer constant bias are augmented into the system state vector, and the system state vector is defined as

$$X\left(t\right)={\left[\begin{array}{ccc}\mathit{\delta }{\mathit{Q}}^{\mathrm{T}}& \mathit{\delta }{\mathit{V}}^{n\mathrm{T}}& \begin{array}{cc}{\epsilon }_b^{b\mathrm{T}}& {\nabla }_b^{b\mathrm{T}}\end{array}\end{array}\right]}^{\mathrm{T}}$$

(13)

where $\mathit{\delta }\mathit{Q}={\left[\begin{array}{cccc}\mathit{\delta }{q}_0& \mathit{\delta }{q}_1& \mathit{\delta }{q}_2& \mathit{\delta }{q}_3\end{array}\right]}_{}^{\mathrm{T}}$ is the error quaternion, $\mathit{\delta }{\mathit{V}}^n={\left[\begin{array}{ccc}\mathit{\delta }{V}_E^n& \mathit{\delta }{V}_N^n& \mathit{\delta }{V}_U^n\end{array}\right]}^{\mathrm{T}}$ is the velocity error, ${\epsilon }_b^b={\left[\begin{array}{ccc}{\epsilon }_{b,x}^b& {\epsilon }_{b,y}^b& {\epsilon }_{b,z}^b\end{array}\right]}^{\mathrm{T}}$ is the gyro random constant drift, and ${\nabla }_b^b={\left[\begin{array}{ccc}{\nabla }_{b,x}^b& {\nabla }_{b,y}^b& {\nabla }_{b,z}^b\end{array}\right]}^{\mathrm{T}}$ is the accelerometer random constant bias.

According to the nonlinear error equations of SINS, the system state equation for nonlinear filter can be established as follows:

$$\dot{X}\left(t\right)=f[X\left(t\right),t]+G\left(t\right)w\left(t\right)$$

(14)

$$f[X\left(t\right),t]=\left[\begin{array}{c}-\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes \widehat{\mathit{Q}}\otimes {\epsilon }_b^b\otimes {\widehat{\mathit{Q}}}^{\ast }-\frac{1}{2}{\widehat{\mathit{\omega }}}_{ie}^n\otimes \mathit{\delta }\mathit{Q}+\frac{1}{2}\mathit{\delta }\mathit{Q}\otimes {\widehat{\mathit{\omega }}}_{ie}^n\\ \left({\mathit{I}}_{3\times 3}^{}-{\mathit{C}}_{n^{\prime }}^n\right){\widehat{\mathit{C}}}_b^n{\tilde{f}}_{sf}^b+C_{n^{\prime }}^n{\widehat{\mathit{C}}}_b^n{\nabla }_b^b\\ 0_{3\times 1}\\ 0_{3\times 1}\end{array}\right]$$

(15)

where $G\left(t\right)$ represents the system noise driving matrix, $w\left(t\right)={\left[\begin{array}{ccc}0& {\epsilon }_w^{b\mathrm{T}}& {\nabla }_w^{b\mathrm{T}}\end{array}\right]}_{}^{\mathrm{T}}$ represents the system driving noise, ${\epsilon }_w^{b\mathrm{T}}$ and ${\nabla }_w^{b\mathrm{T}}$ represent the white noises of gyroscopes and accelerometers, respectively. The expression of system noise driving matrix $G\left(t\right)$ is shown in Equation (16):

$$G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{\left[\widehat{\mathit{Q}}\right]}_L{\left[{\widehat{\mathit{Q}}}^{*}\right]}_R& 0_{4\times 3}\\ 0_{3\times 4}^{}& {\widehat{\mathit{C}}}_b^n\\ 0_{6\times 4}^{}& 0_{6\times 3}^{}\end{array}\right]$$

(16)

where ${\left[\times \right]}_L$ and ${\left[\times \right]}_R$ are two different $4\times 4$ matrices, which are both composed of the elements of corresponding quaternion $\times$. Moreover, they are convenient to realize the quaternion multiplication by using matrix multiplication. The matrices ${\left[\times \right]}_L$ and ${\left[\times \right]}_R$ are defined as follows:

$$P\otimes \mathit{Q}\triangleq {\left[P\right]}_L\mathit{Q}\triangleq {\left[\mathit{Q}\right]}_{R}P$$

(17)

The velocity of SINS is taken as the measurement of nonlinear filtering alignment, the measurement equation of SINS nonlinear alignment can be constructed as

$$Z_k=\mathit{\delta }{\mathit{V}}_k^n=H_k^{}{X}_k^{}+{\mathit{V}}_k$$

(18)

where $H_k^{}=\left[\begin{array}{ccc}{0}_{3\times 4}& {\mathit{I}}_{3\times 3}& \begin{array}{cc}{0}_{3\times 3}& 0_{3\times 3}\end{array}\end{array}\right]$ and ${\mathit{V}}_k$ is the measurement noise which corresponds to the measurement error resulting from the environmental disturbance.

According to Equations (14) and (18), the nonlinear filtering model in state space is obtained, and can be presented as

$$\left\{\begin{array}{l}\dot{X}\left(t\right)=\mathit{f}[X\left(t\right),t]+G\left(t\right)w\left(t\right)\\ Z_k=H_k^{}{X}_k^{}+{\mathit{V}}_k\end{array}\right.$$

(19)

3.2. Simplified Second-Order EKF Algorithm

As shown in Equation (20), the nonlinear filtering model obtained in the previous subsection is copied into this subsection, which includes the system state equation and measurement equation.

$$\left\{\begin{array}{l}\dot{X}\left(t\right)=\mathit{f}[X\left(t\right),t]+G\left(t\right)w\left(t\right)\\ Z_k=H_k^{}{X}_k^{}+{\mathit{V}}_k\end{array}\right.$$

(20)

where $X\left(t\right)$ represents the system state vector at time $t$; $f[X\left(t\right),t]$ represents the nonlinear function of $X\left(t\right)$ which describes the system state equation with continuous form; the subscript $k$ represents the time $t_k^{}$; $Z_k$ represents the measurement vector; $H_k^{}$ represents the measurement matrix; $G\left(t\right)$ represents the system noise driving matrix; $w\left(t\right)$ represents the continuous system noise, which is assumed as a zero-mean Gaussian white noise with covariance $q\left(t\right)$; ${\mathit{V}}_k$ represents the discrete measurement noise sequence, which is assumed as a zero-mean Gaussian white noise sequence with covariance $R_k$; and assuming that $w\left(t\right)$ is uncorrelated with ${\mathit{V}}_k$; the dimensions of $X\left(t\right)$ and $Z_k$ are denoted as $n$ and $m$; in this paper, $n=13$ and $m=3$.

It can be seen from Equation (20) that the system state equation is nonlinear, and the measurement equation is linear. The second-order EKF algorithm can be simplified, and this simplified algorithm is abbreviated as SEKF2. By using the fourth-order Runge–Kutta method, the time update process of SEKF2 is implemented based on the second-order Taylor expansion of nonlinear vector function in state equation. The measurement update process of SEKF2 is the same as that of classical linear Kalman filter. The steps of SEKF2 are as follows:

(1) Selection of Initial Filtering Parameters

$${\widehat{X}}_0^{}=\mathrm{E}\left[X_0^{}\right],P_0^{}=\mathrm{E}\left[\left({\widehat{X}}_0^{}-X_0^{}\right){\left({\widehat{X}}_0^{}-X_0^{}\right)}_{}^{\mathrm{T}}\right]$$

(21)

(2) Time Update

$${\dot{\widehat{X}}}^{-}\left(t\right)=f[{\widehat{X}}^{-}\left(t\right),t]+\frac{1}{2}{\displaystyle \sum _{i=1}^n{e}_i}Tr\left[{\left.\left(\nabla {\nabla }^{\mathrm{T}}{f}_i\right)\right|}_{X\left(t\right)={\widehat{X}}^{-}\left(t\right)}{P}^{-}\left(t\right)\right]$$

(22)

$${\dot{P}}^{-}\left(t\right)=F\left(t\right)P^{-}\left(t\right)+P^{-}\left(t\right)F^{\mathrm{T}}\left(t\right)+G\left(t\right)q{G}^{\mathrm{T}}\left(t\right)$$

(23)

where $f_i$ is the $i$-th component of the nonlinear vector function $f[X\left(t\right),t]$; $e_i$ is the unit vector with the $i$-th component being 1 and the remaining components being 0; $F\left(t\right)$ is the Jacobian matrix of $f[X\left(t\right),t]$ with respect to $X\left(t\right)$; $\nabla {\nabla }^{\mathrm{T}}{f}_i$ is the Hessian matrix of $f_i$ with respect to $X\left(t\right)$; The expressions of $F\left(t\right)$ and $\nabla {\nabla }^{\mathrm{T}}{f}_i$ are shown in Equation (24):

$$\left\{\begin{array}{l}F\left(t\right)={\left.\left(\partial \mathit{f}/\partial X^{\mathrm{T}}\left(t\right)\right)\right|}_{X\left(t\right)={\widehat{X}}^{-}\left(t\right)}\\ \nabla {\nabla }^{\mathrm{T}}{f}_i={\left.\left(\partial \left({\left(\partial f_i/\partial X^{\mathrm{T}}\left(t\right)\right)}^{\mathrm{T}}\right)/\partial X^{\mathrm{T}}\left(t\right)\right)\right|}_{X\left(t\right)={\widehat{X}}^{-}\left(t\right)}\end{array}\right.$$

(24)

By using the fourth-order Runge-Kutta method, the differential Equations (22) and (23) are integrated on the filtering period $[t_{k-1},t_k]$. Thus the one-step predictive estimations of state $X$ and covariance $P$ can be obtained, which are denoted as ${\widehat{X}}_{k/k-1}, P_{k/k-1}$, respectively. In the integral process, the initial values are ${\widehat{X}}^{-}\left(t_{k-1}\right)={\widehat{X}}_{k-1}$ and $P^{-}\left(t_{k-1}\right)=P_{k-1}$, and the final values ${\widehat{X}}_{k/k-1}={\widehat{X}}^{-}\left(t_k\right)$ and $P_{k/k-1}=P^{-}\left(t_k\right)$ will be used in the measurement update process.

(3) Measurement Update

$${\widehat{Z}}_{k/k-1}^{}=H_k^{}{\widehat{X}}_{k/k-1}^{}$$

(25)

$$P_{XZ,k/k-1}^{}=P_{k/k-1}^{}{H}_k^{\mathrm{T}}$$

(26)

$$P_{ZZ,k/k-1}^{}=H_k^{}{P}_{k/k-1}^{}{H}_k^{\mathrm{T}}+R_k^{}$$

(27)

$$K_k^{}=P_{XZ,k/k-1}^{}{P}_{ZZ,k/k-1}^{-1}$$

(28)

$${\widehat{X}}_k^{}={\widehat{X}}_{k/k-1}^{}+K_k^{}\left(Z_k^{}-{\widehat{Z}}_{k/k-1}^{}\right)$$

(29)

$$P_k^{}=P_{k/k-1}^{}-K_k^{}{P}_{ZZ,k/k-1}^{}{K}_k^{\mathrm{T}}$$

(30)

where ${\widehat{Z}}_{k/k-1}^{}$ is the one-step prediction of measurement $Z_k$, $P_{XZ,k/k-1}^{}$ is the cross-covariance matrix of ${\widehat{Z}}_{k/k-1}^{}$ and ${\widehat{X}}_{k/k-1}^{}$, $P_{ZZ,k/k-1}^{}$ is the auto-covariance matrix of ${\widehat{Z}}_{k/k-1}^{}$, $R_k^{}$ is the covariance matrix of measurement noise sequence ${\mathit{V}}_k$, $K_k^{}$ is the gain matrix, ${\widehat{X}}_k^{}$ and $P_k^{}$ are the optimal estimation of system state and the covariance matrix of state estimation error at time $t_k^{}$, respectively.

4. The Algorithms of Strong Tracking Strategy and Fuzzy Adaptive Parameter Adjustment

4.1. Strong Tracking Strategy

Under the condition that the filtering model and noise statistics are accurate, the optimal estimation of state can be obtained by using KF. However, under the conditions of inaccurate filtering model and noise statistics, the estimation error of KF will be increased or even divergent. To solve the problem of filtering divergence caused by inaccurate state equation or non-Gaussian system noise, strong tracking Kalman filter (STKF) was proposed, which is based on the orthogonality of innovation sequence [52,53,54].

In KF filtering, the innovation $e_k$ is defined as

$$e_k=Z_k-H_k{\widehat{X}}_{k/k-1}$$

(31)

Under the assumption of white Gaussian noise, the expectation of innovation covariance is expressed by Equation (32):

$$\mathrm{E}\left(e_k{e}_k^{\mathrm{T}}\right)=E_k^{}\triangleq H_k^{}{P}_{k/k-1}^{}{H}_k^{\mathrm{T}}+R_k^{}$$

(32)

When the process noise is non-Gaussian or the system model is inaccurate, the differences between the calculated value ${\widehat{E}}_k^{}$ and theoretical value $E_k^{}$ of innovation covariance would be significantly large. In this case, the confidence degree of state prediction should be decreased, and the confidence degree of current measurement value should be increased. For the purpose of adjusting the confidence degrees, the value of $P_{k/k-1}$ is increased by introducing attenuation factor ${\mathit{\gamma }}_k^{}$, and the attenuation factor ${\mathit{\gamma }}_k^{}$ is calculated according to the orthogonality of innovation sequence.

According to the filtering model, the covariance matrix of innovations at two different time instants is expressed as

$$\mathrm{E}\left(e_{k+j}{e}_k^{\mathrm{T}}\right)=H_{k+j}^{}\cdot \prod _{l=k+1}^{k+j-1}{\mathit{\Phi }}_{l/l-1}^{}\left(\mathit{I}-K_l^{}{H}_l^{}\right)\cdot {\mathit{\Phi }}_{k/k-1}^{}\left(P_{k/k-1}^{}{H}_k^{\mathrm{T}}-K_k^{}{E}_k^{}\right)j=1,2,\dots$$

(33)

It can be seen that a sufficient condition for the orthogonality of innovation sequence is

$$S_k^{}=P_{k/k-1}^{}{H}_k^{\mathrm{T}}-K_k^{}{E}_k^{}=0$$

(34)

By substituting the filter gain matrix Equation (28) into (34), the sufficient condition could be rewritten as

$$P_{k/k-1}^{}{H}_k^{\mathrm{T}}-P_{k/k-1}^{}{H}_k^{\mathrm{T}}{\left(H_k^{}{P}_{k/k-1}^{}{H}_k^{\mathrm{T}}+R_k^{}\right)}_{}^{-1}{E}_k^{}=0$$

(35)

Under the condition of measurement matrix $H_k^{}$ with full column rank, the equivalent form is obtained, as shown in Equation (36):

$$E_k^{}=H_k^{}{P}_{k/k-1}^{}{H}_k^{\mathrm{T}}+R_k^{}$$

(36)

By comparing Equations (32) and (36), it can be readily known that, under the assumptions of the accurate filtering model and Gaussian process noise, the innovation of Kalman filter meets the orthogonality. However, if the above assumptions are not satisfied, the difference between ${\widehat{E}}_k^{}$ and $E_k^{}$ will be relatively large, and the innovation will not meet the orthogonality. In order to keep orthogonality of innovation sequence, $P_{k/k-1}$ should be modified by utilizing ${\mathit{\gamma }}_k^{}$, as shown in Equation (37):

$$P_{k/k-1}={\mathit{\gamma }}_k^{}{P}_{k/k-1}$$

(37)

Substituting Equation (37) into (36), $E_k^{}$ is modified as

$${\widehat{E}}_k^{}={\mathit{\gamma }}_k^{}{H}_k^{}{P}_{k/k-1}^{}{H}_k^{\mathrm{T}}+\mathit{\eta }{R}_k^{}$$

(38)

where $\mathit{\eta }$ represents the softening factor, the value of $R_k^{}$ can be adjusted by presetting $\mathit{\eta }$, thus the confidence degree of measurement $Z_k^{}$ can be directly adjusted.

Based on Equation (38), the attenuation factor ${\mathit{\gamma }}_k^{}$ can be obtained by using the operations of calculating the matrix trace and absolute value, as shown in Equation (39).

$${\mathit{\gamma }}_k^{}=0.99\cdot \left|\mathrm{tr}\left({\widehat{E}}_k^{}-\mathit{\eta }{R}_k^{}\right)/\mathrm{tr}\left(H_k^{}{P}_{k/k-1}^{}{H}_k^{\mathrm{T}}\right)\right|$$

(39)

where the coefficient 0.99 is a preset constant, which is adjusted according to the actual conditions.

In order to better modify $P_{k/k-1}^{}$, the attenuation factor ${\mathit{\gamma }}_k^{}$ is selected as

$${\mathit{\gamma }}_k^{}\in \left\{1,0.99\cdot \left|\mathrm{tr}\left({\widehat{E}}_k^{}-\mathit{\eta }{R}_k^{}\right)/\mathrm{tr}\left(H_k^{}{P}_{k/k-1}^{}{H}_k^{\mathrm{T}}\right)\right|\right\}$$

(40)

where the calculated value of innovation covariance matrix ${\widehat{E}}_k^{}$ is selected as follows:

$${\widehat{E}}_k=\left\{\begin{array}{l}{e}_k{e}_k^{\mathrm{T}},k=1\hfill \\ \left(\mathit{\rho }{\widehat{E}}_{k-1}+e_k{e}_k^{\mathrm{T}}\right)/\left(1+\mathit{\rho }\right),k>1\hfill \end{array}\right.$$

(41)

where $\mathit{\rho }\in \left(0,1\right)$ represents the forgetting factor, of which the value is usually 0.95.

From the above analysis, it can be concluded that the non-orthogonality of innovation sequence is caused by the non-Gaussian process noise or inaccurate system model. When the process noise or state model is abnormal, the attenuation factor ${\mathit{\gamma }}_k^{}$ of STKF is adaptively adjusted. Moreover, the prediction estimation variance matrix (PECM) $P_{k/k-1}^{}$ of STKF is enlarged to increase the weight of current innovation (or measurement) in filtering estimation. By decreasing the confidence of one-step prediction, the filter gain matrix $K_k$ is adjusted in real-time, thus the state estimation can be satisfied for the requirements of minimum variance and the innovation sequence is forced to keep orthogonal to each other. As a result, the effective information in the innovation sequence is extracted to the maximum extent. Under the conditions of inaccurate system model and non-Gaussian process noise, STKF has the ability to track the system state, robustness on model mismatch and system disturbance, and high accuracy and stability. Combined with strong tracking strategy, EKF2 can be modified to deal with the nonlinear filtering alignment with model uncertainties.

4.2. Fuzzy Adaptive Parameter Adjustment

In 1965, Professor L.A. Zadeh proposed the theory of fuzzy sets and fuzzy logic at the University of California, which contributed to the progress of reasoning with imprecise concepts and provided a new approach to qualitatively represent human knowledge. The principle of fuzzy inference system is shown in Figure 1. By utilizing the fuzzy inference, the mapping from input space to output space is realized in the fuzzy logic system (FLS). Fuzzy rules are the core of fuzzy inference system, and they are generally expressed by the “if-then” rule statements.

For the convenience in designing a fuzzy inference system(FIS) to realize the strong tracking strategy better, the part of $0.99\left|tr\left({\widehat{E}}_k-\mathit{\eta }{R}_k\right)/tr\left(H_k^{}{P}_{k/k-1}{H}_k^T\right)\right|$ in Equation (40) is denoted as $c_k^{}$, i.e., $c_k^{}\triangleq 0.99\left|tr\left({\widehat{E}}_k-\mathit{\eta }{R}_k\right)/tr\left(H_k^{}{P}_{k/k-1}{H}_k^T\right)\right|$. Then the fuzzy inference system with the form of single- input single-output(SISO) is designed, in which the parameters $c_k^{}$ and ${\mathit{\gamma }}_k^{}$ are selected as the input and output, respectively. From the strong tracking theory and engineering experience, it can be obtained that: if the value of $c_k^{}$ is relatively large, it means that the difference between ${\widehat{E}}_k$ and $E_k$ is relatively large, that is to say the errors of system model or prior information are relatively large, then the strong tracking function is enabled, i.e., letting ${\mathit{\gamma }}_k^{}=c_k^{}$; Conversely, if the value of $c_k^{}$ is relatively small, it means that the difference between ${\widehat{E}}_k$ and $E_k$ is relatively small, that is to say the system model and prior information are accurate, then the strong tracking function is disabled and only the EKF2 filter is carried out, i.e., letting ${\mathit{\gamma }}_k^{}=1$. Two fuzzy subsets are defined to cover the input variable domain, and they are referred to as Small (S) and Large (L). The Z-shaped curve membership function (zmf) is selected for the fuzzy subset S and S-shaped curve membership function (smf) is selected for the fuzzy subset L. The fuzzy subsets and membership functions of the input variable $c_k^{}$ are shown in

Table 1. Fuzzy subsets of input variable $c_k^{}$.

Fuzzy Subsets	Domain	Membership Functions	Break Points
Small (S)	[0, 3]	zmf	[0.8, 1.2]
Large (L)	[0, 3]	smf	[0.8, 1.2]

. The fuzzy inference system is designed, and its membership functions, fuzzy rules and input-output surface are shown in Figure 2a–c. In this paper, Sugeno-type fuzzy inference system is adopted, and two fuzzy rules are established as follows:

$$\begin{array}{l}{R}_{}^1:if\left(c_k^{}\mathrm{is}\mathrm{S}\right)\mathrm{then}\left({\mathit{\gamma }}_k^{}=1\right);\\ R_{}^2:if\left(c_k^{}\mathrm{is}\mathrm{L}\right)\mathrm{then}\left({\mathit{\gamma }}_k^{}=c_k^{}\right);\end{array}$$

By combining the second-order EKF filter and fuzzy adaptive parameter adjustment, the algorithm of the second-order extended Kalman filter (EKF2) filter assisted by the adaptive fuzzy inference system (AFIS) can be obtained, of which the flowchart is shown in Figure 3.

5. Experiment Setup and Result Analysis

5.1. The Simulation of Fine Alignment on Stationary Base

In order to analyze the precision of the algorithm, several simulative analyses of AFIS-EKF2 filtering alignment were carried out, and the simulation results were analyzed statistically. In the simulations, the initial attitude errors and inertial measurement unit (IMU) errors were preset, and the IMU data on static base were generated by trajectory simulation.

5.1.1. Simulation Conditions

By using MATLAB software (MATLAB R2009a, The MathWorks. Inc.), we performed simulation with the conditions as follows:

(1) The State of Vehicle.

The vehicle is stationary, and the position is: longitude $108.9°\mathrm{E}$, latitude $34.2°\mathrm{N}$, altitude $400\mathrm{m}$; the velocity of vehicle is $0\mathrm{m}/\mathrm{s}$; and the attitude of vehicle is $5°$, $10°$, $45°$.

(2) IMU Errors.

Gyro errors: the random constant drift is $0.02°/\mathrm{h}$, the angular random walk coefficient is $0.003°/\sqrt{\mathrm{h}}$.

Accelerometer errors: the random constant bias is $0.05\mathrm{mg}$, the velocity random walk coefficient is $0.01\mathrm{mg}/\sqrt{\mathrm{Hz}}$.

(3) Parameter Setting of Filter.

The velocity measurement noise of SINS on stationary base is assumed to be Gaussian white noise with amplitude of $0.05\mathrm{m}/\mathrm{s}\left(1\mathit{\sigma }\right)$; the filtering period is $1\mathrm{s}$; the correction mode is selected as feedback correction. The alignment accuracy is evaluated by using the difference between the fine alignment results and the true attitudes.

5.1.2. Simulation Results

Simulation 1: in order to evaluate the alignment accuracy of the designed nonlinear alignment algorithm with various initial misalignment angles, Monte Carlo simulation was required. In the simulation, the initial values of misalignment angles are set as follows: both the pitch and roll misalignment angles follow the uniform distribution on the interval from $-40°$ to $40°$, i.e., ${\varphi }_E\sim U\left(-40°,40°\right)$ and ${\varphi }_N\sim U\left(-40°,40°\right)$. The yaw misalignment angle follows the uniform distribution on the interval from $-50°$ to $50°$, that is, ${\varphi }_U\sim U\left(-50°,50°\right)$. The simulation number of times is 20, and the length of single run time is $350\mathrm{s}$. The Monte Carlo simulation results of AFIS-EKF2 filtering alignment are shown in Figure 4a–c. It can be seen from Figure 4a,b that the pitch misalignment angle and roll misalignment angle can converge quickly after the start of the alignment process, and the Root Mean Square Errors (RMSE) of both pitch and roll errors have converged to $0.01°$ within $50\mathrm{s}$. As shown in Figure 4c, the yaw misalignment angle converges slowly, and it can converge to the steady state in about $300\mathrm{s}$, the RMSE of steady-state errors is $0.127°$. The statistics of estimation errors of misalignment angles at 350 s are shown in

Table 2. Statistical properties of alignment errors.

Errors (°)	$\mathbf{Pitch}\mathbf{Error}\mathit{\delta }\mathit{\theta }$	$\mathbf{Roll}\mathbf{Error}\mathit{\delta }\mathit{\gamma }$	$\mathbf{Yaw}\mathbf{Error}\mathit{\delta }\mathit{\psi }$
RMSE	0.0032	0.0042	0.1271
Limits	0.0029	−0.0029	0.0924

. The simulation results show that the precision of the designed AFIS-EKF2 alignment algorithm is close to the ultimate precision of the traditional static-base analytical alignment, and can meet the requirements of nonlinear alignment. The expressions of ultimate precision of static-base analytical alignment are shown in Equations (36)–(38).

The residual errors of conventional static-base analytical alignment (i.e., the ultimate precision) are expressed as:

$$\mathit{\delta }{\mathit{\varphi }}_{E0}^{}={\nabla }_N^{}/g$$

(42)

$$\mathit{\delta }{\mathit{\varphi }}_{N0}^{}=-{\nabla }_E^{}/g$$

(43)

$$\mathit{\delta }{\mathit{\varphi }}_{U0}^{}={\mathsf{\epsilon }}_E^{}/{\mathsf{\omega }}_{ie}^{}/\cos \left(L\right)$$

(44)

Simulation 2: In order to verify the accuracy of proposed alignment algorithm under the condition that the initial values of misalignment angles are all very large, the initial values of misalignment angles are set to $80°$, $120°$, $-170°$; and the time of fine alignment is $600\mathrm{s}$. The simulation results are shown in Figure 5a–c.

In Figure 5a,b, the pitch, roll misalignment angles converge quickly during the process of fine alignment with very large initial values of misalignment angles. Within $50\mathrm{s}$, the pitch misalignment angle has converged to $-0.1225°$ and the roll misalignment angle has converges to $0.21°$; when the alignment time is $600\mathrm{s}$, the estimated residual errors of misalignment angles in pitch and roll are $0.0121°$ and $-0.004°$, respectively. It can be seen from Figure 5c that, under the condition that the initial values of three misalignment angles are all very large, the estimate of yaw misalignment angle jumps to the wrong value at the beginning of filtering alignment, and does not converge during the time period from $0\mathrm{s}$ to $200\mathrm{s}$. This is because: In this case, it is difficult to separate and identify the yaw misalignment angle from the error quaternion, the corresponding component of $P_k^{}$ converges too early and will be stabilized at the error value. By enabling FIS at $200\mathrm{s}$, $P_k^{}$ is adaptively adjusted, so that the yaw misalignment angle converges quickly. Finally, the alignment accuracy in yaw reaches $-0.3794°$.

5.2. The Simulation of Fine Alignment on Swaying Base

In order to verify the effectiveness of the proposed nonlinear alignment algorithm under the conditions of large misalignment angles and external angular disturbance, the simulation of filtering alignment on swaying base was performed in this subsection.

5.2.1. Simulation Conditions

The setting of IMU errors is the same as that of previous subsection. The amplitudes of angular disturbances in pitch, roll and yaw are $4°$, $6°$ and $4°$, respectively. Moreover, the swaying frequencies in the three directions are all $0.1\mathrm{Hz}$. The initial values of the pitch, roll and yaw misalignment angles are set to $40°$, $-40°$ and $50°$, respectively. The velocity measurement noise of nonlinear filtering is assumed to be Gaussian white noise with amplitude of $0.1\mathrm{m}/\mathrm{s}$. The filtering period is $1\mathrm{s}$.

5.2.2. Simulation Results

The simulation results of the proposed nonlinear alignment on swaying base are shown in Figure 6. The error curves of the pitch and roll converge quickly, and the yaw misalignment angle converges in around $300\mathrm{s}$. According to the steady-state data of attitude errors from $300\mathrm{s}$ to $600\mathrm{s}$, the RMSE errors of pitch, roll and yaw are $0.0023°$, $-0.0032°$ and $-0.2654°$, respectively. From the simulation results of fine alignment on swaying base, it can be seen that the heading angle error is −0.26° in 600 s, so the difference between the accuracy of heading alignment and corresponding limit accuracy is a little large. That is because the calculation formulas of limit accuracy used in this paper are derived under ideal conditions of stationary base and only considering the gyro drifts. However, under the conditions of swaying base and large misalignment angles, the observable degree of heading misalignment angle is further decreased due to the influence of model error, disturbance and so on, and the accuracy of heading alignment cannot converge to corresponding limit accuracy. According to the engineering experience and existing research literature, the following conclusion is supported: in nonlinear filtering alignment, the larger the initial misalignment angle or disturbance is, the slower the convergence rate is, and the larger the steady-state error is. The simulation results show that the proposed algorithm is effective under the conditions of large misalignment angles and swaying base.

5.3. Experiment on Three-Axis Turntable

In order to further verify the effectiveness of the proposed AFIS-EKF2 algorithm in this paper, the SINS nonlinear filtering fine alignment experiments under different experimental conditions of large misalignment angles were conducted by using FSINS4X fiber-optic-gyroscope (FOG) strapdown inertial navigation system (FSINS4X FOG SINS) on SGT-3T high-precision three-axis turntable.

5.3.1. Experiment Conditions

FSINS4X FOG SINS is composed of three FOG gyroscopes, three quartz flexible accelerometers, navigation computer and power module, etc. FSINS4X FOG SINS is shown in Figure 7, and the main performance parameters of FOG gyroscope and quartz accelerometer are shown in

Table 3. Main parameters of gyro and accelerometer.

Parameters	Value
Gyro measurement range	±600°/s
Gyro bias stability	<0.01°/h
Gyro angle random walk	<0.001°/h1/2
Accelerometer measurement range	±20 g
Accelerometer bias stability	<50 µg
Accelerometer velocity random walk	<5 µg/h1/2

. The main performance indexes of SGT-3T three-axis turntable are shown in

Table 4. Main indexes of SGT-3T turntable.

Parameters	Value
Gyration accuracy	±2″
Rotation range	0~360°
Angle accuracy	±2″
Angular repeatability	±1″
Angular rate range	0.001~200°/s
Angular rate accuracy	5 × 10−4°/s

. FOG SINS is installed on the three-axis turntable through the transition plate, as shown in Figure 8.

In this paper, two groups of experiments were designed to verify the effectiveness of the algorithm, of which Experiment 1 is the alignment experiment under general large misalignment angles, and Experiment 2 is the alignment experiment under extremely large misalignment angles. The ranges of three initial misalignment angles in Experiment 1 are ${\varphi }_E\in \left[-40°,40°\right]$, ${\varphi }_N\in \left[-40°,40°\right]$ ${\varphi }_U\in \left[-50°,50°\right]$, respectively. In Experiment 2, the initial values of three extremely large misalignment angles are set as $80°$, $120°$, $-170°$, respectively. The experimental conditions are shown in

Table 5. Settings of experiment condition.

Experiments	Initial Misalignment Angles
	East	North	Azimuth
Experiment 1	${\varphi }_E\in \left[-40°,40°\right]$	${\varphi }_N\in \left[-40°,40°\right]$	${\varphi }_U\in \left[-50°,50°\right]$
Experiment 2	$80°$	$120°$	$-170°$

.

In the turntable experiment, the position of three-axis turntable in laboratory is ($34.2°\mathrm{N}$, $108.9°\mathrm{E}$), and various initial large misalignment angles can be set by three-axis turntable. Combined with the preset initial attitude value of SINS, the initial misalignment angle can be set as any large value by adjusting the real attitude of SINS on three-axis turntable at the beginning of alignment. For example, supposing that the initial attitude of SINS is roughly set to be [0, 0, 95]°. In order to set the initial misalignment angle of fine alignment as [35.6, 37.2, 44]°, the real attitude of SINS should be adjusted to (35.6, 37.2, 139.0)° by controlling the rotation of turntable. Before the experiment, the calibration and compensation of SINS were carried out, and the sampling rate of IMU was set as 200 $\mathrm{Hz}$. The three-axis turntable installed with FOG SINS was controlled to return to zero position, and then control the turntable to rotate to the angular position corresponding to the preset large misalignment angles. After the FOG-SINS is powered on and preheated, the fine alignment experiments under large misalignment angles is carried out, and the proposed AFIS-EKF2 algorithm in this paper is verified. In order to analyze the advantages and superiority of the proposed algorithms, several nonlinear alignment methods based on EKF, UKF, PF, FOS, and artificial neural networks (ANN) proposed by other literatures are used for the fine alignment with large misalignment angles under the same experimental conditions. In Experiment 1 and Experiment 2, 20 groups of turntable experiments in different attitudes are carried out for each algorithm of AFIS-EKF2, EKF, UKF, PF, FOS and ANN, respectively. Moreover, the fine alignment time of each experiment is 600 s.

5.3.2. Experiment Results

In Experiment 1 and Experiment 2, the 20 groups of misalignment angle errors of nonlinear alignment methods based on AFIS-EKF2, EKF, UKF, PF, FOS, and ANN at alignment completion time are counted, and the statistical curves of misalignment angle errors of each filtering algorithm in Experiment 1 are drawn, as shown in Figure 9. In Experiment 2, only the AFIS–EKF2 algorithm proposed in this paper can achieve alignment successfully, and the other five methods are all failed to align, so the statistical data graphs of Experiment 2 are no longer drawn. The Root Mean Square Error (RMSE) of misalignment angle errors of each algorithm in Experiment 1 and Experiment 2 are calculated, as shown in

Table 6. Comparison of alignment experiment results of AFIS–EKF2, EKF, unscented Kalman filter (UKF), particle filter (PF), and fast orthogonal search (FOS).

Experiments	Method	Misalignment Angle Error RMSE (′)
		East	North	Azimuth
Experiment 1	AFIS-EKF2	0.2280	0.2520	4.5720
	EKF	0.7079	0.9360	8.1605
	UKF	0.8164	0.9245	6.5188
	PF	0.8980	0.8782	5.8669
	EKF-based FOS	0.7787	1.0828	9.4473
	EKF-based ANN	0.9730	1.1793	8.5884
Experiment 2	AFIS-EKF2	0.7302	−0.2396	−22.7640
	EKF	Failure
	UKF	Failure
	PF	Failure
	EKF-based FOS	Failure
	EKF-based ANN	Failure

.

According to the statistical results in Figure 9 and Table 6, the proposed AFIS-EKF2 has higher alignment accuracy, smaller fluctuation and better stability than the other five methods in alignment experiments with large misalignment angles. Moreover, in Experiment 2, only the proposed AFIS-EKF2 method in this paper can successfully complete the fine alignment under extremely large misalignment angles, while the other reference methods have failed in misalignment.

5.4. Navigation Experimental Test of SINS on Vehicle

In order to sufficiently check the quality of proposed INS initial alignment algorithms, the navigation test experiment of SINS on a land vehicle (manufactured by Hunan Leopaard Motors Co. Ltd., Changsha, China) was carried out. The experimental platform is shown in Figure 10, which is composed of the Inertial Measurement Unit (FOG-IMU, manufactured by Harbin Engineering University, Harbin, China), global positioning system (GPS) (manufactured by Beijing BDStar Navigation Co., Ltd., Beijing, China), navigation computer (manufactured by EVAK Technology Co., Ltd., Shenzhen, China) and power supply system. Here, a SINS/GPS integrated navigation system is used to provide the reference position and velocity with high accuracy for analyzing the position and velocity errors of pure SINS navigation. The specifications of IMU are: gyro bias stability 0.01°/h, gyro angle random walk $0.001°/\sqrt{h}$; accelerometer bias stability 50 µg, accelerometer velocity random walk $0.5 \mathsf{\mu }g/\sqrt{Hz}$ Moreover, the accuracy specifications of GPS are: longitude and latitude errors 1.5 m, altitude error 2.5 m; velocity error 0.03 m/s.

The experimental steps are as follows: Firstly, the proposed AFIS-EKF2 alignment method is performed for SINS initial alignment with large misalignment angles under the condition of vehicle idling; after initial alignment, INS switches to the inertial mode, the SINS navigation is carried out for a one hour duration; finally, the levels of SINS errors are analyzed by compared with reference velocity and position from SINS/GPS integrated navigation system. The velocity errors of SINS are shown in Figure 11 and position errors of SINS are shown in Figure 12.

It can be seen from Figure 11 and Figure 12 that the velocity errors and position errors of SINS have the trend of slow oscillation and divergence in the navigation time of 1 h, which is consistent with the error propagation characteristics of SINS in theory. The velocity errors and position errors at some time points (500 s, 1000 s, 1500 s, 2000 s, 2500 s, 3000 s, 3500 s, 3600 s) are listed in

Table 7. Errors of SINS on a vehicle.

t (s)	$\mathit{\delta }$VE (m/s)	$\mathit{\delta }$VN (m/s)	$\mathit{\delta }$L (m)	$\mathit{\delta }$λ (m)
500 s	−0.018818	0.27268	101.89	−1.3953
1000 s	0.045686	−0.044207	183.29	0.72195
1500 s	0.14902	−0.80396	−16.265	59.255
2000 s	0.17986	−1.6915	−644.07	167.41
2500 s	−0.065639	−2.3809	−1680.2	220.19
3000 s	−0.88205	−2.5697	−2946.9	−24.945
3500 s	−2.9383	−2.1155	−4142.2	−1083.3
3600 s	−3.6086	−1.9593	−4345.4	−1474.5

.

6. Conclusions

Under the conditions of unknown initial attitudes or strong disturbance, the error equations of SINS have strong nonlinearity. In such conditions, the linear error models of SINS are inconsistent with the reality. The filtering alignment based on classical linear KF has the disadvantages of too long convergence time or filtering divergence. In this paper, a novel SINS fine alignment method which is based on AFIS-EKF2 filter is proposed to realize the fine alignment of SINS with large misalignment angles. The simulation analyses were performed, which include the Monte Carlo simulation on stationary base, alignment simulation with very large misalignment angles and alignment simulation on swaying base. The simulation results show that the AFIS-EKF2 filtering algorithm is suitable for SINS fine alignment with arbitrarily large misalignment angles. The turntable experiment was performed, and the effectiveness and superiority of the proposed method were further verified by compared with other nonlinear methods. The proposed fine alignment algorithm has important practical significance for some important special application scenarios of too short coarse alignment time (e.g., the missile launch vehicle under the threat of artillery attacks) or the fine alignment without coarse alignment process (e.g., missile-borne SINS).