Application of Improved 5th-Cubature Kalman Filter in Initial Strapdown Inertial Navigation System Alignment for Large Misalignment Angles

Abstract

In view of the fact the accuracy of the third-degree Cubature Kalman Filter (CKF) used for initial alignment under large misalignment angle conditions is insufficient, an improved fifth-degree CKF algorithm is proposed in this paper. In order to make full use of the innovation on filtering, the innovation covariance matrix is calculated recursively by an innovative sequence with an exponent fading factor. Then a new adaptive error covariance matrix scaling algorithm is proposed. The Singular Value Decomposition (SVD) method is used for improving the numerical stability of the fifth-degree CKF in this paper. In order to avoid the overshoot caused by excessive scaling of error covariance matrix during the convergence stage, the scaling scheme is terminated when the gradient of azimuth reaches the maximum. The experimental results show that the improved algorithm has better alignment accuracy with large misalignment angles than the traditional algorithm.

1. Introduction

Initial alignment is the basis for obtaining accurate misalignment angles of Strapdown Inertial Navigation Systems (SINS), and its accuracy and efficiency will directly affect the SINS performance [1,2]. In an application environment with large misalignment angles and moving bases, the linear initial alignment algorithm is greatly restricted [3], while nonlinear algorithms have been increasingly used for initial alignment [4,5].

In order to apply the linear Kalman filter to a nonlinear system, the Extended Kalman Filter (EKF) [6] truncates the Taylor series expansion of the nonlinear function. The accuracy of EKF is low for strongly nonlinear systems, and there may be a problem in that the Jacobi matrix cannot be solved [7]. An Unscented Transformation (UT) is used in the Unscented Kalman Filter (UKF) [8] to replace the local linearization for obtaining the first two moments of the states. Although the precision of UKF is higher than EKF, there is a “dimension disaster” problem [9] in high dimensional applications of UKF, and parameter adjustment is needed when using UKF. Spherical Radial Cubature Rule (SRCR) is used in the third-degree CKF (hereinafter referred to as CKF3) to approximate the nonlinear integral and obtain the two moments of the states, which can achieve third-order accuracy [10]. CKF has a better numerical stability than UKF, and has been widely applied [11,12]. In order to further improve the accuracy of the CKF3, fifth-degree CKFs (hereinafter referred to as CKF5) [13,14] have been applied gradually [15]. CKF5 has problems of numerical stability and big calculation [16], and in this sense, CKF5 is more suitable for low dimensional systems. The adaptive Kalman filter is an effective method to restrain the divergence and improve the accuracy of filtering. The main idea of the adaptive Kalman filter is to scale the part of error covariance matrix or system noise matrix by calculating the ratio of matrix ranks of actual updating and filter updating [17,18]. The traditional method for estimating the updating covariance is the open window method, which uses the mean value of historical data and not takes into account the different weights of new and old updates. The accuracy and real-time performance of the traditional adaptive Kalman filter should be improved. Combining EKF and adaptive filtering [19], the update covariance matrix is directly introduced into the calculation of gain matrix, which improves the utilization rate of updates. However, EKF cannot effectively meet the accuracy requirement of the nonlinear initial alignment with large misalignment angles. Adaptive filtering is introduced into Unscented Particle Filter (UPF) [20], and the efficiency of convergence can be improved by partially scaling the error covariance matrix. Nevertheless, the precision of in-motion and nonlinear initial alignment with large misalignment angles should be further verified experimentally. The 5-th CKF is directly applied to the initial alignment [12], but experiments on initial alignment with large misalignment angles are lacking. The singular value decomposition can improve the robustness of the initial alignment algorithm [21], but there is no verification of actual data except for simulations.

In order to improve the efficiency and accuracy of the 5-th CKF in initial alignment situations, an entire scaling of the error covariance matrix method with the ratio of matrix ranks of actual updates and filtered updates is introduced in this paper. To solve the problem of oscillation or divergence of improved 5-th CKF caused by excessive scaling of the error covariance matrix, the scaling is terminated when the gradient of the azimuth error reaches the maximum. The improved algorithm needs to store the historical IMU data, GNSS data and the calculated azimuth misalignment angle data, and then recalculate it for alignment. To improve the stability of the 5-th CKF algorithm, the Singular Value Decomposition (SVD) algorithm is used instead of the Cholesky’s method in the matrix decomposition. The large misalignment angle model is simplified in this paper, which means that only the velocity errors and the misalignment angles are used as the states. The reduction of the states has the advantages of requiring less calculation and better calculation stability, which can reduce the chances of filter divergence.

In this paper, the nonlinear model of initial alignment with large misalignment angles is first analyzed and a simplified alignment model with 6-dimensional state variables is proposed. Secondly, the traditional CKF5 algorithm is briefly described in this paper, and then the improved algorithms based on fading factor for the updating covariance matrix, short-term scaling of the error covariance matrix and SVD decomposition are introduced. Finally, the effectiveness of the improved algorithms is verified by simulation experiments and the dynamic vehicle experiments.

2. Nonlinear Model of Initial Alignment with Large Misalignment Angle

The SINS updating algorithm includes attitude-updating, speed-updating and position-updating, and the corresponding differential equations are expressed with Equations (1) to (3) [22]:

$${\dot{C}}_b^n=C_b^n({\omega }_{nb}^b\times ),$$

(1)

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

(2)

$$\left[\begin{array}{l}\dot{L}\\ \dot{\lambda }\\ \dot{h}\end{array}\right]=\left[\begin{array}{ccc}0& 1/R_{Mh}& 0\\ \sec L/R_{Nh}& 0& 0\\ 0& 0& 1\end{array}\right]v^n.$$

(3)

where $C_b^n$ is the direction cosine matrix of the attitude converting from the measurement reference frame (abbreviated as b frame) to the navigation reference frame (abbreviated as n frame), ${\omega }_{nb}^b={\omega }_{ib}^b-\left({\omega }_{ie}^b+{\omega }_{en}^b\right)$ is the rotational angular rate of b frame relative to n frame, i is the inertial reference frame, (${\omega }_{nb}^b\times )$ is the antisymmetric matrix of ${\omega }_{nb}^b$, the vector ${\nu }^n={\left[{\nu }_E {\nu }_N {\nu }_U\right]}^T$ expresses the eastern velocity, northern velocity and vertical velocity, respectively, $f_{sb}^b$ is the output of the accelerometer, L is the latitude of the location of carrier, λ is the longitude of the location of carrier, h is the height of the location of carrier, R_Mh is the radius of meridian of the location of carrier and R_Nh is the radius of prime vertical circle of the location of carrier. Assuming n′ and n are the calculation reference frame and ideal navigation reference frame, respectively, the misalignment (expressed as Euler angle) between the two navigation reference frames can be expressed as:

$$\dot{\varphi }=C_{\omega }^{-1}{\omega }_{n{n}^{\prime }}^{n^{\prime }},$$

(4)

$${\omega }_{n{n}^{\prime }}^{n^{\prime }}=(I-C_n^{n^{\prime }}){\widehat{\omega }}_{in}^n+C_n^{n^{\prime }}\delta {\omega }_{in}^n-C_b^{n^{\prime }}({\epsilon }^b+w_g^b),$$

(5)

where ${\omega }_{{nn}^{\prime }}^{n^{\prime }}$ is the relative rotational angular rate between the n reference frame and n′ reference frame, ε^b is the random constant drift of Fiber Optic Gyroscope (FOG) and $w_g^b$ is the random drift of FOG. $C_{\omega }^{-1}$ and $C_n^{n^{\prime }}$ can be expressed as:

$$C_{\omega }^{-1}=\left[\begin{array}{ccc}\cos {\varphi }_y& 0& \sin {\varphi }_y\\ \sin {\varphi }_x\sin {\varphi }_y/\cos {\varphi }_x& 1& -\sin {\varphi }_x\cos {\varphi }_y/\cos {\varphi }_x\\ -\sin {\varphi }_y/\cos {\varphi }_x& 0& \cos {\varphi }_y/\cos {\varphi }_x\end{array}\right]$$

(6)

$$C_n^{n^{\prime }}\approx \left[\begin{array}{ccc}\cos {\varphi }_y\cos {\varphi }_z-\sin {\varphi }_x\sin {\varphi }_y\sin {\varphi }_z& \cos {\varphi }_y\sin {\varphi }_z+\sin {\varphi }_x\sin {\varphi }_y\sin {\varphi }_z& -\cos {\varphi }_x\sin {\varphi }_y\\ -\cos {\varphi }_x\sin {\varphi }_z& \cos {\varphi }_x\cos {\varphi }_z& \sin {\varphi }_x\\ \sin {\varphi }_y\cos {\varphi }_z+\sin {\varphi }_x\cos {\varphi }_y\sin {\varphi }_z& \sin {\varphi }_y\sin {\varphi }_z-\sin {\varphi }_x\cos {\varphi }_y\cos {\varphi }_z& \cos {\varphi }_x\cos {\varphi }_y\end{array}\right].$$

(7)

Under the reference frame, Equation (2) can be written as:

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

(8)

Subtracting Equation (2) from Equation (8) and neglecting the calculation error of gravitational acceleration and higher-order terms, we obtain:

$$\delta {\dot{v}}^n=[I-{(C_n^{n^{\prime }})}^T]C_b^{n^{\prime }}{\widehat{f}}_{sf}^b+{(C_n^{n^{\prime }})}^T{C}_b^{n^{\prime }}{\nabla }^b-(2{\widehat{\omega }}_{ie}^n+{\widehat{\omega }}_{en}^n)\times \delta v^n,$$

(9)

where ${\widehat{\nu }}^{n^{\prime }}={\nu }^n+\delta {\nu }^n, {\widehat{f}}_{sf}^b=f_{sf}^b+\delta f_{sf}^b, {\nabla }^b=\delta f_{sf}^b$, and we have:

$${\dot{\epsilon }}^b=0,$$

(10)

$${\dot{\nabla }}^b=0.$$

(11)

The purpose of initial alignment is to quickly identify the misalignment angles. It is difficult to estimate the errors of inertial sensors accurately, and that is not the purpose of initial alignment, then the errors of ε^b and ∇_b can be regarded as system noises. Therefore, the states used in initial alignment are misalignment angles φ and the velocity error δνⁿ, which can reduce the dimensions of the filter, and thus reduce the computation and improve the numerical stability of CKF5. In order to simplify the filtering error equation, a SINS nonlinear error model which can reflect the characteristics of SINS errors has important practical value in the initial alignment [20]. When the SINS and GNSS are integrated for initial alignment under moving base conditions, GNSS can provide relatively accurate velocity and location information, then Equations (4) and (9) can be simplified as:

$$\dot{\varphi }=C_{\omega }^{-1}[(I-C_n^{n^{\prime }}){\widehat{\omega }}_{in,g}^n-{\epsilon }^{n^{\prime }}]+C_{\omega }^{-1}{C}_b^{n^{\prime }}{w}_g^b,$$

(12)

$$\delta {\dot{v}}^n=[I-{(C_n^{n^{\prime }})}^T]C_b^{n^{\prime }}{\widehat{f}}_{sf}^b+{(C_n^{n^{\prime }})}^T{\nabla }^{n^{\prime }}+{(C_n^{n^{\prime }})}^T{C}_b^{n^{\prime }}{w}_a^b,$$

(13)

where $w_a^b$ is the random drift of accelerometer, ${\widehat{\omega }}_{in,g}^n$ is the angular velocity of the Earth determined by the information from GNSS. With the difference between the velocity of SINS measured by GNSS and the calculated velocity working as the measurement of z, the nonlinear filtering equations of initial alignment with large misalignment angles can be expressed as:

$$\{\begin{array}{l}\dot{x}=f(x)+g(x)w,\\ z=Hx+v,\end{array}$$

(14)

where x = [φ (δνⁿ)^T], $w=\left[{\left(w_g^b\right)}^T{\left(w_a^b\right)}^T\right]$, H = [0_3×3 I_3×3], ν is the error vector of measurement and the function of f can be obtained from Equation (12). The model of initial alignment (Equations (12) and (13)) takes into account of the moving base situation, so the estimation of the initial alignment angles will theoretically not be affected by the line movement and angle movement of SINS.

3. CKF5 and Improved CKF5

3.1. CKF5 Algorithm

For the integral of the global nonlinear function I(f) and variable x = ry in the spherical-radial coordinate system, I(f) can be written as:

$$\mathit{I}(f)={\displaystyle {\int }_0^{\infty }{\displaystyle {\int }_{U_n}f(ry)r^{n-1}{e}^{-r^2}d\sigma (y)dr}},$$

(15)

where δ^Tδ = 1, r ∈ [0, ∞), U_n is n-dimensional unit hypersphere and σ(g) is the element of U_n. I(f) can be divided into two parts, spherical integral S(r) and radial integral R:

$$\{\begin{array}{l}S(r)={\displaystyle {\int }_{U_n}f(ry)d\sigma (y),}\\ R={\displaystyle {\int }_0^{\infty }S(r)r^{n-1}{e}^{-r^2}dr.}\end{array}$$

(16)

The numerical integration is approximated with the summation of weighted sigma points in the algorithm of CKF, then Equation (16) can be expressed as:

$$\{\begin{array}{l}S(r)\approx {\displaystyle \sum _{j=1}^{N_s}{\omega }_{s,j}f(r{y}_j),}\\ R\approx {\displaystyle \sum _{i=1}^{N_r}{\omega }_{r,i}f(r_i).}\end{array}$$

(17)

Then:

$$I(f)\approx {\displaystyle \sum _{i=1}^{N_r}{\displaystyle \sum _{j=1}^{N_s}{\omega }_{r,i}{\omega }_{s,j}f(r_i{y}_j)}}.$$

(18)

The m-order approximation of S(r) can be written as:

$$S=\{\begin{array}{l}\frac{2{\displaystyle \prod _{i=1}^n\mathsf{\Gamma }[(d_i+1)/2]}}{\mathsf{\Gamma }[(\left|d\right|+n)/2]},\left\{d_i\right\}\mathrm{are}\mathrm{even}\mathrm{numbers},\\ 0,\mathrm{at}\mathrm{least}\mathrm{one}\mathrm{odd}\mathrm{number}\mathrm{in}\left\{d_i\right\},\end{array}$$

(19)

where d = [p₁,p₂,…,p_n], $\left|d={\displaystyle \sum }_{i=1}^n{d}_i\right|$, $\left|d\right|\le m$, and Γ(g) is the gamma function. Note that when m = 5, the value of S is calculated with $f_1(y)=1,f_2(y_1)=y_1^2,f_3(y_1)=y_1^4$ and $f_4(y_1,y_2)=y_1^2{y}_2^2$. The points set on the U_n is defined as:

$$\{\begin{array}{l}{[e]}_n=\left\{\pm {\left\{e_i\right\}}_{i=1}^n\right\},\\ {[s]}_n=\left\{\pm {\left\{s_i\right\}}_{i=1}^{n(n-1)}\right\},\end{array}$$

(20)

where ${\left\{e_i\right\}}_{i=1}^n$ and ${\left\{s_i\right\}}_{i=1}^{n(n-1)}$ are defined as:

$$\{\begin{array}{l}{\left\{e_i\right\}}_{i=1}^n=\left\{e_1,e_2,\dots ,e_n\right\},\\ {\left\{s_i\right\}}_{i=1}^{n(n-1)}=\left\{(e_k\pm e_l)/\sqrt{2}:k,l=1,2,\dots ,n.\mathrm{and}k<l\right\},\end{array}$$

(21)

where $e_i$ is the ith column of identity matrix ${\mathit{I}}_{n\times n}$. Then the Equation (19) can be expressed as:

$$S(r)\approx {\omega }_1{\displaystyle \sum _{i=1}^{2n}f(r{[e]}_i)}+{\omega }_2{\displaystyle \sum _{i=1}^{2n(n+1)}f(r{[s]}_i)},$$

(22)

where ${\omega }_1=\frac{(4-n)A_n}{2n(n+2)}$, ${\omega }_2=\frac{A_n}{n(n+2)}$ and $A_n=\left\{{[a]}_n,{[s]}_n\right\}$. For the symmetry, $S_1(r)=1,S_2(r)=r^2$ and $S_3(r)=r^4$ are needed to calculated in the solution of 5-order integral of $S(r)$. Then

$$\{\begin{array}{l}{\omega }_{r,1}{r}_1^0+{\omega }_{r,2}{r}_2^0=\frac{1}{2}\mathsf{\Gamma }(n/2),\\ {\omega }_{r,1}{r}_1^2+{\omega }_{r,2}{r}_2^2=\frac{1}{2}\mathsf{\Gamma }(n/2+1)=\frac{n}{4}\mathsf{\Gamma }(n/2),\\ {\omega }_{r,1}{r}_1^4+{\omega }_{r,2}{r}_2^4=\frac{1}{2}\mathsf{\Gamma }(n/2+2)=\frac{1}{2}(\frac{1}{2}n+1)(n/2)\mathsf{\Gamma }(n/2).\end{array}$$

(23)

Let $r_1=0$, then we have:

$$\{\begin{array}{l}{r}_2=\sqrt{\frac{1}{2}n+1},\\ {\omega }_{r,1}=\frac{1}{(n+2)}\mathsf{\Gamma }(n/2),\\ {\omega }_{r,2}=\frac{n}{2(n+2)}\mathsf{\Gamma }(n/2).\end{array}$$

(24)

Then it follows from Equations (19) and (23) that the integrated result of Equation (18) can be obtained. The algorithm flow of CKF5 is shown as follows:

Prediction phase:

• Let $x_{k-1}~N(x_{k-1};{\widehat{x}}_{k-1|k-1},P_{k-1|k-1})$ be the estimated state at time $k-1$ and decompose $P_{k-1|k-1}$ with Cholesky’s method:

  $$P_{k-1|k-1}=S_{k-1|k-1}{S_{k-1|k-1}}^T.$$

  (25)
• Compute cubature points:

  $$\{\begin{array}{l}{x}_1=S_{k-1|k-1}\cdot 0+{\widehat{x}}_{k-1|k-1},\\ x_2=S_{k-1|k-1}\sqrt{n+2}\cdot s_j^{+}+{\widehat{x}}_{k-1|k-1},\\ x_3=-S_{k-1|k-1}\sqrt{n+2}\cdot s_j^{+}+{\widehat{x}}_{k-1|k-1},\\ x_4=S_{k-1|k-1}\sqrt{n+2}\cdot s_j^{-}+{\widehat{x}}_{k-1|k-1},\\ x_5=-S_{k-1|k-1}\sqrt{n+2}\cdot s_j^{-}+{\widehat{x}}_{k-1|k-1},\\ x_6=S_{k-1|k-1}\sqrt{n+2}\cdot e_j+{\widehat{x}}_{k-1|k-1},\\ x_7=-S_{k-1|k-1}\sqrt{n+2}\cdot e_j+{\widehat{x}}_{k-1|k-1},\end{array}$$

  (26)

  where $\left\{s_j^{+}\right\}\triangleq \left\{\sqrt{\frac{1}{2}}(e_k+e_l):k<l,k,l=1,2,\dots ,n\right\}$ and $\left\{s_j^{-}\right\}\triangleq \left\{\sqrt{\frac{1}{2}}(e_k-e_l):k<l,k,l=1,2,\dots ,n\right\}$.
• Compute the predicted mean ${\widehat{x}}_{k|k-1}$ and predicted covariance $P_{k|k-1}$:

  $$\begin{array}{l}{\widehat{x}}_{k|k-1}=w_{1}f(x_1)+w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(f(x_2)+f(x_3)\right)}\\ +w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(f(x_4)+f(x_5)\right)}+w_3{\displaystyle \sum _{j=1}^n\left(f(x_6)+f(x_7)\right),}\end{array}$$

  (27)

  $$\begin{array}{l}{P}_{k|k-1}=w_{1}f(x_1)f{(x_1)}^T+w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(f(x_2)f{(x_2)}^T+f(x_3)f{(x_3)}^T\right)}\\ +w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(f(x_4)f{(x_4)}^T+f(x_5)f{(x_5)}^T\right)}+w_3{\displaystyle \sum _{j=1}^n\left(f(x_6)f{(x_6)}^T+f(x_7)f{(x_7)}^T\right)}\\ -{\widehat{x}}_{k|k-1}\cdot {{\widehat{x}}_{k|k-1}}^T+Q_{k-1},\end{array}$$

  (28)

  where $w_1=\frac{2}{n+2}$, $w_2=\frac{1}{{(n+2)}^2}$, $w_3=\frac{4-n}{2{(n+2)}^2}$ and $Q_{k-1}$ is system noise covariance matrix.

Update phase:

4.

Decompose $P_{k|k-1}$ with Cholesky’s method:

$$P_{k|k-1}=S_{k|k-1}{S_{k|k-1}}^T.$$

(29)

5.

Compute cubature points:

$$\{\begin{array}{l}{x}_1^{\prime }=S_{k|k-1}\cdot 0+{\widehat{x}}_{k|k-1},\\ x_2^{\prime }=S_{k|k-1}\sqrt{n+2}\cdot s_j^{+}+{\widehat{x}}_{k|k-1},\\ x_3^{\prime }=-S_{k|k-1}\sqrt{n+2}\cdot s_j^{+}+{\widehat{x}}_{k|k-1},\\ x_4^{\prime }=S_{k|k-1}\sqrt{n+2}\cdot s_j^{-}+{\widehat{x}}_{k|k-1},\\ x_5^{\prime }=-S_{k|k-1}\sqrt{n+2}\cdot s_j^{-}+{\widehat{x}}_{k|k-1},\\ x_6^{\prime }=S_{k|k-1}\sqrt{n+2}\cdot e_j+{\widehat{x}}_{k|k-1},\\ x_7^{\prime }=-S_{k|k-1}\sqrt{n+2}\cdot e_j+{\widehat{x}}_{k|k-1}.\end{array}$$

(30)

6.

Compute the estimated measurement ${\widehat{z}}_{k|k-1}$, the gain matrix $K_k$ and the covariance matrixes of $P_{k|k-1}^{zz}$, $P_{k|k-1}^{xz}$:

$$\begin{array}{l}{\widehat{z}}_{k|k-1}=w_{1}h(x_1^{\prime })+w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(h(x_2^{\prime })+h(x_3^{\prime })\right)}\\ +w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(h(x_4^{\prime })+h(x_5^{\prime })\right)}+w_3{\displaystyle \sum _{j=1}^n\left(h(x_6^{\prime })+h(x_7^{\prime })\right)},\end{array}$$

(31)

$$\begin{array}{l}{P}_{k|k-1}^{zz}=w_{1}h(x_1^{\prime })h{(x_1^{\prime })}^T+w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(h(x_2^{\prime })h{(x_2^{\prime })}^T+h(x_3^{\prime })h{(x_3^{\prime })}^T\right)}\\ +w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(h(x_4^{\prime })h{(x_4^{\prime })}^T+h(x_5^{\prime })h{(x_5^{\prime })}^T\right)}+w_3{\displaystyle \sum _{j=1}^n\left(h(x_6^{\prime })h{(x_6^{\prime })}^T+h(x_7^{\prime })h{(x_7^{\prime })}^T\right)}\\ -{\widehat{z}}_{k|k-1}\cdot {{\widehat{z}}_{k|k-1}}^T+R_k,\end{array}$$

(32)

$$\begin{array}{l}{P}_{k|k-1}^{xz}=w_1{x}_1^{\prime }h{(x_1^{\prime })}^T+w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(x_2^{\prime }h{(x_2^{\prime })}^T+x_3^{\prime }h{(x_3^{\prime })}^T\right)}\\ +w_2{\displaystyle \sum _{j=1}^{n(n-1)/2}\left(x_4^{\prime }h{(x_4^{\prime })}^T+x_5^{\prime }h{(x_5^{\prime })}^T\right)}+w_3{\displaystyle \sum _{j=1}^n\left(x_6^{\prime }h{(x_6^{\prime })}^T+x_7^{\prime }h{(x_7^{\prime })}^T\right)}\\ -{\widehat{x}}_{k|k-1}\cdot {{\widehat{z}}_{k|k-1}}^T,\end{array}$$

(33)

$$K_k=P_{k|k-1}^{xz}{P_{k|k-1}^{zz}}^{-1}.$$

(34)

7.

Update ${\widehat{x}}_{k|k}$ and $P_{k|k}$:

$$P_{k|k}=P_{k|k-1}-K_k{P}_{k|k-1}^{zz}{K_k}^T,$$

(35)

$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k(z_k-{\widehat{z}}_{k|k-1}),$$

(36)

where $z_k$ is the measurement at time $k$.

3.2. Improved CKF5 Algorithm

The filtering precision of CKF5 can reach the fourth order, and the accuracy of CKF3 can reach the second order. The amount of sigma points of CKF5 is $2n^2+1$, and that of CKF3 is $2n$, so the cost of computing of CKF3 is less than that of CKF5. In order to estimate the numerical stability of nonlinear filter, the stability factor can be defined as [23]:

$$I={\displaystyle \sum _{i=1}^N\left|{\omega }_i\right|},$$

(37)

where ${\omega }_i$ is the weight of sigma point and $N$ is the amount of sigma points. If $I=1$, the numerical integration has absolute stability theoretically, on the contrary, non-absolute stability for $I\ne 1$. When $I\ne 1$, the cumulative truncation error may leads to a non-positive error covariance matrix, even further to the error of calculation or divergence. For CKF3, $I=1$ and for CKF5, $I\ne 1$, which means that CKF3 has better numerical stability than CKF5. For CKF5, we have:

$$\begin{array}{ll}\hfill I& =\left|\frac{2}{n+2}\right|+\left|\frac{n(n-1)}{{(n+2)}^2}\right|+\left|\frac{n(4-n)}{2{(n+2)}^2}\right|\hfill \\ \hfill & =\frac{2}{n+2}+\frac{n(n-1)}{{(n+2)}^2}+\frac{n(n-4)}{2{(n+2)}^2},n>4,\hfill \\ \hfill & =\frac{3n^2-2n+8}{2{(n+2)}^2},n>4.\hfill \end{array}$$

(38)

According to Equation (14), $n=6$ and $I=13/16$, the robustness of CKF is poor in this situation. The non-positive covariance matrix caused by strong nonlinear or data errors may bring many problems in Kalman filters, such as filtering error or divergence, which also can lead to the error of Cholesky decomposition in CKF. In order to improve the numerical stability of CKF5, and considering that the singular value decomposition (SVD) has better numerical robustness than Cholesky [24], SVD is introduced into CKF5.

For s linear optimal Kalman filter, when the gain matrix $K_k$ is optimal, the innovation sequence ${\mathit{\mathbb{Z}}}_k$ should be orthogonal. This conclusion can be extended to nonlinear filtering. The adaptive Kalman (hereinafter referred to as AKF) scales the error covariance matrix or system noise matrix by calculating the ratio of matrix ranks of actual innovation and filtered innovation [17,18]. Essentially, the traditional AKF only partly scale the error covariance matrix in the next filtering period, and the utilization rate of innovation is low. For the innovation sequence, the contribution of the new innovation to the state estimation is large than the old one. Therefore, ICKF5 uses the exponent fading factor to weigh the innovation sequence, and then scale the entire error covariance matrix in the next filtering period. Assuming the current filter period is $k$ and the weight set of innovation sequence is $\left\{{\alpha }_i\right\}$, we have:

$${\displaystyle \sum _{i=1}^k{\alpha }_i}=1,{\alpha }_{i-1}={\alpha }_i\cdot \xi ,$$

(39)

where $\xi$ is fading factor and $\xi \in (0,1)$, empirically, $\xi \in (0.6,1)$. To illustrate the impact of $\xi$ on ${\alpha }_i$, $\xi$ was taken as 0.7, 0.8, and 0.9, respectively, for the convenience of observation, the innovations are weighted by the exponent fading factor from $k=100$. The distribution of ${\alpha }_i$ is shown in Figure 1.

Figure 1 shows that if the $\xi$ is big, the weight of the current innovation is small, correspondingly, the weights of old innovations are big, which means that the old innovation has bigger influence than the new innovation on calculating the covariance matrix of innovation sequence. The opposite conclusion can be obtained if $\xi$ is small. A recursive formula for the covariance matrix of the innovation sequence ${\mathit{\mathbb{Z}}}_k$ is given below. When the forgetting factor $\xi$ is determined, then:

$${\rho }_k={\displaystyle \sum _{i=1}^{k-1}{\xi }^i}.$$

(40)

It follows from Equations (39) and (40) that:

$${\alpha }_i=\frac{{\xi }^{k-i}}{{\rho }_k},i=1,2,\dots ,k.$$

(41)

The open window method is used in traditional AKF, the covariance matrix ${\widehat{P}}_{\mathsf{\mathbb{Z}},k}$ of innovation ${\mathsf{\mathbb{Z}}}_k$ at time $k$ can be expressed as:

$${\widehat{P}}_{\mathsf{\mathbb{Z}},k}=\frac{1}{k-1}{\displaystyle \sum _{i=1}^k{\mathsf{\mathbb{Z}}}_i}{\mathsf{\mathbb{Z}}}_i^T.$$

(42)

The weight of $1/(k-1)$ in Equation (42) can be replaced by the weights of ${\alpha }_i$ in Equation (41), then a recursive formula between ${\widehat{P}}_{\mathsf{\mathbb{Z}},k}$ and ${\widehat{P}}_{\mathsf{\mathbb{Z}},k-1}$ can be expressed as:

$$\begin{array}{cc}\hfill {\widehat{P}}_{\mathsf{\mathbb{Z}},k}& ={\displaystyle \sum _{i=1}^k\frac{{\xi }^{k-i}}{{\rho }_k}{Z}_i}{Z}_i^T\hfill \\ \hfill & =\frac{Z_k{Z}_k^T}{{\rho }_k}+\frac{{\displaystyle \sum _{i=1}^{k-1}{\xi }^{k-i}{Z}_i}{Z}_i^T}{{\rho }_k}\hfill \\ \hfill & =\frac{Z_k{Z}_k^T}{{\rho }_k}+\frac{\xi {\rho }_{k-1}}{{\rho }_k{\rho }_{k-1}}{\displaystyle \sum _{i=1}^{k-1}{\xi }^{k-i-1}{Z}_i}{Z}_i^T\hfill \\ \hfill & =\frac{Z_k{Z}_k^T}{{\rho }_k}+\frac{\xi {\rho }_{k-1}}{{\rho }_k{\rho }_{k-1}}{\displaystyle \sum _{i=1}^{k-1}{\xi }^{k-i-1}{Z}_i}{Z}_i^T\hfill \\ \hfill & =\frac{Z_k{Z}_k^T}{{\rho }_k}+\frac{{\rho }_k-1}{{\rho }_k}\left(\frac{1}{{\rho }_{k-1}}{\displaystyle \sum _{i=1}^{k-1}{\xi }^{k-i-1}{Z}_i}{Z}_i^T\right)\hfill \\ \hfill & =\frac{Z_k{Z}_k^T}{{\rho }_k}+\frac{{\rho }_k-1}{{\rho }_k}{\widehat{P}}_{\mathsf{\mathbb{Z}},k-1}.\hfill \end{array}$$

(43)

In order to improve the feedback effect of innovation, we scale the error covariance matrix of the next filtering period ${\beta }_k$ times entirely. The increase of the error covariance matrix makes the filter gain $K_k$ increase and the error correction ability of the innovation in the next filtering period is strengthened. This method can reduce the dependence of the filter on the model, and then improve the filtering accuracy and reduce the risk of divergence. ${\beta }_k$ is defined as:

$${\beta }_k=Max\left\{\frac{trace({\widehat{P}}_{\mathsf{\mathbb{Z}},k}-R_k)}{trace(H_k{P}_{k|k-1}{H_k}^T)},1\right\},$$

(44)

where $trace(·)$ is the operator of obtaining the rank of a matrix, $R_k$ is the measurement error covariance matrix at time $k$, $H_k$ is the measurement matrix at time $k$. The error covariance matrix of next filtering period is scaled as:

$$P_{k+1|k}={\beta }_k\cdot P_{k+1|k}.$$

(45)

The algorithm of ICKF5 is summarized as follows:

Prediction phase:

(1)

Input the initial values for filtering: $x_{k-1|k-1},P_{k-1|k-1},Q_{k-1},{\beta }_{k-1}=1,R_k$.

(2)

Compute cubature points $x_i,(i=1,\dots ,7)$ according to Equation (26).

(3)

Compute the predicted mean ${\widehat{x}}_{k|k-1}$ and predicted covariance $P_{k|k-1}$ according to Equations (27) and (28), respectively.

(4)

Scaling the entire $P_{k|k-1}$: $P_{k|k-1}={\beta }_{k-1}\cdot P_{k|k-1}$.

Update phase:

(5)

Decompose $P_{k|k-1}$ with the SVD method.

(6)

Compute cubature points $x_i^{\prime },(i=1,\dots ,7)$ according to Equation (30).

(7)

Compute the estimated measurement ${\widehat{z}}_{k|k-1}$, the gain matrix $K_k$ and the covariance matrixes of $P_{k|k-1}^{zz}$, $P_{k|k-1}^{xz}$ according to Equations (31)–(34), respectively.

(8)

Update ${\widehat{x}}_{k|k}$, $P_{k|k}$ and ${\beta }_k$ according to Equations (35), (36) and (44), respectively.

The scaling of the error covariance matrix by ICKF5 can effectively improve the utilization of innovation and speed up the convergence of initial alignment. ICKF5 is a kind of suboptimal algorithm, if the scaling value of ${\beta }_k$ is too large, the system will be in a state of overshoot, which may lead to oscillation or divergence. For example, when getting the initial alignment in the convergence stage, the gain $K_k$ and the error covariance matrix $P_{k|k-1}$ should ideally remain unchanged or little changed. If the feedback for error covariance matrix is big at the stage of alignment convergence, it may cause the filtering convergence to be unstable or lead to the divergence. In order to reduce the possibility of divergence of initial alignment filtering and ensure the stationarity after convergence, we use a short-term feedback strategy that we stop the scaling of ${\beta }_k$ in ICKF5 before the filtering occurs oscillating or diverging, which can ensure the convergence of initial alignment. For convenience, we call the short-term feedback algorithm IICKF5. The first step of IICKF5 is that the ICKF5 is used to complete the initial alignment firstly, then the gradient sequence $\nabla {\psi }_i=\mathrm{diff}\left({\psi }_i\right)$ of the set of azimuth misalignment angle $\left\{{\psi }_i\right\}$ outputted by ICKF5 can be obtained, where $diff(·)$ is gradient operator. The second step, the time ${\mathrm{T}}_{\max }$ corresponding to the maximum value of the gradient is chosen as the time to terminate the feedback to the error covariance matrix $P_{k|k-1}$. ${\mathrm{T}}_{\max }$ can be expressed as:

$${\mathrm{T}}_{\max }=Max\left\{abs\left\{\nabla \psi \right\}\right\}.$$

(46)

To be brief, ICKF5 is used in the period of $[1,{\mathrm{T}}_{\max })$ and CKF5 is used in $[{\mathrm{T}}_{\max },{\mathrm{T}}_{end}]$, where ${\mathrm{T}}_{end}$ is the end time of initial alignment filtering. The flow chart of IICKF5 is shown in Figure 2.

Compared to ICKF5, IICKF5 needs to recalculate the stored IMU data and the misalignment angle set {x_k} output by ICKF5. IICKF5 has a certain delay compared with ICKF5, and some storage space is needed.

3.3. Simulation and Its Analysis

In order to verify the effectiveness of ICKF5 and IICKF5, a simulation of initial alignment under static base conditions is performed. The simulation conditions are set as follows: the gyro constant drift is ${\epsilon }_C=0.01{(}^{\circ }/h)$, random drift is ${\epsilon }_N=0.001{(}^{\circ }/\sqrt{h})$; the accelerometer zero bias is ${\nabla }_C=50\mu g$, random deviation is ${\nabla }_N=5(\mu g/\sqrt{h})$; the local geographic latitude is $34°$, longitude is $108°$; the misalignment angles are set as $\varphi (0)=[20°,-40°,170°]$ and the simulation period is $800s$. The general parameters of the initial alignment algorithms are set with the same values. The initial value of $X_0$ is set as $X_0=[0;0;0;0;0;0]$. The matrix of $Q$ is determined according to the random drift of IMU, and $Q_k=diag{\{({0.001}^{\circ });({0.001}^{\circ });({0.001}^{\circ });(5ug);(5ug);(5ug)\}}^2$. The initial error covariance matrix is set as $P_0=diag{([[10;10;180]\cdot \pi /180;[1;1;1]])}^2$. Where $diag(·)$ is the operator of obtaining the diagonal matrix with diagonal elements. For UKF, $\alpha =1e-3$, $\beta =2$ and $\lambda =0$. EKF, UKF [25], CKF3, CKF5, ICKF5 and IICKF5 algorithms are applied to the nonlinear initial alignment. In addition to IICKF5, the calculated misalignment angles are as shown in Figure 3.

Figure 3 shows that the pitching misalignment angles converge to 20° and the rolling misalignment angles converge to −40° with the exception of EKF, and only the azimuth misalignment angles of CKF5 and ICKF5 converge to 170°.

The gradient values of the azimuth angle errors of ICKF5 are shown in Figure 4.

Figure 4 shows that there is an obvious mutation in the azimuth angle error at $t=4s$, which is the overshoot caused by ICKF5, so ${\mathrm{T}}_{\max }=4s.$ The estimated azimuth angles of CKF5, ICKF5 and IICKF5 are shown in Figure 5.

As seen in Figure 5, IICKF5 enters the convergence stage at about 400 s, while CKF5 and ICKF5 converge slower than IICKF5. Figure 3 also shows that the convergences of horizontal misalignment angles are faster than that of the azimuth misalignment angles, which is due to the fact that the observability of the horizontal misalignment angles is high under the velocity matching mode. There is a coupling relationship between the horizontal misalignment angles and the bias of accelerometer. When the variation of heading angle is small, the accuracies of the horizontal misalignment angles depend on the bias of the accelerometer. The simulation experiment in this paper is based on static base, so the variation of heading angle is very small, and the horizontal misalignment has a slow drift due to the bias of the accelerometer. From this point of view, the estimation period of the horizontal misalignment angles is not the longer the better. For the sake of accuracy, we select the estimations of horizontal misalignment angles at 50 s and select the estimation value of azimuth misalignment angle at the last moment (800 s) as the outputs of alignment, and the alignment errors are shown in

Table 1. Comparison of alignment errors (°).

	EKF	UKF	CKF3	CKF5	ICKF5	IICKF5
error of pitch (°)	−7.0205	0.0899	0.1270	0.1590	0.0829	0.0291
error of roll (°)	7.8234	−0.1464	−0.1634	−0.2078	−0.2949	−0.2738
error of heading (°)	62.7783	157.7361	61.2902	13.6403	−1.2458	−0.0878

.

Table 1 shows that the pitch and the azimuth misalignment errors of IICKF5 are the smallest, and the azimuth misalignment error is close to the limitd alignment precision of 0.05°, which meets the initial alignment requirement.

4. Dynamic Vehicle Experiment

The experimental equipment is shown in Figure 6. The GNSS real-time differential system consisted of two model ProPak6 receivers (NovAtel, Calgary, AB, Canada). The attitude reference system is a NovAtel SPAN system (NovAtel, Calgary, AB, Canada), which can reach centimeter level precision. An LN-200 IMU named from the Novatel Company is used for the initial alignment experiment. The bias of the accelerometer is 0.3 mg and its scale factor is 300 ppm. The constant drift, the random drift and the scale factor of gyro are ${\epsilon }_C=1{(}^{\circ }/h)$, ${\epsilon }_N=0.07{(}^{\circ }/\sqrt{h})$ and 100 ppm, respectively. The output rates of the LN-200, SPAN system and GNSS system are 200 Hz, 100 Hz and 5 Hz, respectively.

The period of initial alignment is 700 s. In addition to IICKF5, the calculated azimuth angle errors are as shown in Figure 7.

For the azimuth angle errors, Figure 7 shows that EKF, UKF, CKF3 did not converge to the accurate azimuth angle, ICKF5 has faster convergence rate than CKF5, and ICKF5 has much more oscillations in the convergence process than CKF5 because of the overshoot. For the pitch angle and the roll angle errors, we can see that CKF5 and ICKF5 have greater oscillations than other algorithms, which is caused by the inaccuracy of model noises, such as the GPS’s measurement noise, IMU noise and so on. The nonlinear filter of high order precision amplifies the noises with high frequencies, which easily results in larger estimation errors if the estimated state values are small. When obtaining the gradient of the azimuth angles of ICKF5 in Figure 7c, the gradient values are as shown in Figure 8.

Figure 8 shows that there is a mutation in the azimuth angle error at $t=50s$, so ${\mathrm{T}}_{\max }=50s.$ The estimated misalignment angles of all the algorithms are shown in Figure 9.

From Figure 9 we can find that IICKF5 has a faster convergence rate than CKF5 and ICKF5 except for the pitching misalignment angle, and the azimuth misalignment angle of IICKF5 has better convergence stability than ICKF5. The alignment errors at the end of alignment (at 700 s) are shown in

Table 2. Comparison of alignment errors with in-motion base (°).

	EKF	UKF	CKF3	CKF5	ICKF5	IICKF5
error of pitch (°)	−3.2814	−1.2868	−2.6771	−3.7189	3.0270	5.9149
error of roll (°)	−1.2960	−0.2867	3.3751	5.4245	−2.9030	1.8357
error of heading (°)	−139.5622	−266.3212	−204.3457	7.9986	42.5004	−1.9540

.

Table 2 shows that IICKF5 has better accuravy than CKF5 in estimating the rolling misalignment angle and azimuth misalignment angle. When the alignment is finished, the alignment errors of ICKF5 and IICKF5 did not converge to small values (e.g., 1°), so more time is needed for fine alignment. In order to further compare the convergence abilities of different algorithms, the mean and standard deviation of azimuth misalignment errors are calculated in the range of [100 s, 700 s], which can be regarded as a relatively complete convergence period for ICKF5 and IICKF5, and the results are shown in

Table 3. Mean and Standard Deviation (SD) of azimuth misalignment errors (°).

	EKF	UKF	CKF3	CKF5	ICKF5	IICKF5
Mean (°)	−155.2618	−264.1352	−201.1962	1.6920	−6.1625	−0.6852
SD (°)	38.8935	3.8062	11.5359	10.2307	41.2056	4.6229

.

From Table 3 we can see that IICKF5 has the smallest errors of mean and standard deviation. In this sense, the improved algorithm is suitable for quickly identifying a large azimuth misalignment angle in a complex alignment environment. ICKF5 is a kind of real-time algorithm that represents an improvement on CKF5, which does not need to process historical IMU data and no storage space for the IMU is needed. IICKF5 needs to obtain the gradient value of the historical azimuth angle, so the historical IMU data need to be stored for the second initial alignment filtering. For the 700 s period of in-motion initial alignment, the length of IMU data is 70,000 (in order to synchronize with the reference system), and the storage space for IMU data is 2869 KB. The data stored are double and floating types with 64 bits. The update rate of GNSS is 5 Hz, so the storage space it needs is small, and the size is 10 KB. The capacity of the azimuth misalignment angle data is 6 KB. The total storage space needed for IICKF5 is 2885 KB, and the additional burden on the navigation computer is small.

5. Conclusions

It is not only convenient to calculate the innovation variance matrix in a recursive and exponent fading factor way, but also to effectively use the current and old updates. The speed and precision of convergence can be effectively improved by scaling error covariance matrix with the ratio of matrix ranks of actual updates and filter updates. In order to solve the problem of oscillation or divergence caused by excessive scaling of the error covariance matrix, the scaling scheme is terminated when the azimuth gradient reaches the maximum. The improved CKF can improve the precision of initial alignment better than the traditional CKF. The length of filtering time is also dependent on the experience, and the robustness of the improved algorithm is also worthy of further study.