An Improved SINS Alignment Method Based on Adaptive Cubature Kalman Filter

Initial alignment is critical and indispensable for the inertial navigation system (INS), which determines the initial attitude matrix between the reference navigation frame and the body frame. The conventional initial alignment methods based on the Kalman-like filter require an accurate noise covariance matrix of state and measurement to guarantee the high estimation accuracy. However, in a real-life practical environment, the uncertain noise covariance matrices are often induced by the motion of the carrier and external disturbance. To solve the problem of initial alignment with uncertain noise covariance matrices and a large initial misalignment angle in practical environment, an improved initial alignment method based on an adaptive cubature Kalman filter (ACKF) is proposed in this paper. By virtue of the idea of the variational Bayesian (VB) method, the system state, one step predicted error covariance matrix, and measurement noise covariance matrix of initial alignment are adaptively estimated together. Simulation and vehicle experiment results demonstrate that the proposed method can improve the accuracy of initial alignment compared with existing methods.


Introduction
The strapdown inertial navigation system (SINS), which is based on a numerical integration procedure, can provide consecutive navigation parameters, including the attitude, velocity, and position for carriers [1,2]. For the dead-reckoning navigation stage, the performance of SINS heavily depends on the accuracy of initial navigation information [3]. The large initial errors (especially the attitude error) will seriously degrade the navigation accuracy. Thus, it is important to estimate the initial attitude of SINS and reduce the initial attitude error. The procedure of determining the initial attitude is named as initial alignment [4][5][6].
Traditional initial alignment methods can be divided into two stages: coarse alignment and fine alignment [7,8]. Coarse alignment, which mainly includes analytic coarse alignment [9], inertial frame coarse alignment [10], and Davenport's q method based coarse alignment [11][12][13], is to decrease the large misalignment angles into a small range to ensure that the fine alignment has a linear model. However, the average coarse alignment time is hundreds of seconds. When the required time of initial alignment has a stringent restriction, it is difficult to ensure that the misalignment angles have converged to a small range in the coarse alignment stage [4,14]. The condition of a fine alignment with a large misalignment angle may occur. Furthermore, this alignment process needs to switch from coarse alignment to fine alignment at a suitable point of time, which increases the complexity and uncertainty of the alignment frame and the calculated navigation frame, respectively, which are the east-north-up (E-N-U) orientation. The schematic diagram of these frames is shown in Figure 1. The standard state equation of initial alignment is the same as the error model of SINS [4]. Considering the application of carborne or shipborne equipment, the height error of the carrier is neglected. The nonlinear error equation for the moving state of SINS is given as follows: ib − (2ω n ie +ω n en ) × δv n −(2δω n ie + δω n en ) × (ṽ n − δv n ) + (C n n ) T C n b (∇ b + η b a ) δL = δv n N R e δφ = secL R e δv n E +ṽ n E secL tanL R e δL ε b = 0,∇ b = 0 (1) with: where the misalignment angles φ = [φ x ; φ y ; φ z ] are the attitude error angle between frame n and frame n . δv n = [δv n E ; δv n N ] is the error of calculated velocityṽ n . δL and δϕ are the errors of calculated latitudeL and longitudeφ. ε b is the gyro constant drift. η b g is the gyroscope random noise. ∇ b is the accelerometer bias. η b a is the accelerometer random noise. R e is the radius of the Earth. (·)× denotes the 3 × 3 skew symmetric matrix.
[·] T denotes the transposition of the matrix. The attitude error matrix C n n is formulated as follows: The erroneousω n ie andω n en are given as follows: where ω ie is the Earth's rotation angular velocity. The errors ofω n ie andω n en are given as follows: Therefore,ω n in and δω n in are given as follows: ω n in =ω n ie +ω n en (8) δω n in = δω n ie + δω n en (9) The velocity and position differences between SINS and aiding equipment such as GPS are selected as the measurement, which is formulated as: where ν v and ν P are velocity measurement noise and position measurement noise, respectively. The state vector is defined as The state noise vector is defined as w = [η s g ; η s a ; 0 7×1 ]. The measurement vector and noise vector are defined as z = [z v ; z P ] and ν = [ν v ; ν P ], respectively. By discretizing the continuous equations, the standard discrete state error equation and measurement equation are formulated as: where f (·) and g(·) are the nonlinear functions, which are formulated based on (1), and the measurement model is a linear function, w k and ν k are uncorrelated Gaussian white noises with mean value 0 and covariance Q k and R k , respectively. For the sake of simplification in the next sections, we assume that the dimensions of x k and w k are n x and the dimensions of z k and ν k are n z . The standard nonlinear initial alignment model can be obtained by Equation (11). It can be seen that the initial alignment state model has high dimensions and that the states are coupling strongly. Generally, the state noise and measurement noise of initial alignment are set as Gaussian white noise. However, the severe maneuver and the external disturbance will induce the uncertain noise covariance matrices of SINS, which will degrade the accuracy of initial alignment. In the next section, the adaptive CKF is proposed to solve the problem mentioned above.

Adaptive Cubature Kalman Filter
In this section, a novel adaptive CKF is proposed to solve the estimation problem with the uncertain noise covariance matrix. Without loss of generality, we introduce this method based on the standard nonlinear model with the nonlinear state and measurement functions.

Gaussian Kalman Filter and Cubature Kalman Filter
The Gaussian filter is the main method to solve the nonlinear estimation, which has two key assumptions, that the one step predicted PDFs of the state and measurement are Gaussian, i.e., wherex k|k−1 and P k|k−1 denote the mean and variance of p (x k |z 1:k−1 ).
whereẑ k|k−1 and P zz k|k−1 denote the mean and variance of p (z k |z 1:k−1 ). Obviously, the joint one step predicted PDF of the state and measurement p (x k , z k |z 1:k−1 ) is also Gaussian, i.e., where P xz k|k−1 is the covariance of x k and z k . Based on (14) and (15), in the Bayesian theorem, the posterior PDF of x k is also Gaussian, i.e., wherex k|k and P k|k denote the mean and variance of p (x k |z 1:k ).x k|k and P k|k are derived as follows: where K k is the filter gain, and the other parameters are calculated as follows: where E[·] means the expectation operation. From (20) to (24), the general framework of the Gaussian filter is established, and the core idea of the Gaussian filter is to calculate Gaussian weighted integrals. Due to the nonlinearity of f (·) and h(·), it is difficult to obtain the accurate numerical solution of (20)- (24), and the approximation solution is necessary, i.e., where x j and W j are the sampling points and corresponding weights of x. CKF, which is a typical Gaussian filter, uses the third degree spherical-radial cubature rule to obtain these weighted samples. In (20), the cubature points of x k−1 are selected based onx k−1|k−1 and P k−1|k−1 . These cubature points are defined as follows: where (A) (j) denotes the jth column of A. Propagating the cubature points of x k−1 by f (·), the state one step predicted meanx k|k−1 and covariance P k|k−1 can be obtained as follows based on (20) and (21): Furthermore, the cubature points of x k based onx k|k−1 and P k|k−1 are selected as follows: Propagating the cubature points of x k by h(·), the measurement one step predicted mean and covariance can be obtained as follows: Filter gain K k and measurement update are given as follows:

The Proposed Adaptive Cubature Kalman Filter
When the state noise covariance Q k and measurement noise covariance R k are unknown or inaccurate, the estimation accuracy of CKF may degrade or diverge. Because the one step predicted state error covariance P k|k−1 is influenced by the inaccurate Q k , it is easier to estimate P k|k−1 than Q k . Therefore, in our works, the state, one step predicted state error covariance P k|k−1 and R k are jointly estimated to improve the accuracy of CKF with inaccurate noise statistical properties.
In the frame of Bayesian probability theory, the conjugate prior distribution is selected to guarantee the unified form of the prior and posterior distribution. For the Gaussian distribution with known mean, the standard inverse Wishart (IW) PDF is always used as the conjugate prior distribution. The IW PDF is formulated as follows: When ζ > d + 1, the mean of B is shown as follows: Therefore, the prior distribution p(P k|k−1 |z 1:k−1 ) and p(R k |z 1:k−1 ) are modeled as follows: wheret k|k−1 andû k|k−1 are dof parameters andT k|k−1 andÛ k|k−1 are inverse scale matrices. The mean value of P k|k−1 is set as nominalP k|k−1 , determined by: whereQ k−1 is the nominal state noise covariance matrix, which means an inaccurate value.
and sett k|k−1 = n x + τ + 1, where τ is a tuning parameter. We can obtain: According to the Bayesian theorem, p (R k |z 1:k−1 ) is formulated as: Because the posterior and prior PDF of R k−1 has the same distribution, the posterior PDF of R k−1 is also formulated as the inverse Wishart distribution, as follows: Because of the unknown dynamic model of p (R k |R k−1 ), we selected a forgetting factor ξ ∈ (0, 1] to spread the previous posterior to the current prior, and the prior parameters in (41) are written as follows: The initial R 0 is also assumed as an inverse Wishart PDF, i.e., p (R 0 ) = IW(R 0 ;û 0|0 ,Û 0|0 ), where the mean value of R 0 is set as the initial nominalR 0 : In order to estimate the state x k , one step predicted state error covariance P k|k−1 and R k , their joint posterior PDF p x k , P k|k−1 , R k |z 1:k is calculated. Due to the coupling of these parameters, the analytical solution cannot be obtained. Therefore, the VB method is used to solve the estimation problem in coupling.
q(x k ), q(P k|k−1 ), q(R k ) are calculated by minimizing the Kullback-Leibler divergence (KLD): The optimal solution of (51) is given by: where log(·) means the logarithmic function, α is the arbitrary element of Ξ, Ξ (−α) contains all elements in Ξ except for α, and c α means the constant dependent on α. According to the Bayesian theorem, the joint PDF p(Ξ, z 1:k ) is factored as: where likelihood PDF p (z k |x k ) is assumed as a normal distribution.
The cubature points of x k based onx k|k−1 and modifiedP k|k−1 are given as: , (j = n x + 1, n x + 2, · · · , 2n x ) After N fixed point iterations, we can obtain the approximate solution of q(x k ), q(P k|k−1 ) and q(R k ): When the measurement model is linear, such as the initial alignment measurement model in (12), we can obtain the simplified algorithm, where (66) and (81)-(89) are formulated as follows: The implementation pseudocode of the proposed adaptive cubature Kalman filter is shown in Algorithm 1.
To implement the proposed ACKF method, we need to select the tuning parameter τ, the forgetting factor ξ, and the iteration number N. Tuning parameter τ can be seen as an adjustment parameter ofP k|k−1 . If τ is too large, the prior uncertainties induced by nominalQ k will influence the measurement update. If τ is too small, the information of the process model will be also lost. According to the research result of [27], the optimal range of the turning parameter is τ ∈ [2, 6], which has better estimation performance and estimation accuracy. The forgetting factor ξ also adjusts the influence ofR k−1 . Note that ξ = 1 means the stationary measurement noise covariance. A large iteration number N will improve the estimation accuracy, but also increase the computational cost. According to our experience, N > 5 will have good performance in the alignment. Inputs:x k−1|k−1 , P k−1|k−1 , z k ,û k−1|k−1 ,Û k−1|k−1 ,Q k−1 , τ, ξ, N. Time update 1. Calculate cubature points based onx k−1|k−1 and P k−1|k−1 .

χ x(j)
9. Calculate the mean and variance of posterior PDF, Outputs:x k|k , P k|k ,û k|k ,Û k|k .

Simulation
Firstly, the simulation is given as follows. Through the designed trajectory of the carrier, the outputs of the gyroscope and accelerometer with errors could be obtained according to their mathematic models. In addition, by adding errors into the true velocity and position of the trajectory, the measurements for initial alignment were built as the output of a virtual GPS receiver. The simulation process can be summarized as the following block diagram which is shown in Figure 2.
The initial latitude and longitude of the carrier were set as 45.776 • N and 126.446 • E, respectively. The constant velocity was set as 10 m/s. The motion of the carrier was set as the typical swing process based on the sine function: where pitch m , roll m , and heading m are swing amplitudes, which were selected as pitch m = 5 • , roll m = 6 • , and heading m = 7 • ; T p , T R , and T H are swing periods, which were selected as T p = 7 s, T R = 8 s, and T H = 9 s; pitch 0 , roll 0 , and heading 0 are initial swing phases, which were selected as pitch 0 = 0 • , roll 0 = 0 • , and heading 0 = 0 • ; pitchI, roll I, and headingI are initial attitude angles, which were selected as pitchI = 0 • , roll I = 0 • , and headingI = 45 • ; T s = 0.01 s is the discrete time of the system. The initial misalignment angles were set as φ = [5 • ; 5 • ; 15 • ]. The nominal sensor specifications of SINS in the simulation were set as the practical SINS, which are shown in Table 1.   [18], CKF [14], the Sage-Husa adaptive Kalman filter (SHKF) [26], and CKF with true noise covariance matrices (TCKF) were selected to compare the performance with the proposed ACKF method. For the proposed ACKF, the tuning parameter, forgetting factor, and iteration number were set as τ = 5, ξ = 0.98, and N = 10, respectively. The initial state was set asx 0|0 = 0 12×1 . The initial state error covariance matrix was set as P 0|0 = diag([5 • ; 5 • ; 15 • ; 0.1m/sI 2×2 ; arctan(10m/R e )I 2×2 ; 0.01 • /hI 3×3 ; 10 −4 gI 2×2 ]) 2 . The frequency of measurement updating was set as 10 Hz. The total number of Monte Carlo runs was set as M = 30. Furthermore, to evaluate the estimation accuracy of P k|k−1 and R k , the square root of normalized Frobenius norm (SRNFN) was used, which is defined as follows: whereP k|k−1 means the accurate one step predicted state error covariance matrix provided by TCKF.
The alignment errors of different methods are shown in Figure 3. The average alignment errors in the latter 20 s are shown in Table 2. The results of SRNFNs are shown in Figure 4. Note that due to the bad stability of SHKF, its simulation results are not shown in the following simulation. It was because that when the error covariance matrices of the state model and measurement model needed to be estimated simultaneously, the estimation accuracy of SHKF was poor, and its stability was heavily influenced by the non-positive definite of noise covariance matrices.  From the results of simulation, it can be seen from Figure 3 that the proposed ACKF had better alignment accuracy than the existing CKF and UKF, and its alignment error was close to the error from TCKF. This was because the proposed ACKF could adaptively estimate P k|k−1 and R k and eliminate the influence of the inaccurate noise covariance information. It also can be seen from Figure 4 that the proposed ACKF had smaller SRNFNs than the existing CKF and UKF. In the conventional Kalman filter, P k|k−1 represents the predicted error based on the measurement information z 1:k−1 . Due to the inaccurate noise covariance matrices of the state and measurement, the conventional filters relied on the wrong information. Thus, the convergence speed and estimation accuracy of CKF and UKF were lower than those of TCKF and ACKF.
To further discuss the influence of the parameters used in the proposed ACKF, simulations with different tuning parameters τ, forgetting factors ξ, and iteration numbers N were conducted. Tables 3-5 show respectively the estimation errors with different τ, ξ, and N. It can be seen that the proposed ACKF had a consistent estimation performance when τ ∈ [2, 6], ξ = 0.96, 0.97, 0.98, 0.99 and N > 5, which also corresponded to the aforementioned analyses.

Vehicle Experiment
Secondly, a vehicle experiment was performed to validate the performance of the proposed ACKF method in practice. The photonics inertial navigation system (PHINS) produced by the company IXSEA France, a self-made SINS, and the antenna of GPS receiver were mounted on the car as shown in Figure 5. The self-made SINS had a three axis fiber optic gyroscope and accelerometer to measure body angular rate and specific force, respectively. The theoretical sensor specifications of SINS were the same as the simulation in Table 1. The GPS receiver could provide position and Doppler derived velocity measurements to carry out initial alignment and integrate with PHINS to constitute a high accuracy attitude reference system for SINS. The lever arm and the installation error angles between SINS and PHINS were compensated. The output frequency of SINS and PHINS/GPS integrated navigation system were 100 Hz and 10 Hz, respectively. The root mean square of the attitude accuracy of the PHINS/GPS integrated navigation system was 0.01 • for roll and pitch angles and 0.01 • sec(L) for the heading angle. Because of the uncertainty of the sensor parameters of SINS and the influence of external disturbance in motion conditions, we could not obtain the accurate sensor parameters, which determined the state noise covariance values in the Kalman filter. Therefore, the theoretical sensor parameters were nominal and inaccurate in the practical environment. The experiment was carried out in an urban area (45.776 • N, 126.446 • E), and the running stage continued for 1850 s, including two parts: smooth running stage (0 s to 735 s) and maneuvering stage (735 s to 1850 s). The running trajectory, attitude, and velocity of the car provided by the PHINS/GPS integrated navigation system are shown in Figures 6-8, respectively. It can be seen that the car had frequent turn movements in the severe maneuvering stage, which would subsequently induce the uncertain sensor parameters of SINS. Therefore, the data of this running stage could better verify the effectiveness of the proposed ACKF method.
Normally, the initial attitude matrix of SINS was set as the identity matrix. However, this may not induce the large misalignment angles sometimes. Therefore, the additional large misalignment angles, which were set as φ = [5 • ; 5 • ; 15 • ], were added into the reference initial attitude matrix of PHINS/GPS integrated navigation system to make up the initial attitude matrix of SINS. The nominal state noise covariance matrixQ k and nominal measurement noise covariance matrixR k were set the same as the simulation. The tuning parameter, forgetting factor, and iteration number were set as τ = 5, ξ = 0.98, and N = 10, respectively. Firstly, to compare the performance of the proposed ACKF and the existing methods, we used the whole data to simulate the initial alignment process. Because the true noise covariance matrices were unknown in the practical environment, the calibration results of SINS as shown in Table 1 were selected as the parameters of the filters. The alignment errors of these methods are shown in Figure 9. From the results of the experiment, it can be seen that the performances of ACKF, UKF, and CKF were similar in the smooth running stage, which was because the preset nominal values were close to the true values for the high accuracy SINS. However, in the maneuvering stage, the performances of UKF and CKF were worse than the proposed ACKF, especially the heading angle error, which is because the noise covariance matrices were changing when the carrier was maneuvering, which had a severe influence on the alignment accuracy. Secondly, because the alignment times were very short, we selected six different segments in the whole test data, including smooth segments (0 s to 100 s, 200 s to 300 s, 500 s to 600 s) and maneuvering segments (800 s to 900 s, 1300 s to 1400 s, 1700 s to 1800 s). The alignment errors of these methods are shown in Figures 10-15 Tables 6 and 7, it can be seen that the proposed ACKF always had better performance than UKF and CKF. This was because the proposed ACKF could better estimate the measurement noise covariance matrix and the one step prediction covariance matrix influenced by the state noise covariance matrix. Besides, combined with the heading angle curve in Figure 7, we can find that, when the car was making a turn, the performance of these three methods was becoming worse simultaneously. That was because the residual scale error and dynamic error of initial sensors in the turning process were large and would degrade the accuracy of initial alignment. However, the proposed ACKF could quickly converge when the turning process ended. Therefore, compared with the traditional methods and existing adaptive methods, we could conclude that the proposed ACKF had better stability and estimation accuracy, which could eliminate the influence of the uncertain state noise covariance matrix and measurement noise covariance matrix.

Conclusions
This paper proposed a novel adaptive cubature Kalman filter based variational Bayesian method to solve the alignment problem of SINS with initial large misalignment angles and uncertain noise covariance matrices. The one step predicted error covariance matrix and measurement noise covariance matrix were adaptively estimated together. Simulation and vehicle experiment results illustrated that the proposed ACKF had better stability and estimation accuracy than the existing adaptive filter methods and traditional nonlinear filter methods.
Author Contributions: Y.Z. and G.X. performed the formal analysis and validated the feasibility; Y.Z. provided the experiment tools and designed the experiment; G.X. analyzed the experiment data. Y.Z., G.X., and X.L. wrote this paper.