An Improved SINS Alignment Method Based on Adaptive Cubature Kalman Filter

Abstract

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.

1. 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 process [15]. Therefore, nonlinear filtering algorithm based initial alignment, such as the extended Kalman filter (EKF) [16] and unscented Kalman filter (UKF) [17], has been subsequently developed for decades, to solve the problem of moving state initial alignment with large initial misalignment angles directly. EKF is widely used to solve the nonlinear estimation problem. However, it has low accuracy for a high-dimensional system and a high calculation cost of the Jacobian matrix. UKF was proposed by Julier and Uhlmann in [18]. Based on unscented transformation (UT), UKF can capture the posterior mean and covariance as the second order of the Taylor series of any nonlinearity. Its extensions include marginalized UKF [19] and high order UKF [20]. The cubature Kalman filter (CKF), which is based on the third degree spherical–radial cubature rule, was proposed in [21] and has been used in initial alignment [14]. CKF not only guarantees the accuracy, but also has better stability in a high-dimensional system.

However, the performance of CKF heavily depends on precise prior knowledge of noise. When the carrier is in the motion state, due to the influence of vibration and severe maneuvers, it is difficult to determine the accurate noise covariance value of sensors [22,23,24]. Inaccurate or time varying noise statistical information will degrade the accuracy of initial alignment dramatically. Therefore, there is a great demand to use the adaptive Kalman filter to solve this problem. In [25], an adaptive Kalman filter based on the expectation maximization (EM) method was proposed. However, it met the restriction of needed large windows of data to guarantee reliable estimations. Therefore, it was not suitable in a practical experiment. In [26], the Sage–Husa adaptive Kalman filter based on the maximum a posteriori criterion was used to estimate the statistical properties of noises. Through the introduction of a fading parameter to adjust the filter parameter, it could theoretically estimate the state and measurement noise covariance matrices simultaneously. However, the stability of this filter was critically influenced by the non-positive definiteness of noise covariance matrices, and the capacity of the fading adaptive adjustment factor was limited. Due to the flaws inherent in these adaptive estimation methods, it is urged to adopt the advanced adaptive Kalman filter to meet the requirement. The variational Bayesian (VB) based adaptive Kalman filter was proposed in [27] to further improve the estimation accuracy of unknown noise covariance and has been used in target tracking and cooperative navigation. However, it is not suitable for the initial alignment when the state equation is nonlinear.

Concerning the alignment problem with large misalignment angles and uncertain noise covariance matrices, this paper proposes an adaptive CKF (ACKF) based on the VB method to further improve the accuracy of initial alignment. In this work, the one step predicted covariance matrix and measurement noise covariance matrix are modeled by the inverse Wishart (IW) distribution. The VB method based CKF is then used to approximate the joint posterior probability density function (PDF) of the state, one step predicted error covariance matrix, and measurement noise covariance matrix. The proposed method and existing adaptive filter method are tested based on the vehicle experiment of SINS aided by GPS. Experimental results show that proposed method has better performance than the well known methodologies when the carrier has a severe maneuver.

The outline of this paper is shown as follows. The mathematical model of initial alignment is given in Section 2. In Section 3, the ACKF algorithm is designed for the application of initial alignment. The simulation and vehicle experiment are conducted to verify the effectiveness of the proposed ACKF in Section 4. The conclusions are drawn in Section 5.

2. Nonlinear Model of Initial Alignment

Firstly, the definitions of the frame are given in this section. The inertial non-rotating frame is denoted by i. The Earth-fixed frame is denoted by e. The geographic frame is denoted by t. The body frame of SINS is denoted as b, which is the right-forward-up (R-F-U) orientation. n and $n^{\prime }$ represent the navigation 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:

$$\left\{\begin{array}{c}\dot{\mathit{\varphi }}=C_{\varphi }^{-1}\left[(\mathit{I}-{\mathit{C}}_n^{n^{\prime }}){\tilde{\mathit{\omega }}}_{in}^n+{\mathit{C}}_n^{n^{\prime }}\delta {\mathit{\omega }}_{in}^n-{\mathit{C}}_b^{n^{\prime }}({\mathit{\epsilon }}^b+{\mathit{\eta }}_g^b)\right]\hfill \\ \delta {\dot{\mathit{v}}}^n=\left[\mathit{I}-{\left({\mathit{C}}_n^{n^{\prime }}\right)}^T\right]{\mathit{C}}_b^{n^{\prime }}{\tilde{\mathit{f}}}_{ib}^b-(2{\tilde{\mathit{\omega }}}_{ie}^n+{\tilde{\mathit{\omega }}}_{en}^n)\times \delta {\mathit{v}}^n\hfill \\ -(2\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{en}^n)\times ({\tilde{\mathit{v}}}^n-\delta {\mathit{v}}^n)+{\left({\mathit{C}}_n^{n^{\prime }}\right)}^T{\mathit{C}}_b^{n^{\prime }}({\nabla }^b+{\mathit{\eta }}_a^b)\hfill \\ \delta \dot{L}=\frac{\delta v_N^n}{R_e}\hfill \\ \delta \dot{\phi }=\frac{sec\tilde{L}}{R_e}\delta v_E^n+\frac{{\tilde{v}}_E^{n}sec\tilde{L}tan\tilde{L}}{R_e}\delta L\hfill \\ {\dot{\mathit{\epsilon }}}^b=0,{\dot{\nabla }}^b=0\hfill \end{array}\right.$$

(1)

with:

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

(2)

where the misalignment angles $\mathit{\varphi }=[{\varphi }_x;{\varphi }_y;{\varphi }_z]$ are the attitude error angle between frame n and frame $n^{\prime }$. $\delta {\mathit{v}}^n=[\delta v_E^n;\delta v_N^n]$ is the error of calculated velocity ${\tilde{\mathit{v}}}^n$. $\delta L$ and $\delta \phi$ are the errors of calculated latitude $\tilde{L}$ and longitude $\tilde{\phi }$. ${\mathit{\epsilon }}^b$ is the gyro constant drift. ${\mathit{\eta }}_g^b$ is the gyroscope random noise. ${\nabla }^b$ is the accelerometer bias. ${\mathit{\eta }}_a^b$ is the accelerometer random noise. $R_e$ is the radius of the Earth. $(·)\times$ denotes the $3\times 3$ skew symmetric matrix. ${[·]}^T$ denotes the transposition of the matrix. The attitude error matrix ${\mathit{C}}_n^{n^{\prime }}$ is formulated as follows:

$${\mathit{C}}_n^{n^{\prime }}=\left[\begin{array}{ccc}cos{\varphi }_y& 0& -sin{\varphi }_y\\ 0& 1& 0\\ sin{\varphi }_y& 0& cos{\varphi }_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos{\varphi }_x& sin{\varphi }_x\\ 0& -sin{\varphi }_x& cos{\varphi }_x\end{array}\right]\left[\begin{array}{ccc}cos{\varphi }_z& sin{\varphi }_z& 0\\ -sin{\varphi }_z& cos{\varphi }_z& 0\\ 0& 0& 1\end{array}\right]$$

(3)

The erroneous ${\tilde{\mathit{\omega }}}_{ie}^n$ and ${\tilde{\mathit{\omega }}}_{en}^n$ are given as follows:

$${\tilde{\mathit{\omega }}}_{ie}^n=\left[\begin{array}{c}0;{\omega }_{ie}cos\tilde{L};{\omega }_{ie}sin\tilde{L}\end{array}\right]$$

(4)

$${\tilde{\mathit{\omega }}}_{en}^n=\left[\begin{array}{c}-\frac{{\tilde{v}}_N^n}{R_e};\frac{{\tilde{v}}_E^n}{R_e};\frac{{\tilde{v}}_E^n}{R_e}tan\tilde{L}\end{array}\right]$$

(5)

where ${\omega }_{ie}$ is the Earth’s rotation angular velocity. The errors of ${\tilde{\mathit{\omega }}}_{ie}^n$ and ${\tilde{\mathit{\omega }}}_{en}^n$ are given as follows:

$$\delta {\mathit{\omega }}_{ie}^n=\left[\begin{array}{c}0;-{\omega }_{ie}sin\tilde{L}\delta L;{\omega }_{ie}cos\tilde{L}\delta L\end{array}\right]$$

(6)

$$\delta {\mathit{\omega }}_{en}^n=\left[\begin{array}{c}-\frac{\delta v_N^n}{R_e};\frac{\delta v_E^n}{R_e};\frac{\delta v_E^{n}tanL+{\tilde{v}}_E^n{sec}^{2}L\delta L}{R_e}\end{array}\right]$$

(7)

Therefore, ${\tilde{\mathit{\omega }}}_{in}^n$ and $\delta {\mathit{\omega }}_{in}^n$ are given as follows:

$${\tilde{\mathit{\omega }}}_{in}^n={\tilde{\mathit{\omega }}}_{ie}^n+{\tilde{\mathit{\omega }}}_{en}^n$$

(8)

$$\delta {\mathit{\omega }}_{in}^n=\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{en}^n$$

(9)

The velocity and position differences between SINS and aiding equipment such as GPS are selected as the measurement, which is formulated as:

$$\left\{\begin{array}{c}{\mathit{z}}_v=\left[\begin{array}{c}\delta {\mathit{v}}_E^n;\delta {\mathit{v}}_N^n\end{array}\right]+{\mathit{\nu }}_v\hfill \\ {\mathit{z}}_P=\left[\begin{array}{c}\delta L;\delta \lambda \end{array}\right]+{\mathit{\nu }}_P\hfill \end{array}\right.$$

(10)

where ${\mathit{\nu }}_v$ and ${\mathit{\nu }}_P$ are velocity measurement noise and position measurement noise, respectively.

The state vector is defined as $\mathit{x}=[{\varphi }_x;{\varphi }_y;{\varphi }_z;\delta v_E^n;\delta v_N^n;$$\delta L;\delta \lambda ;{\epsilon }_x^s;{\epsilon }_y^s;{\epsilon }_z^s;{\nabla }_x^s;{\nabla }_y^s]$. The state noise vector is defined as $\mathit{w}=[{\mathit{\eta }}_g^s;{\mathit{\eta }}_a^s;0_{7\times 1}]$. The measurement vector and noise vector are defined as $\mathit{z}=[{\mathit{z}}_v;{\mathit{z}}_P]$ and $\mathit{\nu }=[{\mathit{\nu }}_v;{\mathit{\nu }}_P]$, respectively. By discretizing the continuous equations, the standard discrete state error equation and measurement equation are formulated as:

$$\left\{\begin{array}{c}{\mathit{x}}_k=\mathit{f}\left({\mathit{x}}_{k-1}\right)+\mathit{g}\left({\mathit{x}}_{k-1}\right){\mathit{w}}_{k-1}\hfill \\ {\mathit{z}}_k=\mathit{h}\left({\mathit{x}}_k\right)+{\mathit{\nu }}_k\hfill \end{array}\right.$$

(11)

where $\mathit{f}(·)$ and $\mathit{g}(·)$ are the nonlinear functions, which are formulated based on (1), and the measurement model is a linear function,

$$\mathit{h}\left({\mathit{x}}_k\right)=\mathit{H}{\mathit{x}}_k=[{\mathbf{0}}_{4\times 3},{\mathit{I}}_{4\times 4},{\mathbf{0}}_{4\times 5}]{\mathit{x}}_k$$

(12)

${\mathit{w}}_k$ and ${\mathit{\nu }}_k$ are uncorrelated Gaussian white noises with mean value $\mathbf{0}$ and covariance ${\mathit{Q}}_k$ and ${\mathit{R}}_k$, respectively. For the sake of simplification in the next sections, we assume that the dimensions of ${\mathit{x}}_k$ and ${\mathit{w}}_k$ are $n_x$ and the dimensions of ${\mathit{z}}_k$ and ${\mathit{\nu }}_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.

3. 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.

3.1. 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.,

$$p\left({\mathit{x}}_k|{\mathit{z}}_{1:k-1}\right)=\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})$$

(13)

where ${\widehat{\mathit{x}}}_{k|k-1}$ and ${\mathit{P}}_{k|k-1}$ denote the mean and variance of $p\left({\mathit{x}}_k|{\mathit{z}}_{1:k-1}\right)$.

$$p\left({\mathit{z}}_k|{\mathit{z}}_{1:k-1}\right)=\mathrm{N}({\mathit{z}}_k;{\widehat{\mathit{z}}}_{k|k-1},{\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}})$$

(14)

where ${\widehat{\mathit{z}}}_{k|k-1}$ and ${\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}}$ denote the mean and variance of $p\left({\mathit{z}}_k|{\mathit{z}}_{1:k-1}\right)$.

Obviously, the joint one step predicted PDF of the state and measurement $p\left({\mathit{x}}_k,{\mathit{z}}_k|{\mathit{z}}_{1:k-1}\right)$ is also Gaussian, i.e.,

$$p\left({\mathit{x}}_k,{\mathit{z}}_k|{\mathit{z}}_{1:k-1}\right)=\mathrm{N}\left(\left[\begin{array}{c}{\mathit{x}}_k\\ {\mathit{z}}_k\end{array}\right];\left[\begin{array}{c}{\widehat{\mathit{x}}}_{k|k-1}\\ {\widehat{\mathit{z}}}_{k|k-1}\end{array}\right],\left[\begin{array}{cc}{\mathit{P}}_{k|k-1}& {\mathit{P}}_{k|k-1}^{\mathit{x}\mathit{z}}\\ {\left({\mathit{P}}_{k|k-1}^{\mathit{x}\mathit{z}}\right)}^T& {\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}}\end{array}\right]\right)$$

(15)

where ${\mathit{P}}_{k|k-1}^{\mathit{x}\mathit{z}}$ is the covariance of ${\mathit{x}}_k$ and ${\mathit{z}}_k$. Based on (14) and (15), in the Bayesian theorem, the posterior PDF of ${\mathit{x}}_k$ is also Gaussian, i.e.,

$$p\left({\mathit{x}}_k|{\mathit{z}}_{1:k}\right)=\frac{p\left({\mathit{x}}_k,{\mathit{z}}_k|{\mathit{z}}_{1:k-1}\right)}{p\left({\mathit{z}}_k|{\mathit{z}}_{1:k-1}\right)}=\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k},{\mathit{P}}_{k|k})$$

(16)

where ${\widehat{\mathit{x}}}_{k|k}$ and ${\mathit{P}}_{k|k}$ denote the mean and variance of $p\left({\mathit{x}}_k|{\mathit{z}}_{1:k}\right)$. ${\widehat{\mathit{x}}}_{k|k}$ and ${\mathit{P}}_{k|k}$ are derived as follows:

$${\mathit{K}}_k={\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}}{\left({\mathit{P}}_{k|k-1}^{\mathit{x}\mathit{z}}\right)}^{-1}$$

(17)

$${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k-1}+{\mathit{K}}_k({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1})$$

(18)

$${\mathit{P}}_{k|k}={\mathit{P}}_{k|k-1}-{\mathit{K}}_k{\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}}{\mathit{K}}_k^T$$

(19)

where ${\mathit{K}}_k$ is the filter gain, and the other parameters are calculated as follows:

$${\widehat{\mathit{x}}}_{k|k-1}=\mathrm{E}\left[\mathit{f}\left({\mathit{x}}_{k-1}\right)|{\mathit{z}}_{1:k-1}\right]=\int \mathit{f}\left({\mathit{x}}_{k-1}\right)\mathrm{N}({\mathit{x}}_{k-1};{\widehat{\mathit{x}}}_{k-1|k-1},{\mathit{P}}_{k-1|k-1})\mathrm{d}{\mathit{x}}_{k-1}$$

(20)

$$\begin{array}{cc}\hfill {\mathit{P}}_{k|k-1}=& \mathrm{E}\left[({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}){({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1})}^T|{\mathit{z}}_{1:k-1}\right]\hfill \\ \hfill =& \int (\mathit{f}\left({\mathit{x}}_{k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1}){(\mathit{f}\left({\mathit{x}}_{k-1}\right)-{\widehat{\mathit{x}}}_{k|k-1})}^T\mathrm{N}({\mathit{x}}_{k-1};{\widehat{\mathit{x}}}_{k-1|k-1},{\mathit{P}}_{k-1|k-1})\mathrm{d}{\mathit{x}}_{k-1}\hfill \\ \hfill & +\mathit{g}\left({\widehat{\mathit{x}}}_{k|k-1}\right){\mathit{Q}}_{k-1}\mathit{g}{\left({\widehat{\mathit{x}}}_{k|k-1}\right)}^T\hfill \end{array}$$

(21)

$${\widehat{\mathit{z}}}_{k|k-1}=\mathrm{E}\left[\mathit{h}\left({\mathit{x}}_k\right)|{\mathit{z}}_{1:k-1}\right]=\int \mathit{h}\left({\mathit{x}}_k\right)\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})\mathrm{d}{\mathit{x}}_k$$

(22)

$$\begin{array}{cc}\hfill {\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}}=& \mathrm{E}\left[({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1}){({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1})}^T|{\mathit{z}}_{1:k-1}\right]\hfill \\ \hfill =& \int (\mathit{h}\left({\mathit{x}}_k\right)-{\widehat{\mathit{z}}}_{k|k-1}){(\mathit{h}\left({\mathit{x}}_k\right)-{\widehat{\mathit{z}}}_{k|k-1})}^T\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})\mathrm{d}{\mathit{x}}_k+{\mathit{R}}_k\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill {\mathit{P}}_{k|k-1}^{\mathit{x}\mathit{z}}=& \mathrm{E}\left[({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}){({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1})}^T|{\mathit{z}}_{1:k-1}\right]\hfill \\ \hfill =& \int ({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}){(\mathit{h}\left({\mathit{x}}_k\right)-{\widehat{\mathit{z}}}_{k|k-1})}^T\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})\mathrm{d}{\mathit{x}}_k\hfill \end{array}$$

(24)

where $\mathrm{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 $\mathit{f}(·)$ and $\mathit{h}(·)$, it is difficult to obtain the accurate numerical solution of (20)–(24), and the approximation solution is necessary, i.e.,

$$\int \mathit{f}\left(\mathit{x}\right)\mathrm{N}(\mathit{x};\mathit{a},\mathit{b})\mathrm{d}\mathit{x}\approx \sum _{j=1}^N{W}_j\mathit{f}\left({\mathit{x}}_j\right)$$

(25)

where ${\mathit{x}}_j$ and $W_j$ are the sampling points and corresponding weights of $\mathit{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 ${\mathit{x}}_{k-1}$ are selected based on ${\widehat{\mathit{x}}}_{k-1|k-1}$ and ${\mathit{P}}_{k-1|k-1}$. These cubature points are defined as follows:

$$\left\{\begin{array}{c}{\mathit{\chi }}_{k-1}^{\left(j\right)}={\widehat{\mathit{x}}}_{k-1|k-1}+\sqrt{n_x}{\left(\sqrt{{\mathit{P}}_{k-1|k-1}}\right)}_{\left(j\right)},(j=1,2,\cdots ,n_x)\hfill \\ {\mathit{\chi }}_{k-1}^{\left(j\right)}={\widehat{\mathit{x}}}_{k-1|k-1}-\sqrt{n_x}{\left(\sqrt{{\mathit{P}}_{k-1|k-1}}\right)}_{\left(j\right)},(j=n_x+1,n_x+2,\cdots ,2n_x)\hfill \end{array}\right.$$

(26)

where ${\left(A\right)}_{\left(j\right)}$ denotes the jth column of A. Propagating the cubature points of ${\mathit{x}}_{k-1}$ by $\mathit{f}(·)$, the state one step predicted mean ${\widehat{\mathit{x}}}_{k\mid k-1}$ and covariance ${\mathit{P}}_{k\mid k-1}$ can be obtained as follows based on (20) and (21):

$${\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}=\mathit{f}\left({\mathit{\chi }}_{k-1}^{\left(j\right)}\right),(j=1,2,\cdots ,2n_x)$$

(27)

$${\widehat{\mathit{x}}}_{k\mid k-1}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}{\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}$$

(28)

$${\mathit{P}}_{k\mid k-1}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}\left[({\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1}){({\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1})}^T\right]+\mathit{g}\left({\widehat{\mathit{x}}}_{k|k-1}\right){\mathit{Q}}_{k-1}\mathit{g}{\left({\widehat{\mathit{x}}}_{k|k-1}\right)}^T$$

(29)

Furthermore, the cubature points of ${\mathit{x}}_k$ based on ${\widehat{\mathit{x}}}_{k\mid k-1}$ and ${\mathit{P}}_{k\mid k-1}$ are selected as follows:

$$\left\{\begin{array}{c}{\mathit{\chi }}_{k\mid k-1}^{\left(j\right)}={\widehat{\mathit{x}}}_{k\mid k-1}+\sqrt{n_x}{\left(\sqrt{{\mathit{P}}_{k|k-1}}\right)}_{\left(j\right)},(j=1,2,\cdots ,n_x)\hfill \\ {\mathit{\chi }}_{k\mid k-1}^{\left(j\right)}={\widehat{\mathit{x}}}_{k\mid k-1}-\sqrt{n_x}{\left(\sqrt{{\mathit{P}}_{k|k-1}}\right)}_{\left(j\right)},(j=n_x+1,n_x+2,\cdots ,2n_x)\hfill \end{array}\right.$$

(30)

Propagating the cubature points of ${\mathit{x}}_k$ by $\mathit{h}(·)$, the measurement one step predicted mean and covariance can be obtained as follows:

$${\mathrm{Z}}_{k|k-1}^{\left(j\right)}=\mathit{h}\left({\mathit{\chi }}_{k|k-1}^{\left(j\right)}\right),(j=1,2,\cdots ,2n_x)$$

(31)

$${\widehat{\mathit{z}}}_{k|k-1}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}{\mathrm{Z}}_{k|k-1}^{\left(j\right)}$$

(32)

$${\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}\left[({\mathrm{Z}}_{k|k-1}^{\left(j\right)}-{\widehat{\mathit{z}}}_{k|k-1}){({\mathrm{Z}}_{k\mid k-1}^{\left(j\right)}-{\widehat{\mathit{z}}}_{k\mid k-1})}^T\right]+{\mathit{R}}_k$$

(33)

$${\mathit{P}}_{k\mid k-1}^{\mathit{x}\mathit{z}}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}\left[({\mathit{\chi }}_{k\mid k-1}^{\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1}){({\mathrm{Z}}_{k\mid k-1}^{\left(j\right)}-{\widehat{\mathit{z}}}_{k\mid k-1})}^T\right]$$

(34)

Filter gain ${\mathit{K}}_k$ and measurement update are given as follows:

$${\mathit{K}}_k={\mathit{P}}_{k\mid k-1}^{\mathit{x}\mathit{z}}{\left({\mathit{P}}_{k\mid k-1}^{\mathit{z}\mathit{z}}\right)}^{-1}$$

(35)

$${\widehat{\mathit{x}}}_{k\mid k}={\widehat{\mathit{x}}}_{k\mid k-1}+{\mathit{K}}_k({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k\mid k-1})$$

(36)

$${\mathit{P}}_{k\mid k}={\mathit{P}}_{k\mid k-1}-{\mathit{K}}_k{\mathit{P}}_{k\mid k-1}^{\mathit{z}\mathit{z}}{\mathit{K}}_k^T$$

(37)

3.2. The Proposed Adaptive Cubature Kalman Filter

When the state noise covariance ${\mathit{Q}}_k$ and measurement noise covariance ${\mathit{R}}_k$ are unknown or inaccurate, the estimation accuracy of CKF may degrade or diverge. Because the one step predicted state error covariance ${\mathit{P}}_{k\mid k-1}$ is influenced by the inaccurate ${\mathit{Q}}_k$, it is easier to estimate ${\mathit{P}}_{k\mid k-1}$ than ${\mathit{Q}}_k$. Therefore, in our works, the state, one step predicted state error covariance ${\mathit{P}}_{k\mid k-1}$ and ${\mathit{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:

$$\mathrm{IW}(\mathit{B};\mathsf{\zeta },\mathsf{\Psi })=\frac{{\left|\mathsf{\Psi }\right|}^{\mathsf{\zeta }/2}{\left|\mathit{B}\right|}^{-(\mathsf{\zeta }+d+1)/2}{\mathrm{e}}^{-tr\left(\mathsf{\Psi }{\mathit{B}}^{-1}\right)/2}}{2^{d\mathsf{\zeta }/2}{\mathsf{\Gamma }}_d(\mathsf{\zeta }/2)}$$

(38)

where $\mathit{B}$ is positive definite random matrix, $\mathsf{\Psi }$ is the inverse scale matrix, $\mathsf{\zeta }$ is the degrees of freedom (dof) parameter, d is the dimension of $\mathit{B}$, $tr(·)$ is the trace calculation, and ${\mathsf{\Gamma }}_d(·)$ is the d-variate Gamma function. When $\mathsf{\zeta }>d+1$, the mean of $\mathit{B}$ is shown as follows:

$$\mathrm{E}\left[\mathbf{B}\right]=\frac{\mathsf{\Psi }}{{(\mathsf{\zeta }-d-1)}^{-1}}$$

(39)

Therefore, the prior distribution $p\left({\mathit{P}}_{k\mid k-1}\right|{\mathit{z}}_{1:k-1})$ and $p\left({\mathit{R}}_k\right|{\mathit{z}}_{1:k-1})$ are modeled as follows:

$$p\left({\mathit{P}}_{k|k-1}|{\mathit{z}}_{1:k-1}\right)=\mathrm{IW}({\mathit{P}}_{k|k-1};{\widehat{t}}_{k|k-1},{\widehat{\mathit{T}}}_{k|k-1})$$

(40)

$$p\left({\mathit{R}}_k|{\mathit{z}}_{1:k-1}\right)=\mathrm{IW}({\mathit{R}}_k;{\widehat{u}}_{k|k-1},{\widehat{\mathit{U}}}_{k|k-1})$$

(41)

where ${\widehat{t}}_{k|k-1}$ and ${\widehat{u}}_{k|k-1}$ are dof parameters and ${\widehat{\mathit{T}}}_{k|k-1}$ and ${\widehat{\mathit{U}}}_{k|k-1}$ are inverse scale matrices.

The mean value of ${\mathit{P}}_{k|k-1}$ is set as nominal ${\tilde{\mathit{P}}}_{k|k-1}$, determined by:

$${\tilde{\mathit{P}}}_{k|k-1}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}\left[({\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1}){({\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1})}^T\right]+\mathit{g}\left({\widehat{\mathit{x}}}_{k|k-1}\right){\tilde{\mathit{Q}}}_{k-1}\mathit{g}{\left({\widehat{\mathit{x}}}_{k|k-1}\right)}^T$$

(42)

where ${\tilde{\mathit{Q}}}_{k-1}$ is the nominal state noise covariance matrix, which means an inaccurate value.

Let:

$$\frac{{\widehat{\mathit{T}}}_{k|k-1}}{{\widehat{t}}_{k|k-1}-n_x-1}={\tilde{\mathit{P}}}_{k|k-1}$$

(43)

and set ${\widehat{t}}_{k|k-1}=n_x+\tau +1$, where $\tau$ is a tuning parameter. We can obtain:

$${\widehat{\mathit{T}}}_{k|k-1}=\tau {\tilde{\mathit{P}}}_{k|k-1}$$

(44)

According to the Bayesian theorem, $p\left({\mathit{R}}_k|{\mathit{z}}_{1:k-1}\right)$ is formulated as:

$$p\left({\mathit{R}}_k|{\mathit{z}}_{1:k-1}\right)=\int p\left({\mathit{R}}_k|{\mathit{R}}_{k-1}\right)p\left({\mathit{R}}_{k-1}|{\mathit{z}}_{1:k-1}\right)\mathrm{d}{\mathit{R}}_{k-1}$$

(45)

where $p\left({\mathit{R}}_{k-1}|{\mathit{z}}_{1:k-1}\right)$ is the posterior PDF of ${\mathit{R}}_{k-1}$. Because the posterior and prior PDF of ${\mathit{R}}_{k-1}$ has the same distribution, the posterior PDF of ${\mathit{R}}_{k-1}$ is also formulated as the inverse Wishart distribution, as follows:

$$p\left({\mathit{R}}_{k-1}|{\mathit{z}}_{1:k-1}\right)=\mathrm{IW}({\mathit{R}}_{k-1};{\widehat{u}}_{k|k-1},{\widehat{\mathit{U}}}_{k|k-1})$$

(46)

Because of the unknown dynamic model of $p\left({\mathit{R}}_k|{\mathit{R}}_{k-1}\right)$, we selected a forgetting factor $\xi \in (0,1]$ to spread the previous posterior to the current prior, and the prior parameters in (41) are written as follows:

$${\widehat{u}}_{k|k-1}=\xi \left({\widehat{u}}_{k-1|k-1}-n_z-1\right)+n_z+1$$

(47)

$${\widehat{\mathit{U}}}_{k|k-1}=\xi {\widehat{\mathit{U}}}_{k-1|k-1}$$

(48)

The initial ${\mathit{R}}_0$ is also assumed as an inverse Wishart PDF, i.e., $p\left({\mathit{R}}_0\right)=\mathrm{IW}({\mathit{R}}_0;{\widehat{u}}_{0|0},{\widehat{\mathit{U}}}_{0|0})$, where the mean value of ${\mathit{R}}_0$ is set as the initial nominal ${\tilde{\mathit{R}}}_0$:

$$\frac{{\widehat{\mathit{U}}}_{0|0}}{{\widehat{u}}_{0|0}-n_z-1}={\tilde{\mathit{R}}}_0$$

(49)

In order to estimate the state ${\mathit{x}}_k$, one step predicted state error covariance ${\mathit{P}}_{k\mid k-1}$ and ${\mathit{R}}_k$, their joint posterior PDF $p\left({\mathit{x}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k|{\mathit{z}}_{1:k}\right)$ 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.

$$p({\mathit{x}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k|{\mathit{z}}_{1:k})\approx q\left({\mathit{x}}_k\right)q\left({\mathit{P}}_{k|k-1}\right)q\left({\mathit{R}}_k\right)$$

(50)

$\left\{q\left({\mathit{x}}_k\right),q\left({\mathit{P}}_{k|k-1}\right),q\left({\mathit{R}}_k\right)\right\}$ are calculated by minimizing the Kullback–Leibler divergence (KLD):

$$\left\{q\left({\mathit{x}}_k\right),q\left({\mathit{P}}_{k|k-1}\right),q\left({\mathit{R}}_k\right)\right\}=argminKLD\left(q\left({\mathit{x}}_k\right),q\left({\mathit{P}}_{k|k-1}\right),q\left({\mathit{R}}_k\right)\parallel p({\mathit{x}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k|{\mathit{z}}_{1:k})\right)$$

(51)

The optimal solution of (51) is given by:

$$logq\left(\alpha \right)=E_{{\mathbf{\Xi }}^{(-\alpha )}}\left[logp\left(\mathbf{\Xi },{\mathit{z}}_{1:k}\right)\right]+c_{\alpha }$$

(52)

$$\mathbf{\Xi }\triangleq \left\{{\mathit{x}}_k,{\mathit{P}}_{k|k-1},{\mathit{R}}_k\right\}$$

(53)

where $log(·)$ means the logarithmic function, $\alpha$ is the arbitrary element of $\mathbf{\Xi }$, ${\mathbf{\Xi }}^{(-\alpha )}$ contains all elements in $\mathbf{\Xi }$ except for $\alpha$, and $c_{\alpha }$ means the constant dependent on $\alpha$. According to the Bayesian theorem, the joint PDF $p(\mathbf{\Xi },{\mathit{z}}_{1:k})$ is factored as:

$$p(\mathbf{\Xi },{\mathit{z}}_{1:k})=p\left({\mathit{z}}_k\right|{\mathit{x}}_k,{\mathit{R}}_k)p\left({\mathit{x}}_k\right|{\mathit{z}}_{1:k-1},{\mathit{P}}_{k|k-1})p\left({\mathit{P}}_{k|k-1}\right|{\mathit{z}}_{1:k-1})p\left({\mathit{R}}_k\right|{\mathit{z}}_{1:k-1})p\left({\mathit{z}}_{1:k-1}\right)$$

(54)

where likelihood PDF $p\left({\mathit{z}}_k|{\mathit{x}}_k\right)$ is assumed as a normal distribution.

$$p\left({\mathit{z}}_k\right|{\mathit{x}}_k)=\mathrm{N}({\mathit{z}}_k;\mathit{h}\left({\mathit{x}}_k\right),{\mathit{R}}_k)$$

(55)

Substituting (13), (38), (41), and (55) into (54), we have:

$$\begin{array}{cc}\hfill p(\mathbf{\Xi },{\mathit{z}}_{1:k})=& \mathrm{N}({\mathit{z}}_k;\mathit{h}\left({\mathit{x}}_k\right),{\mathit{R}}_k)\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1})\mathrm{IW}({\mathit{P}}_{k|k-1};{\widehat{t}}_{k|k-1},{\widehat{\mathit{T}}}_{k|k-1})\hfill \\ \hfill & \times \mathrm{IW}({\mathit{R}}_k;{\widehat{u}}_{k|k-1},{\widehat{\mathit{U}}}_{k|k-1})p\left({\mathit{z}}_{1:k-1}\right)\hfill \end{array}$$

(56)

Taking the logarithm on both sides of (56), the normal distribution $N(\mathit{A};a,\mathsf{\Sigma })$ and IW distribution $\mathrm{IW}(\mathit{B};\mathsf{\zeta },\mathsf{\Psi })$ are formulated as follows:

$$\begin{array}{cc}\hfill log\left(\mathrm{N}\right(\mathit{A};a,\mathsf{\Sigma }\left)\right)& =log\left\{\frac{1}{\sqrt{2\pi }{\left|\mathsf{\Sigma }\right|}^{\frac{1}{2}}}{e}^{-\frac{1}{2}{(\mathit{A}-a)}^T{\mathsf{\Sigma }}^{-1}(\mathit{A}-a)}\right\}\hfill \\ \hfill & =-\frac{1}{2}log\left|\mathsf{\Sigma }\right|-\frac{1}{2}{(\mathit{A}-a)}^T{\mathsf{\Sigma }}^{-1}(\mathit{A}-a)+log\frac{1}{\sqrt{2\pi }}\hfill \\ \hfill & =-\frac{1}{2}log\left|\mathsf{\Sigma }\right|-\frac{1}{2}{(\mathit{A}-a)}^T{\mathsf{\Sigma }}^{-1}(\mathit{A}-a)+c_{\mathit{A}}\hfill \end{array}$$

(57)

$$\begin{array}{cc}\hfill log\left(\mathrm{IW}\right(\mathit{B};\mathsf{\zeta },\mathsf{\Psi }\left)\right)=& log\left\{\frac{{\left|\mathsf{\Psi }\right|}^{\mathsf{\zeta }/2}{\left|\mathit{B}\right|}^{-(\mathsf{\zeta }+d+1)/2}{\mathrm{e}}^{-tr\left(\mathsf{\Psi }{\mathit{B}}^{-1}\right)/2}}{2^{d\mathsf{\zeta }/2}{\mathsf{\Gamma }}_d(\mathsf{\zeta }/2)}\right\}\hfill \\ \hfill =& -\frac{(\mathsf{\zeta }+d+1)}{2}log\left|\mathit{B}\right|-\frac{1}{2}tr\left(\mathsf{\Psi }{\mathit{B}}^{-1}\right)+\frac{\mathsf{\zeta }}{2}log\left|\mathsf{\Psi }\right|\hfill \\ \hfill & -\frac{d\mathsf{\zeta }}{2}log2-log{\mathsf{\Gamma }}_d(\mathsf{\zeta }/2)\hfill \\ \hfill =& -\frac{(\mathsf{\zeta }+d+1)}{2}log\left|\mathit{B}\right|-\frac{1}{2}tr\left(\mathsf{\Psi }{\mathit{B}}^{-1}\right)+c_{\mathit{B}}\hfill \end{array}$$

(58)

According to (57) and (58), $log\left(p\left(\mathbf{\Xi },{\mathit{z}}_{1:k}\right)\right)$ is formulated as:

$$\begin{array}{cc}\hfill log\left(p\left(\mathbf{\Xi },{\mathit{z}}_{1:k}\right)\right)=& -\frac{1}{2}{({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right))}^T{\mathit{R}}_k^{-1}({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right))\hfill \\ \hfill & -\frac{1}{2}({\widehat{u}}_{k|k-1}+n_z+2)log\left|{\mathit{R}}_k\right|-\frac{1}{2}tr\left({\mathit{U}}_{k|k-1}{\mathit{R}}_k^{-1}\right)\hfill \\ \hfill & -\frac{1}{2}({\widehat{t}}_{k|k-1}+n_x+2)log\left|{\mathit{P}}_{k|k-1}\right|-\frac{1}{2}tr\left({\mathit{T}}_{k|k-1}{\mathit{P}}_{k|k-1}^{-1}\right)\hfill \\ \hfill & -\frac{1}{2}{({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1})}^T{\mathit{P}}_{k|k-1}^{-1}({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1})+c_{\mathbf{\Xi }}\hfill \end{array}$$

(59)

Using (59) in (52) and letting $\alpha ={\mathit{P}}_{k|k-1}$, we have:

$$\begin{array}{cc}\hfill log{q}^{(i+1)}\left({\mathit{P}}_{k|k-1}\right)=& -\frac{1}{2}{\mathrm{E}}^{\left(i\right)}[tr\left({\mathit{U}}_{k|k-1}{\mathit{R}}_k^{-1}\right)]-\frac{1}{2}{\mathrm{E}}^{\left(i\right)}\left[{({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right))}^T{\mathit{R}}_k^{-1}({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right))\right]\hfill \\ \hfill & -\frac{1}{2}({\widehat{u}}_{k|k-1}+n_z+2){\mathrm{E}}^{\left(i\right)}[log|{\mathit{R}}_k\left|\right]-\frac{1}{2}({\widehat{t}}_{k|k-1}+n_x+2)log\left|{\mathit{P}}_{k|k-1}\right|\hfill \\ \hfill & -\frac{1}{2}tr\left[({\mathit{A}}_k^{\left(i\right)}+{\widehat{\mathit{T}}}_{k|k-1}){\mathit{P}}_{k|k-1}^{-1}\right]+c_{{\mathit{P}}_{k|k-1}}\hfill \\ \hfill =& -\frac{1}{2}({\widehat{t}}_{k|k-1}+n_x+2)log\left|{\mathit{P}}_{k|k-1}\right|-\frac{1}{2}tr\left[({\mathit{A}}_k^{\left(i\right)}+{\widehat{\mathit{T}}}_{k|k-1}){\mathit{P}}_{k|k-1}^{-1}\right]+c_{{\mathit{P}}_{k|k-1}}\hfill \end{array}$$

(60)

where $q^{(i+1)}(·)$ is the approximation of PDF $q(·)$ at the iteration $i+1$, and ${\mathit{A}}_k^{\left(i\right)}$ is given as follows:

$$\begin{array}{cc}\hfill {\mathit{A}}_k^{\left(i\right)}=& {\mathrm{E}}^{\left(i\right)}\left[({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}){({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1})}^T\right]\hfill \\ \hfill =& {\mathrm{E}}^{\left(i\right)}\left[({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}+{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-{\widehat{\mathit{x}}}_{k|k-1}){({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}+{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-{\widehat{\mathit{x}}}_{k|k-1})}^T\right]\hfill \\ \hfill =& {\mathit{P}}_{k|k}^{\left(i\right)}+({\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-{\widehat{\mathit{x}}}_{k|k-1}){({\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-{\widehat{\mathit{x}}}_{k|k-1})}^T\hfill \end{array}$$

(61)

$q^{(i+1)}\left({\mathit{P}}_{k|k-1}\right)$ is updated as an IW PDF with dof parameter ${\widehat{t}}_k^{(i+1)}$ and inverse scale matrix ${\widehat{\mathit{T}}}_k^{(i+1)}$:

$$q^{(i+1)}\left({\mathit{P}}_{k|k-1}\right)=\mathrm{IW}({\mathit{P}}_{k|k-1};{\widehat{t}}_k^{(i+1)},{\widehat{\mathit{T}}}_k^{(i+1)})$$

(62)

where:

$${\widehat{t}}_k^{(i+1)}={\widehat{t}}_{k|k-1}+1$$

(63)

$${\widehat{\mathit{T}}}_k^{(i+1)}={\mathit{A}}_k^{\left(i\right)}+{\widehat{\mathit{T}}}_{k|k-1}$$

(64)

Let $\alpha ={\mathit{R}}_k$; we have:

$$log{q}^{(i+1)}\left({\mathit{R}}_k\right)=-0.5\left({\widehat{u}}_{k|k-1}+L_z+2\right)log\left|{\mathit{R}}_k\right|-\frac{1}{2}tr\left[({\mathit{B}}_k^{\left(i\right)}+{\widehat{\mathit{U}}}_{k|k-1}){\mathit{R}}_k^{-1}\right]+c_{{\mathit{R}}_k}$$

(65)

where ${\mathit{B}}_k^{\left(i\right)}$ is given by:

$$\begin{array}{cc}\hfill {\mathit{B}}_k^{\left(i\right)}& ={\mathrm{E}}^{\left(i\right)}\left[({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right)){({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right))}^T\right]\hfill \\ \hfill & =\int ({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right)){({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right))}^T\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)},{\mathit{P}}_{k|k}^{\left(i\right)})\mathrm{d}{\mathit{x}}_k\hfill \\ \hfill & =\frac{1}{2n_x}\sum _{j=1}^{2n_x}\left[({\mathit{z}}_k-\mathit{h}\left({\mathit{\chi }}_k^{\left(i\right)\left(j\right)}\right)){({\mathit{z}}_k-\mathit{h}\left({\mathit{\chi }}_k^{\left(i\right)\left(j\right)}\right))}^T\right]\hfill \end{array}$$

(66)

where ${\mathit{\chi }}_k^{\left(i\right)}$ are cubature points based on ${\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}$ and ${\mathit{P}}_{k|k}^{\left(i\right)}$.

$$\left\{\begin{array}{c}{\mathit{\chi }}_k^{\left(i\right)\left(j\right)}={\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}+\sqrt{n_x}{\left(\sqrt{{\mathit{P}}_{k|k}^{\left(i\right)}}\right)}_{\left(j\right)},(j=1,2,\cdots ,n_x)\hfill \\ {\mathit{\chi }}_k^{\left(i\right)\left(j\right)}={\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-\sqrt{n_x}{\left(\sqrt{{\mathit{P}}_{k|k}^{\left(i\right)}}\right)}_{(j-n_x)},(j=n_x+1,n_x+2,\cdots ,2n_x)\hfill \end{array}\right.$$

(67)

$q^{(i+1)}\left({\mathit{R}}_k\right)$ is updated as an IW PDF with dof parameter ${\widehat{u}}_k^{(i+1)}$ and inverse scale matrix ${\widehat{\mathit{U}}}_k^{(i+1)}$:

$$q^{(i+1)}\left({\mathit{R}}_k\right)=\mathrm{IW}({\mathit{R}}_k;{\widehat{u}}_k^{(i+1)},{\widehat{\mathit{U}}}_k^{(i+1)})$$

(68)

where:

$${\widehat{u}}_k^{(i+1)}={\widehat{u}}_{k|k-1}+1$$

(69)

$${\widehat{\mathit{U}}}_k^{(i+1)}={\mathit{B}}_k^{\left(i\right)}+{\widehat{\mathit{U}}}_{k|k-1}$$

(70)

Let $\alpha ={\mathit{x}}_k$; we have:

$$\begin{array}{cc}\hfill log{q}^{(i+1)}\left({\mathit{x}}_k\right)=& -\frac{1}{2}{\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}\right)}^T{\mathrm{E}}^{(i+1)}\left[{\mathit{P}}_{k|k-1}^{-1}\right]\left({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k|k-1}\right)\hfill \\ \hfill & -\frac{1}{2}{\left({\mathit{z}}_k-\mathit{h}\left({\mathit{x}}_k\right)\right)}^T{\mathrm{E}}^{(i+1)}\left[{\mathit{R}}_k^{-1}\right]\left({\mathit{z}}_k-h\left({\mathit{x}}_k\right)\right)+c_{\mathit{x}}\hfill \end{array}$$

(71)

where:

$${\mathrm{E}}^{(i+1)}\left[{\mathit{R}}_k^{-1}\right]=({\widehat{u}}_k^{(i+1)}-n_x-1){\left({\widehat{\mathit{U}}}_k^{(i+1)}\right)}^{-1}$$

(72)

$${\mathrm{E}}^{(i+1)}\left[{\mathit{P}}_{k|k-1}^{-1}\right]=({\widehat{t}}_k^{(i+1)}-n_z-1){\left({\widehat{\mathit{T}}}_k^{(i+1)}\right)}^{-1}$$

(73)

The one step predicted PDF $p^{(i+1)}\left({\mathit{x}}_k\right|{\mathit{z}}_{1:k-1})$ and likelihood PDF $p^{(i+1)}\left({\mathit{z}}_k\right|{\mathit{x}}_k)$ at iteration $i+1$ are defined as follows:

$$p^{(i+1)}\left({\mathit{x}}_k\right|{\mathit{z}}_{1:k-1})=\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k-1},{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)})$$

(74)

$$p^{(i+1)}\left({\mathit{z}}_k\right|{\mathit{x}}_k)=\mathrm{N}({\mathit{z}}_k;\mathit{h}\left({\mathit{x}}_k\right),{\widehat{\mathit{R}}}_k^{(i+1)})$$

(75)

where:

$${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}={\left\{{\mathrm{E}}^{(i+1)}\left[{\mathit{P}}_{k|k-1}^{-1}\right]\right\}}^{-1}$$

(76)

$${\widehat{\mathit{R}}}_k^{(i+1)}={\left\{{\mathrm{E}}^{(i+1)}\left[{\mathit{R}}_k^{-1}\right]\right\}}^{-1}$$

(77)

Employing (74)–(77) in (71), we have:

$$q^{(i+1)}\left({\mathit{x}}_k\right)=\frac{1}{c_k^{(i+1)}}{p}^{(i+1)}\left({\mathit{z}}_k|{\mathit{x}}_k\right)p^{(i+1)}\left({\mathit{x}}_k|{\mathit{z}}_{1:k-1}\right)$$

(78)

where the normalization constant $c_k^{(i+1)}$ is given as:

$$c_k^{(i+1)}=\int p^{(i+1)}\left({\mathit{z}}_k|{\mathit{x}}_k\right)p^{(i+1)}\left({\mathit{x}}_k|{\mathit{z}}_{1:k-1}\right)\mathrm{d}{\mathit{x}}_k$$

(79)

$q^{(i+1)}\left({\mathit{x}}_k\right)$ is updated as the normal distribution with mean ${\widehat{\mathit{x}}}_{k|k}^{(i+1)}$ and variance ${\widehat{\mathit{P}}}_{k|k}^{(i+1)}$:

$$q^{(i+1)}\left({\mathit{x}}_k\right)=\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k}^{(i+1)},{\widehat{\mathit{P}}}_{k|k}^{(i+1)})$$

(80)

where ${\widehat{\mathit{x}}}_{k|k}^{(i+1)}$ and ${\widehat{\mathit{P}}}_{k|k}^{(i+1)}$ at iteration $i+1$ are calculated similarly to (31)–(37).

The cubature points of ${\mathit{x}}_k$ based on ${\widehat{\mathit{x}}}_{k\mid k-1}$ and modified ${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$ are given as:

$$\left\{\begin{array}{c}{\mathit{\chi }}_{k\mid k-1}^{(i+1)\left(j\right)}={\widehat{\mathit{x}}}_{k\mid k-1}+\sqrt{n_x}{\left(\sqrt{{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}}\right)}_{\left(j\right)},(j=1,2,\cdots ,n_x)\hfill \\ {\mathit{\chi }}_{k\mid k-1}^{(i+1)\left(j\right)}={\widehat{\mathit{x}}}_{k\mid k-1}-\sqrt{n_x}{\left(\sqrt{{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}}\right)}_{(j-n_x)},(j=n_x+1,n_x+2,\cdots ,2n_x)\hfill \end{array}\right.$$

(81)

$${\mathrm{Z}}_{k|k-1}^{(i+1)\left(j\right)}=\mathit{h}\left({\mathit{\chi }}_{k|k-1}^{(i+1)\left(j\right)}\right),(j=1,2,\cdots ,2n_x)$$

(82)

$${\widehat{\mathit{z}}}_{k|k-1}^{(i+1)}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}{\mathrm{Z}}_{k|k-1}^{(i+1)\left(j\right)}$$

(83)

$${\mathit{P}}_{k|k-1}^{\mathit{z}\mathit{z}(i+1)}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}\left[({\mathrm{Z}}_{k|k-1}^{(i+1)\left(j\right)}-{\widehat{\mathit{z}}}_{k|k-1}^{(i+1)}){({\mathrm{Z}}_{k\mid k-1}^{(i+1)\left(j\right)}-{\widehat{\mathit{z}}}_{k\mid k-1}^{(i+1)})}^T\right]+{\widehat{\mathit{R}}}_k^{(i+1)}$$

(84)

$${\mathit{P}}_{k\mid k-1}^{\mathit{x}\mathit{z}(i+1)}=\frac{1}{2n_x}\sum _{j=1}^{2n_x}\left[({\mathit{\chi }}_{k\mid k-1}^{(i+1)\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1}){({\mathrm{Z}}_{k\mid k-1}^{(i+1)\left(j\right)}-{\widehat{\mathit{z}}}_{k\mid k-1}^{(i+1)})}^T\right]$$

(85)

$${\mathit{K}}_k^{(i+1)}={\mathit{P}}_{k\mid k-1}^{\mathit{x}\mathit{z}(i+1)}{\left({\mathit{P}}_{k\mid k-1}^{\mathit{z}\mathit{z}(i+1)}\right)}^{-1}$$

(86)

$${\widehat{\mathit{x}}}_{k\mid k}^{(i+1)}={\widehat{\mathit{x}}}_{k\mid k-1}+{\mathit{K}}_k^{(i+1)}({\mathit{z}}_k-{\widehat{\mathit{z}}}_{k|k-1}^{(i+1)})$$

(87)

$${\widehat{\mathit{P}}}_{k|k}^{(i+1)}={\mathit{P}}_{k\mid k-1}^{(i+1)}-{\mathit{K}}_k^{(i+1)}{\mathit{P}}_{k\mid k-1}^{\mathit{z}\mathit{z}(i+1)}{\left({\mathit{K}}_k^{(i+1)}\right)}^T$$

(88)

After N fixed point iterations, we can obtain the approximate solution of $q\left({\mathit{x}}_k\right)$, $q\left({\mathit{P}}_{k|k-1}\right)$ and $q\left({\mathit{R}}_k\right)$:

$$q\left({\mathit{x}}_k\right)\approx q^{\left(N\right)}\left({\mathit{x}}_k\right)=\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k}^{\left(N\right)},{\widehat{\mathit{P}}}_{k|k}^{\left(N\right)})$$

(89)

$$q\left({\mathit{P}}_{k|k-1}\right)\approx q^{\left(N\right)}\left({\mathit{P}}_{k|k-1}\right)=\mathrm{IW}({\mathit{P}}_{k|k-1};{\widehat{t}}_k^{\left(N\right)},{\widehat{\mathit{T}}}_k^{\left(N\right)})$$

(90)

$$q\left({\mathit{R}}_k\right)\approx q^{\left(N\right)}\left({\mathit{R}}_k\right)=\mathrm{IW}({\mathit{R}}_k;{\widehat{u}}_k^{\left(N\right)},{\widehat{\mathit{U}}}_k^{\left(N\right)})$$

(91)

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:

$$\begin{array}{cc}\hfill {\mathit{B}}_k^{\left(i\right)}=& {\mathrm{E}}^{\left(i\right)}\left[({\mathit{z}}_k-\mathit{H}{\mathit{x}}_k){({\mathit{z}}_k-\mathit{H}{\mathit{x}}_k)}^T\right]\hfill \\ \hfill =& {\mathrm{E}}^{\left(i\right)}\left[({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}+\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-\mathit{H}{\mathit{x}}_k){({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}+\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-\mathit{H}{\mathit{x}}_k)}^T\right]\hfill \\ \hfill =& ({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}){({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)})}^T+\mathit{H}{\mathit{P}}_{k|k}^{\left(i\right)}{\mathit{H}}^T\hfill \end{array}$$

(92)

$${\mathit{K}}_k^{(i+1)}={\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}{\mathit{H}}^T{(\mathit{H}{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}{\mathit{H}}^T+{\widehat{\mathit{R}}}_k^{(i+1)})}^{-1}$$

(93)

$${\widehat{\mathit{x}}}_{k\mid k}^{(i+1)}={\widehat{\mathit{x}}}_{k\mid k-1}+{\mathit{K}}_k^{(i+1)}({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k\mid k-1})$$

(94)

$${\widehat{\mathit{P}}}_{k|k}^{(i+1)}={\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}-{\mathit{K}}_k^{(i+1)}\mathit{H}{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$$

(95)

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 $\tau$, the forgetting factor $\xi$, and the iteration number N. Tuning parameter $\tau$ can be seen as an adjustment parameter of ${\tilde{\mathit{P}}}_{k|k-1}$. If $\tau$ is too large, the prior uncertainties induced by nominal ${\tilde{\mathit{Q}}}_k$ will influence the measurement update. If $\tau$ 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 $\tau \in [2,6]$, which has better estimation performance and estimation accuracy. The forgetting factor $\xi$ also adjusts the influence of ${\widehat{\mathit{R}}}_{k-1}$. Note that $\xi =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.

Algorithm 1: One-step of the proposed adaptive cubature Kalman filter.

Inputs: ${\widehat{\mathit{x}}}_{k-1|k-1}$, ${\mathit{P}}_{k-1|k-1}$, ${\mathit{z}}_k$, ${\widehat{u}}_{k-1|k-1}$, ${\widehat{\mathit{U}}}_{k-1|k-1}$, ${\tilde{\mathit{Q}}}_{k-1}$, $\tau$, $\xi$, N.

Time update

1. Calculate cubature points based on ${\widehat{\mathit{x}}}_{k-1|k-1}$ and ${\mathit{P}}_{k-1|k-1}$.

2. ${\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}=\mathit{f}\left({\mathit{\chi }}_{k-1}^{\left(j\right)}\right),(j=1,2,\cdots ,2n_x)$.

3. ${\widehat{\mathit{x}}}_{k\mid k-1}=\frac{1}{2n_x}{\sum }_{j=1}^{2n_x}{\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}$.

4. ${\tilde{\mathit{P}}}_{k\mid k-1}=\frac{1}{2n_x}{\sum }_{j=1}^{2n_x}\left[({\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1}){({\mathit{\chi }}_{k\mid k-1}^{\mathit{x}\left(j\right)}-{\widehat{\mathit{x}}}_{k\mid k-1})}^T\right]+\mathit{g}\left({\widehat{\mathit{x}}}_{k|k-1}\right){\tilde{\mathit{Q}}}_{k-1}\mathit{g}{\left({\widehat{\mathit{x}}}_{k|k-1}\right)}^T$.

Iterated measurement update

5. Initialization: ${\widehat{\mathit{x}}}_{k|k}^{\left(0\right)}={\widehat{\mathit{x}}}_{k|k-1}$, ${\widehat{\mathit{P}}}_{k|k}^{\left(0\right)}={\tilde{\mathit{P}}}_{k|k-1}$, ${\widehat{\mathit{T}}}_{k|k-1}=\tau {\tilde{\mathit{P}}}_{k|k-1}$,${\widehat{t}}_{k|k-1}=n_x+\tau +1$,

${\widehat{u}}_{k|k-1}=\xi \left({\widehat{u}}_{k-1|k-1}-n_z-1\right)+n_z+1$, ${\widehat{\mathit{U}}}_{k|k-1}=\xi {\widehat{\mathit{U}}}_{k-1|k-1}$.

For$i=0:N-1$

6. Update $q^{(i+1)}\left({\mathit{P}}_{k|k-1}\right)=\mathrm{IW}\left({\mathit{P}}_{k|k-1};{\widehat{t}}_k^{(i+1)},{\widehat{\mathit{T}}}_k^{(i+1)}\right)$,

${\widehat{t}}_k^{(i+1)}={\widehat{t}}_{k|k-1}+1$, ${\widehat{\mathit{T}}}_k^{(i+1)}={\mathit{A}}_k^{\left(i\right)}+{\widehat{\mathit{T}}}_{k|k-1}$, where ${\mathit{A}}_k^{\left(i\right)}={\mathit{P}}_{k|k}^{\left(i\right)}+({\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-{\widehat{\mathit{x}}}_{k|k-1}){({\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}-{\widehat{\mathit{x}}}_{k|k-1})}^T$.

7. Update $q^{(i+1)}\left({\mathit{R}}_k\right)=\mathrm{IW}\left({\mathit{R}}_k;{\widehat{u}}_k^{(i+1)},{\widehat{\mathit{U}}}_k^{(i+1)}\right)$,

${\widehat{u}}_k^{(i+1)}={\widehat{u}}_{k|k-1}+1$, ${\widehat{\mathit{U}}}_k^{(i+1)}={\mathit{B}}_k^{\left(i\right)}+{\widehat{\mathit{U}}}_{k|k-1}$, where ${\mathit{B}}_k^{\left(i\right)}=({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)}){({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k|k}^{\left(i\right)})}^T+\mathit{H}{\mathit{P}}_{k|k}^{\left(i\right)}{\mathit{H}}^T$.

8. Update $q^{(i+1)}\left({\mathit{x}}_k\right)=\mathrm{N}({\mathit{x}}_k;{\widehat{\mathit{x}}}_{k|k}^{(i+1)},{\widehat{\mathit{P}}}_{k|k}^{(i+1)})$,

${\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}={\left(({\widehat{t}}_k^{(i+1)}-n_x-1){\left({\widehat{\mathit{T}}}_k^{(i+1)}\right)}^{-1}\right)}^{-1}$, ${\widehat{\mathit{R}}}_k^{(i+1)}={\left(({\widehat{u}}_k^{(i+1)}-n_z-1){\left({\widehat{\mathit{U}}}_k^{(i+1)}\right)}^{-1}\right)}^{-1}$.

9. Calculate the mean and variance of posterior PDF,

${\mathit{K}}_k^{(i+1)}={\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}{\mathit{H}}^T{(\mathit{H}{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}{\mathit{H}}^T+{\widehat{\mathit{R}}}_k^{(i+1)})}^{-1}$,

${\widehat{\mathit{x}}}_{k\mid k}^{(i+1)}={\widehat{\mathit{x}}}_{k\mid k-1}+{\mathit{K}}_k^{(i+1)}({\mathit{z}}_k-\mathit{H}{\widehat{\mathit{x}}}_{k\mid k-1})$,

${\widehat{\mathit{P}}}_{k|k}^{(i+1)}={\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}-{\mathit{K}}_k^{(i+1)}\mathit{H}{\widehat{\mathit{P}}}_{k|k-1}^{(i+1)}$.

End for

10. ${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k}^{\left(N\right)}$,${\mathit{P}}_{k|k}={\widehat{\mathit{P}}}_{k|k}^{\left(N\right)}$, ${\widehat{u}}_{k|k}={\widehat{u}}_k^{\left(N\right)}$, ${\widehat{\mathit{U}}}_{k|k}={\widehat{\mathit{U}}}_k^{\left(N\right)}$.

Outputs: ${\widehat{\mathit{x}}}_{k|k}$, ${\mathit{P}}_{k|k}$, ${\widehat{u}}_{k|k}$, ${\widehat{\mathit{U}}}_{k|k}$.

4. Simulation and Vehicle Experiment of SINS

4.1. 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 ${}^{\circ }\mathrm{N}$ and 126.446 ${}^{\circ }\mathrm{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:

$$\left\{\begin{array}{c}pitch=pitc{h}_{m}sin(2\pi k{T}_s/T_p+pitc{h}_0)+pitchI\hfill \\ roll=rol{l}_{m}sin(2\pi k{T}_s/T_R+rol{l}_0)+rollI\hfill \\ heading=headin{g}_{m}sin(2\pi k{T}_s/T_H+headin{g}_0)+headingI\hfill \end{array}\right\}$$

(96)

where $pitc{h}_m$, $rol{l}_m$, and $headin{g}_m$ are swing amplitudes, which were selected as $pitc{h}_m=5^{\circ }$, $rol{l}_m=6^{\circ }$, and $headin{g}_m=7^{\circ }$; $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; $pitc{h}_0$, $rol{l}_0$, and $headin{g}_0$ are initial swing phases, which were selected as $pitc{h}_0=0^{\circ }$, $rol{l}_0=0^{\circ }$, and $headin{g}_0=0^{\circ }$; $pitchI$, $rollI$, and $headingI$ are initial attitude angles, which were selected as $pitchI=0^{\circ }$, $rollI=0^{\circ }$, and $headingI={45}^{\circ }$; $T_s=0.01$ s is the discrete time of the system. The initial misalignment angles were set as $\mathit{\varphi }=[5^{\circ };5^{\circ };{15}^{\circ }]$. The nominal sensor specifications of SINS in the simulation were set as the practical SINS, which are shown in

Table 1. The nominal sensor specifications of the strapdown inertial navigation system (SINS).

Parameters	Value
Gyroscope bias stability	${0.01}^{\circ }/$h
Gyroscope angular random walk	${0.1}^{\circ }$/h($\sqrt{\mathrm{s}})$
Accelerometer bias stability	${10}^{-4}g$
Accelerometer velocity random walk	${10}^{-5}g\left(\sqrt{\mathrm{s}}\right)$

.

Based on the parameters in Table 1, the nominal state noise covariance matrix was set as ${\tilde{\mathit{Q}}}_k=T_s\mathrm{diag}{\left([{0.1}^{\circ }/\mathrm{h}\left(\sqrt{s}\right){\mathit{I}}_{3\times 3};{10}^{-5}g\left(\sqrt{\mathrm{s}}\right){\mathit{I}}_{2\times 2};{\mathbf{0}}_{7\times 1}]\right)}^2$. The nominal measurement noise covariance matrix was set as ${\tilde{\mathit{R}}}_k=\mathrm{diag}{\left([0.1\mathrm{m}/\mathrm{s}{\mathit{I}}_{2\times 2};arctan(10m/R_e){\mathit{I}}_{2\times 2}]\right)}^2$. Considering that the true ${\mathit{Q}}_k$ was always larger than the nominal value due to the external disturbance such as vibration and the slowly time varying characteristic, the true ${\mathit{Q}}_k$ was set as ${\mathit{Q}}_k=[1+0.1cos\left(\frac{\pi k{T}_s}{H_n}\right)]T_s\mathrm{diag}{\left([1^{\circ }/\mathrm{h}\left(\sqrt{\mathrm{s}}\right){\mathit{I}}_{3\times 3};{10}^{-4}g\left(\sqrt{\mathrm{s}}\right){\mathit{I}}_{2\times 2};{\mathbf{0}}_{7\times 1}]\right)}^2$, where $H_n=100s$ denotes the simulation time. The true ${\mathit{R}}_k$ was set as ${\mathit{R}}_k=[1+0.1cos\left(\frac{\pi k{T}_s}{H_n}\right)]\mathrm{diag}$${\left([0.01\mathrm{m}/\mathrm{s}{\mathit{I}}_{2\times 2};arctan(1m/R_e){\mathit{I}}_{2\times 2}]\right)}^2$. Existing UKF [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 $\tau =5$, $\xi =0.98$, and $N=10$, respectively. The initial state was set as ${\widehat{\mathit{x}}}_{0|0}={\mathbf{0}}_{12\times 1}$. The initial state error covariance matrix was set as ${\mathit{P}}_{0|0}=\mathrm{diag}{\left([5^{\circ };5^{\circ };{15}^{\circ };0.1\mathrm{m}/\mathrm{s}{\mathit{I}}_{2\times 2};arctan(10m/R_e){\mathit{I}}_{2\times 2};{0.01}^{\circ }/\mathrm{h}{\mathit{I}}_{3\times 3};{10}^{-4}g{\mathit{I}}_{2\times 2}]\right)}^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 ${\mathit{P}}_{k|k-1}$ and ${\mathit{R}}_k$, the square root of normalized Frobenius norm (SRNFN) was used, which is defined as follows:

$$\left\{\begin{array}{c}SRNF{N}_P={\left(\frac{1}{n_x^{2}M}\sum _{m=1}^M{∥{\widehat{\mathit{P}}}_{k|k-1}^m-{\overline{\mathit{P}}}_{k|k-1}^m∥}^2\right)}^{\frac{1}{4}}\hfill \\ SRNF{N}_R={\left(\frac{1}{n_z^{2}M}\sum _{m=1}^M{∥{\widehat{\mathit{R}}}_k^m-{\mathit{R}}_k^m∥}^2\right)}^{\frac{1}{4}}\hfill \end{array}\right.$$

(97)

where ${\overline{\mathit{P}}}_{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. Average alignment errors of different methods in the latter 20 s.

Methods	Pitch	Roll	Heading
TCKF	${0.0001}^{\circ }$	${0.0001}^{\circ }$	${0.0214}^{\circ }$
UKF	$-{0.0003}^{\circ }$	$-{0.0003}^{\circ }$	$-{0.1177}^{\circ }$
CKF	${0.0002}^{\circ }$	${0.0010}^{\circ }$	${0.3119}^{\circ }$
ACKF	${0.0001}^{\circ }$	${0.0002}^{\circ }$	${0.0283}^{\circ }$

. 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 ${\mathit{P}}_{k|k-1}$ and ${\mathit{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, ${\mathit{P}}_{k|k-1}$ represents the predicted error based on the measurement information ${\mathit{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 $\tau$, forgetting factors $\xi$, and iteration numbers N were conducted.

Table 3. Average alignment errors with different tuning parameters $\tau$ ($\xi =0.98$ and $N=10$).

Parameters	Value	Pitch	Roll	Heading
$\tau$	$\tau =2$	${0.0001}^{\circ }$	${0.0001}^{\circ }$	${0.0267}^{\circ }$
	$\tau =3$	${0.0001}^{\circ }$	${0.0002}^{\circ }$	${0.0275}^{\circ }$
	$\tau =4$	${0.0001}^{\circ }$	${0.0002}^{\circ }$	${0.0279}^{\circ }$
	$\tau =5$	${0.0001}^{\circ }$	${0.0002}^{\circ }$	${0.0289}^{\circ }$
	$\tau =6$	${0.0002}^{\circ }$	${0.0003}^{\circ }$	${0.0302}^{\circ }$

,

Table 4. Average alignment errors with different forgetting factors $\xi$ ($\tau =5$ and $N=10$).

Parameters	Value	Pitch	Roll	Heading
$\xi$	$\xi =0.96$	${0.0001}^{\circ }$	${0.0001}^{\circ }$	${0.0262}^{\circ }$
	$\xi =0.97$	${0.0001}^{\circ }$	${0.0001}^{\circ }$	${0.0266}^{\circ }$
	$\xi =0.98$	${0.0001}^{\circ }$	${0.0002}^{\circ }$	${0.0284}^{\circ }$
	$\xi =0.99$	${0.0002}^{\circ }$	${0.0002}^{\circ }$	${0.0289}^{\circ }$

and

Table 5. Average alignment errors with different iteration numbers N ($\tau =5$ and $\xi =0.98$).

Parameters	Value	Pitch	Roll	Heading
N	$N=5$	${0.0002}^{\circ }$	${0.0003}^{\circ }$	${0.0463}^{\circ }$
	$N=10$	${0.0001}^{\circ }$	${0.0001}^{\circ }$	${0.0286}^{\circ }$
	$N=20$	${0.0001}^{\circ }$	${0.0001}^{\circ }$	${0.0278}^{\circ }$
	$N=30$	${0.0001}^{\circ }$	${0.0001}^{\circ }$	${0.0263}^{\circ }$

show respectively the estimation errors with different $\tau$, $\xi$, and N. It can be seen that the proposed ACKF had a consistent estimation performance when $\tau \in [2,6]$, $\xi =0.96,0.97,0.98,0.99$ and $N>5$, which also corresponded to the aforementioned analyses.

4.2. 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}^{\circ }$ for roll and pitch angles and ${0.01}^{\circ }sec\left(L\right)$ 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${}^{\circ }\mathrm{N}$, 126.446${}^{\circ }\mathrm{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 Figure 6, Figure 7 and Figure 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 $\mathit{\varphi }=[5^{\circ };5^{\circ };{15}^{\circ }]$, 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 matrix ${\tilde{\mathit{Q}}}_k$ and nominal measurement noise covariance matrix ${\tilde{\mathit{R}}}_k$ were set the same as the simulation. The tuning parameter, forgetting factor, and iteration number were set as $\tau =5$, $\xi =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 Figure 10, Figure 11, Figure 12, Figure 13, Figure 14 and Figure 15, where Figure 10, Figure 11 and Figure 12 are the results in the smooth segment and Figure 13, Figure 14 and Figure 15 are the results in the maneuvering segment. The average alignment errors in the latter 20 s are shown in

Table 6. Average alignment errors in the smooth segment.

Segments	Methods	Pitch	Roll	Heading
0 s to 100 s	UKF	${0.005}^{\circ }$	${0.016}^{\circ }$	$-{0.015}^{\circ }$
	CKF	${0.004}^{\circ }$	${0.016}^{\circ }$	$-{0.040}^{\circ }$
	ACKF	${0.006}^{\circ }$	${0.015}^{\circ }$	$-{0.013}^{\circ }$
200 s to 300 s	UKF	$-{0.014}^{\circ }$	${0.022}^{\circ }$	${0.026}^{\circ }$
	CKF	$-{0.014}^{\circ }$	${0.021}^{\circ }$	${0.058}^{\circ }$
	ACKF	$-{0.013}^{\circ }$	${0.020}^{\circ }$	${0.014}^{\circ }$
500 s to 600 s	UKF	$-{0.010}^{\circ }$	${0.005}^{\circ }$	$-{0.594}^{\circ }$
	CKF	$-{0.010}^{\circ }$	${0.005}^{\circ }$	$-{0.607}^{\circ }$
	ACKF	$-{0.009}^{\circ }$	${0.008}^{\circ }$	$-{0.303}^{\circ }$

and

Table 7. Average alignment errors in the maneuvering segment.

Segments	Methods	Pitch	Roll	Heading
800 s to 900 s	UKF	$-{0.002}^{\circ }$	${0.006}^{\circ }$	$-{0.127}^{\circ }$
	CKF	$-{0.003}^{\circ }$	${0.006}^{\circ }$	$-{0.126}^{\circ }$
	ACKF	$-{0.002}^{\circ }$	${0.001}^{\circ }$	${0.060}^{\circ }$
1300 s to 1400 s	UKF	${0.081}^{\circ }$	${0.011}^{\circ }$	${0.186}^{\circ }$
	CKF	${0.083}^{\circ }$	${0.011}^{\circ }$	${0.265}^{\circ }$
	ACKF	${0.075}^{\circ }$	$-{0.004}^{\circ }$	${0.028}^{\circ }$
1700 s to 1800 s	UKF	$-{0.013}^{\circ }$	${0.006}^{\circ }$	$-{0.489}^{\circ }$
	CKF	$-{0.014}^{\circ }$	${0.005}^{\circ }$	$-{0.415}^{\circ }$
	ACKF	$-{0.012}^{\circ }$	${0.011}^{\circ }$	$-{0.186}^{\circ }$

.

According to Figure 10, Figure 11, Figure 12, Figure 13, Figure 14 and Figure 15 and Table 6 and Table 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.

5. 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.