Robust SCKF Filtering Method for MINS/GPS In-Motion Alignment

Abstract

This paper presents a novel multiple strong tracking adaptive square-root cubature Kalman filter (MSTASCKF) based on the frame of the Sage–Husa filter, employing the multi-fading factor which could automatically adjust the Q value according to the rapidly changing noise in the flight process. This filter can estimate the system noise in real-time during the filtering process and adjust the system noise variance matrix Q so that the filtering accuracy is not significantly reduced with the noise. At the same time, the residual error in the filtering process is used as a measure of the filtering effect, and a multiple fading factor is introduced to adjust the posterior error variance matrix in the filtering process, so that the residual error is always orthogonal and the stability of the filtering is maintained. Finally, a vibration test is designed which simulates the random noise of the short-range guided weapon in flight through the shaking table and adds the noise to the present simulation trajectory for semi-physical simulation. The simulation results show that the proposed filter can significantly reduce the attitude estimation error caused by random vibration.

1. Introduction

With the development of Micro-Electro-Mechanical System (MEMS) technology, the MEMS inertial measurement unit (MIMU) is gaining attention due to its integrability and miniaturization in several fields such as guided weaponry, unmanned automatic vehicles (UAVs), and robots [1,2,3,4,5,6]. As a dead-reckoning method, the initial alignment accuracy has a vital impact to the final inertial navigation results, while there are only tens of seconds for a tactical weapon. Thus, achieving rapid, accurate, and robust initial alignment is essential to the application of the MIMU. As for the MIMU, it cannot detect the Earth’s rotation properly with high gyro noise, so alignment methods for the high-accuracy Inertial Navigation System (INS) are not suitable for it. A widely used method to achieve initial alignment for the MIMU is to use an extra sensor to provide additional information as a reference for initial alignment during the flight process. The in-flight alignment has been investigated as one of the key technologies for guided weapons and UAV applications in MIMUs in the past 10 years.

The numerous methods to solve in-flight alignment for the MIMU could be mainly divided into two categories, namely the optimization-based approach and the Kalman filter approach. The first one usually turns the problem of alignment into a Whaba’s problem. To solve the problem, several algorithms have been developed, such as the q-method, Quaternion ESTimator (QUEST), and Recursive QUEST (REQUEST) [7,8]. These methods calculate the optimal quaternion by constructing the optimal observation vectors through different filtering methods [9,10,11,12,13]. The latter method is to obtain the misalignment angle between the navigation frame to the computing frame of the INS by constructing an appropriate model for filtering. Owing to the nonlinearity of the misalignment model and the large noise caused by environment, the nonlinear filtering methods occupy a great proportion of the in-motion alignment methods to achieve stability, accuracy, and robustness.

In order to handle with the strong nonlinear system, several filtering methods based on the Kalman filter have been proposed, such as the extended Kalman filter (EKF) [14,15], unscented Kalman filter (UKF) [16,17,18], Gauss–Hermite quadrature filter (GHQF) [19,20], sparse grid quadrature filter (SGQF) [21,22,23], and cubature Kalman filter (CKF) [24,25]. The EKF approximates the nonlinear model to a linear model through Taylor expansion to deal with nonlinear problems. The advantage of this is that the filter structure is simple and the Kalman filter framework can be directly applied. Fang proposed an innovative adaptive extended Kalman filter method for the aerial alignment of airborne position and azimuth measurement systems [14]. This method improves the real-time performance of the algorithm and reduces the interference from GPS measurement noise. However, EKF can only achieve first-order accuracy and cannot handle strong nonlinear problems. In order to solve this problem, UKF is proposed. UKF assumes that the nonlinear system is a Gaussian probability density function (PDF) distribution and creates a series of sigma points based on the unscented transform rule to approximate the distribution of the system. It can achieve the second-order Taylor expansion polynomial. Saman Mukhtar Siddiqui applied the initialization process of the Central Difference Unscented Kalman filter (CDUKF) to the square-root unscented Kalman filter and verified the advantages of CDUKF in calculation time and accuracy in the on-board experiment to solve the alignment problem of misalignment angles within 30° [26]. Wang Dingjie proposed an adaptive unscented Kalman filter for a small UAV MEMS navigation system. With an initial error angle of 30°, the posture is completed in 75 s, which is more accurate than EKF and traditional UKF [27]. However, there is also a problem: For systems with a dimension higher than 3, the weight of the sigma points could be negative, and the filter process will be unstable. As a matter of fact, this problem is caused by the Unscented Transformation (UT) rule. To avoid the negativity of the sigma points, GHQF, SGQF, and CKF have been proposed using different integrated rules rather than the UT rule, which not only solves the mentioned problem but also maintains the accuracy of the second-order Taylor expansion. The GHQF utlizes the Gauss–Hermite quadrature (GHQ) rule to conduct the filter process with the quadrature sampling points that it can be accurate to the arbitrary order Taylor expansion polynomial [23]. However, the drawback of the GHQ rule is that it needs a large number of the sampling points due to the application of the direct tensor product. Thus, the GHQF would also have problems in application in high-dimensional systems. CKF with the cubature rule guarantees the positive weight, and its number of sampling points is 2n. It could also achieve a higher accuracy while applying the high-degree cubature rule with less computational burden [24]. Compared with CKF, the square-root CKF (SCKF) has more advantage not only in convergence speed but also in calculation efficiency [25]. To achieve a better performance of robustness during the in-flight process, we added several methods, such as Sage–Husa noise statistic and fading factors, to the frame of SCKF.

The rest of this paper is organized as follows. An Optimisation-Based initial Alignment (OBA) method based on the Rodrigues parameter for random misalignment angle is introduced in Section 2, where the state model and the measurement equation are used in the simulation. Then, the specific formulation of MSTASCKF is presented in Section 3. In Section 4, a semi-physical simulation is carried out to evaluate the performance of the proposed algorithm under the vibration condition. The alignment results are compared with the conventional SCKF. Finally, the conclusions are drawn in Section 5.

2. Alignment Model for Random Misalignment Angle

In this section, a dynamic alignment model in the form of a random misalignment angle [28] is introduced.

The coordinate systems are defined as follows:

Body frame (b-frame): The origin of the coordinate system is at the center of mass of the carrier, the Y axis is the longitudinal axis of the carrier, the X axis points to the right of the carrier and is perpendicular to the Y axis, and the Z axis and X and Y axes form a right-handed coordinate system.

Navigation frame (n-frame): The origin of the coordinate system is at the center of mass of the carrier, the X axis points to the geographic north direction, the Y axis points to the geographic east direction, and the Z axis points to the up direction.

Inertial navigation frame ($i_n$-frame): An inertial coordinate system, which coincides with the initial navigation frame.

Inertial body frame ($i_b$-frame): The inertial coordinate system, which coincides with the body frame at the initial time.

The attitude matrix can be decomposed into three parts based on the chain rule as follows:

$$C_b^n(\mathrm{t})=C_{in}^n(\mathrm{t})C_{ib}^{in}{C}_b^{ib}(\mathrm{t})$$

(1)

where $C_{in}^n(\mathrm{t})$ $C_b^{ib}(\mathrm{t})$ denotes the time-varying transformation matrix from the $i_n$-frame to the n-frame and the transformation matrix from the b-frame to the $i_b$-frame, respectively. $C_{ib}^{in}$ is a constant transformation matrix from the $i_b$-frame to the $i_n$-frame.

From Equation (1), we can see that the attitude matrix $C_b^n(\mathrm{t})$ can be divided into three parts:

$C_{in}^n(\mathrm{t})$ represents the time-varying attitude due to the rotation of the n-frame caused by the earth rotation rate and the vehicle’s transport rate related to earth, which can be calculated with the change of latitude and the longitude.

$C_b^{ib}(\mathrm{t})$ describes the time-varying attitude due to the rotation of the vehicle from the b-frame to the $i_b$-frame, which can be computed with the angular rate measured by the gyro. It is easy to derive the recursive form:

$$C_b^{ib}(t_k)=C_b^{ib}(t_{k-1})C_{b(t_k)}^{b(t_{k-1})},\hspace{0.17em}{C}_b^{ib}(0)=\mathrm{I}$$

(2)

The $C_{b(t_k)}^{b(t_{k-1})}$ represents the rotation of the b-frame from time $t_{k-1}$ to time $t_k$. It is appropriate to assume the rotation is tiny while the sampling rate of INS is high.

$$C_{b(t_k)}^{b(t_{k-1})}=[\mathrm{I}-(-{\omega }_b(t_k))\times ]$$

(3)

As for the last part, $C_{ib}^{in}$ can be derived by solving the attitude determination problem using the relationship between vectors:

$$V^{in}=C_{ib}^{in}{V}^{ib}$$

(4)

where the $V^{in}$ and $V^{ib}$ are the integration of the specific force in the $i_n$-frame and the $i_b$-frame, respectively. We assume the specific force detected by accelerometer at time $t_k$ in the $i_b$-frame is given by following expression:

$${\widehat{V}}^{ib}(t_k)=V^{ib}(t_k)+\delta V^{ib}(t_k)$$

(5)

where the $\delta V^{ib}(t_k)$ is the integration error of ${\widehat{V}}^{ib}(t_k)$.

Equation (4) could be rewritten as:

$$V^{in}(t_k)=C_{ib}^{in}({\widehat{V}}^{ib}(t_k)-\delta V^{ib}(t_k))$$

(6)

The attitude matrix $C_{ib}^{in}$ can be written in the form of the Rodrigues vector [29] as:

$$C_{ib}^{in}={\left[I+\left(l\times \right)\right]}^{-1}\left[I-\left(l\times \right)\right]$$

(7)

where the $l\times$ is the skew-symmetric matrix of the Rodrigues vector.

Substituting Equation (7) to Equation (6) leads to:

$${\widehat{V}}^{ib}(t_k)-V^{in}(t_k)=({\widehat{V}}^{ib}(t_k)+V^{in}(t_k))\times l+l\times \delta V^{ib}(t_k)+w_{t_k}$$

(8)

where the $w_{t_k}$ is the inertial sensor error.

Defining the sum and difference as follows:

$$D_{t_k}={\widehat{V}}^{ib}(t_k)-V^{in}(t_k),\hspace{0.17em}{S}_{t_k}={\widehat{V}}^{ib}(t_k)+V^{in}(t_k)$$

(9)

We could obtain:

$$D_{t_k}=S_{t_k}\times l+l\times \delta V^{ib}(t_k)+w_{t_k}$$

(10)

Equation (10) is the measurement equation of our filtering model. Once the optimal estimation of the Rodrigues vector $l$ is obtained, we can derive $C_{ib}^{in}$ from Equation (7).

Considering the bias and the random noise of the gyro and the accelerometer, the output of gyro and accelerometer can be derived as:

$${\widehat{\omega }}^b={\omega }^b+{\epsilon }^b+w_g^b$$

(11)

$${\widehat{f}}^b=f^b+{\nabla }^b+w_a^b$$

(12)

Referring to the error model of INS under n-frame, the error equation of the attitude and velocity can be obtained:

$${\dot{\phi }}^{ib}=-{\widehat{C}}_b^{ib}({\epsilon }^b+w_g^b),{\phi }^{ib}(0)=0$$

(13)

$$\delta {\dot{V}}^{ib}=-{\widehat{f}}^{ib}\times {\phi }^{ib}+{\widehat{C}}_b^{ib}({\nabla }^b+w_a^b),\delta {\dot{V}}^{ib}(0)=0$$

(14)

The gradient of the Rodrigues vector is zero, since the attitude matrix $C_{ib}^{in}$ is a constant matrix.

$$\dot{l}=0$$

(15)

The gyroscope drifts ${\epsilon }^b$ and the accelerometer bias ${\nabla }^b$ are irrelevant with time, so we can derive:

$$\begin{array}{l}{\epsilon }^b=0\\ {\nabla }^b=0\end{array}$$

(16)

The state vector was selected as follows:

$$X={\left[\begin{array}{ccccc}l& {\phi }^{ib}& \delta V^{ib}& {\epsilon }^b& {\nabla }^b\end{array}\right]}^T$$

(17)

3. Mstasckf Method

In this section, we demonstrate the algorithm design ideas and the overall process of the algorithm. The algorithm was based on the frame of square-root Kalman filter and the Sage–Husa noise estimator. The aim to our work was to add a criterion to the Sage–Husa noise estimator to ensure the calculated system error variance remains positive, which could make the filter stable. Besides that, a multiple fading factor was implemented to make the residual orthogonal, which could accelerate system convergence and improve the accuracy.

3.1. Sage–Husa Noise Estimator

The Sage–Husa filtering method was originally proposed by P.A Sage and G.W Husa in 1969 [30]. The core idea of the algorithm is to construct a real-time filtered noise statistic based on maximum a posteriori information (MAP) to count the system noise and measurement noise in the filtering process.

For a nonlinear system as shown in (18):

$$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.$$

(18)

where the $w_{k-1}$ and $v_k$ are the system noise and measurement noise, respectively. $f\left(*\right)$ and $h\left(*\right)$ denote the nonlinear state model and measurement model, respectively.

Definite the system’s posterior error variance matrix at the initial moment as:

$$P_0=E\left[\left(x_0-{\overline{x}}_0\right){\left(x_0-{\overline{x}}_0\right)}^T\right]$$

(19)

where the ${\overline{x}}_0$ is the mean value of the initial state vector $x_0$. At time $t_k$, assuming the system noise $q_k$, the measurement noise $r_k$, the system noise covariance matrix $Q_k$, and the measurement noise covariance matrix $R_k$ are unknown. The state quantity set and quantity measurement set from the initial time to k time are, respectively, $X_k=\left[x_0,x_1,\cdots ,x_k\right]$, $Z_k=\left[z_1,z_2,\cdots ,z_k\right]$.

It can be seen from the Bayes formula that the probability of estimating the state vector at time $t_k$, the system noise, the measurement noise, and its noise variance matrix from the measurement sequence from the initial time to time $t_k$ can be expressed as:

$$\begin{array}{l}p\left(X_k,q_k,Q_k,r_k,R_k|Z_k\right)=J\\ =p\left(Z_k|X_k,q_k,Q_k,r_k,R_k\right)p\left(X_k|q_k,Q_k,r_k,R_k\right)\times p\left(Z_k\right)\end{array}$$

(20)

where $p\left(Z_k\right)$ has nothing to do with noise estimation. Then, (20) can be reduced to the problem of solving the maximum value of probability density as follows:

$$p\left(Z_k|X_k,q_k,Q_k,r_k,R_k\right)\times p\left(X_k|q_k,Q_k,r_k,R_k\right)\times p\left(q_k,r_k,Q_k,R_k\right)$$

(21)

Considering that $Cov\left(q_k,r_k\right)=0$ and each measurement can be regarded as independent.

$${\widehat{q}}_k=\frac{1}{k}{\displaystyle \sum _{j=1}^k\left[{\widehat{x}}_{j|k}-f_{j-1}\left(x_{j-1}\right)\right]}$$

(22)

$${\widehat{Q}}_k=\frac{1}{k}{\displaystyle \sum _{j=1}^k\left[{\widehat{x}}_{jk}-f_{j-1}\left(x_{j-1}\right)-q_k\right]}\times {\left[{\widehat{x}}_{jk}-f_{j-1}\left(x_{j-1}\right)-q_k\right]}^{\mathrm{T}}$$

(23)

$${\widehat{r}}_k=\frac{1}{k}{\displaystyle \sum _{j=1}^k\left[z_j-h\left(x_j\right)\right]}$$

(24)

$${\widehat{R}}_k=\frac{1}{k}{\displaystyle \sum _{j=1}^k\left\{\left[z_j-h\left({\widehat{x}}_{j|j-1}\right)-r_k\right]\times {\left[z_j-h\left({\widehat{x}}_j\right)-r_k\right]}^{\mathrm{T}}\right\}}$$

(25)

When the noise changes with time, the filter estimates and measurements at the more recent time can better reflect the current noise situation. Therefore, it is necessary to adjust the weights of the filter estimates and measurements at different times in the noise statistics process to obtain more accurate noise statistics.

Define the weighting coefficient set $\left\{{\lambda }_i\right\}$, ${\lambda }_i={\lambda }_{i-1}b$, $d_k=\frac{1-b}{1-b^k}$, $i=0,1,2,\dots ,k$, b as a constant value between 0 to 1. Hence, we can derive a discrete noise estimator for time-varying noise as:

$${\widehat{q}}_k=\left(1-d_k\right){\widehat{q}}_{k-1}+d_k\left({\widehat{x}}_k-{\mathsf{\Phi }}_{klk-1}{X}_{k-1}\right)$$

(26)

$${\widehat{r}}_k=\left(1-d_k\right){\widehat{r}}_{k-1}+d_k\left(z_k-H_k{\widehat{X}}_{k/k-1}\right)$$

(27)

$${\widehat{R}}_k=\left(1-d_k\right){\widehat{R}}_{k-1}+d_k\left(v_k{v}_k^{\mathrm{T}}-H_k{P}_{k|k-1}{H}_k^{\mathrm{T}}\right)$$

(28)

$${\widehat{Q}}_k=\left(1-d_k\right){\widehat{Q}}_{k-1}+d_k\left(K_k{v}_k{v}_k^{\mathrm{T}}{K}_k^{\mathrm{T}}+P_k-{\mathsf{\Phi }}_{k/k-1}{P}_{k-1}{\mathsf{\Phi }}_{k/k-1}^{\mathrm{T}}\right)$$

(29)

where ${\mathsf{\Phi }}_{klk-1}$ is the one-step prediction matrix from time k − 1 to time k and H_k is the measurement model of the system. $v_k=z_k-H_k{\widehat{X}}_{k|k-1}-r_k$ is the residual of the measurement updating process.

3.2. SCKF with Sage–Husa Noise Estimator

There is a problem when applying the Sage–Husa noise estimator to the SCKF frame. Under the framework of square-root volume Kalman filtering, it is necessary to use the Cholesky decomposition form of the system noise variance matrix and the measurement noise variance matrix to perform filtering iterations during the filtering process [24], which requires us to always obtain positive definite in the noise estimator. Otherwise, the filter will be unstable.

Define the second part of (29) and (28) as:

$$J_q=K_k{v}_k{v}_k^{\mathrm{T}}{K}_k^{\mathrm{T}}+P_k-{\mathsf{\Phi }}_{k/k-1}{P}_{k-1}{\mathsf{\Phi }}_{k/k-1}^{\mathrm{T}}$$

(30)

$$J_r=v_k{v}_k^{\mathrm{T}}-H_k{P}_{k|k-1}{H}_k^{\mathrm{T}}$$

(31)

When the initial system noise covariance is positive, the sign of ${\widehat{Q}}_k$ is only related to the sign of $J_q$, and the same situation occurs for ${\widehat{R}}_k$. To ensure the stability of the filter, we used the equation as follows to compute ${\widehat{Q}}_k$ and ${\widehat{R}}_k$ in the condition $J_q<0$ or $J_r<0$.

$${\widehat{Q}}_k=\left(1-d_k\right){\widehat{Q}}_{k-1}+d_k\left(K_k{v}_k{v}_k^{\mathrm{T}}{K}_k^{\mathrm{T}}+P_k\right)$$

(32)

$${\widehat{R}}_k=\left(1-d_k\right){\widehat{R}}_{k-1}+d_k{v}_k{v}_k^{\mathrm{T}}$$

(33)

Unfortunately, when the covariance matrix is computed with (32) or (33), the result is no longer unbiased. Thus, to make up to the accuracy loss caused by the bias, a multiple fading factor was introduced.

3.3. Multiple Fading Factor Calculation

The multiple fading factors method is based on the orthogonality principle of the residual. The computation process of multiple fading factors under SCKF could be summarized as follows, and readers can refer to the methodology described by the authors of [31] for more analysis and additional derivations.

Definite the multiple fading factor sequence as:

$${\gamma }_k=diag[{\gamma }_k{}^1,{\gamma }_k{}^2,\dots ,{\gamma }_k{}^n]$$

(34)

where n is the dimension of the state vector.

$${\gamma }_k^i=\left\{\begin{array}{ll}{\gamma }_k^{0i},\hfill & {\gamma }_k^{0i}>1\hfill \\ 1,\hfill & {\gamma }_k^{0i}\le 1\hfill \end{array}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}i=1,\cdots ,n\right.$$

(35)

$${\gamma }_k^{0i}=\frac{\mathrm{tr}\left[N_k\right]}{{\displaystyle \sum _{i=1}^n{\alpha }_i}{M}_k^{in}}$$

(36)

$$N_k=V_k-{\left[P_{xz,k|k-1}^{(l)}\right]}^{\mathrm{T}}{\left[{\left(P_{k|k-1}^{(l)}\right)}^{-1}\right]}^{\mathrm{T}}{Q}_{k-1}\left[{\left(P_{k|k-1}^{(l)}\right)}^{-1}\right]\left[P_{xzk|k-1}^{(l)}\right]-R_k$$

(37)

$$M_k=\left(P_{k|k-1}^{(l)}-Q_{k-1}\right)\left[{\left(P_{k|k-1}^{(l)}\right)}^{-1}\right]\left[P_{xz,k|k-1}^{(l)}\right]{\left[P_{xz,k|k-1}^{(l)}\right]}^{\mathrm{T}}{\left[{\left(P_{k|k-1}^{(l)}\right)}^{-1}\right]}^{\mathrm{T}}$$

(38)

3.4. MSTASCKF Update

Step 1: Initialization

$$S_{k-1}=chol\left(P_{k-1}\right)$$

(39)

where $S_{k-1}$ is the lower triangle matrix of the result of the Cholesky decomposition.

Step 2: Prediction

In the prediction step, MSTASCKF computes the mean $x_{k|k-1}$ and $S_{k|k-1}^{\left(l\right)}$, the square-root form of the associated covariance without the correction of the multiple fading factors, using the cubature rule, in which each cubature point has the same weight.

Obtain the cubature points:

$$X_{i,k-1|k-1}=S_{k-1}{\xi }_i+x_{k-1}$$

(40)

Propagate the cubature points through the state model:

$$X_{i,k|k-1}^{*}=f\left(X_{i,k-1|k-1}\right)$$

(41)

Estimate the predicted state:

$$x_{k|k-1}={\displaystyle \sum _{i=1}^m{\omega }_i{X}_{i,k|k-1}^{*}}$$

(42)

$$X_{k|k-1}^{*}={\left(\sqrt{{\omega }_i}\left(X_{i,k|k-1}-x_{k|k-1}\right)\right)}_{i=1}^m$$

(43)

In the proposed algorithm, the weights of the cubature points are equal, which is ${\omega }_i=\frac{1}{n}$.

Square-root factor of the predicted error covariance:

$$S_{k|k-1}^{\left(l\right)}=qr\left(\begin{array}{cc}{X}_{k|k-1}^{*}& S_{Q_{k-1}}\end{array}\right)$$

(44)

where $S_{Q_{k-1}}$ is the square-root of the system noise $Q_{k-1}$ at time k − 1 and $qr\left(\right)$ is the QR decomposition function to obtain the square-root factor of the matrix.

Step 3: Calculating the multiple fading factor

Obtain the cubature points:

$$X_{i,k|k-1}^{(l)}=S_{k|k-1}^{(l)}{\xi }_i+x_{k|k-1}$$

(45)

Propagate the cubature points through measurement model:

$$Z_{i,k|k-1}^{*(l)}=h\left(X_{i,k|k-1}^{(l)}\right)$$

(46)

Obtain the predicted measurement:

$$z_{k|k-1}^{\left(l\right)}={\displaystyle \sum _{i=1}^m{\omega }_i{Z}_{i,k|k-1}^{*\left(l\right)}}$$

(47)

$$Z_{k|k-1}^{*\left(l\right)}={\left(\sqrt{{\omega }_i}\left(Z_{i,k|k-1}^{\left(l\right)}-z_{k|k-1}^{\left(l\right)}\right)\right)}_{i=1}^m$$

(48)

$$S_{zz,k|k-1}^{\left(l\right)}=qr\left(\begin{array}{cc}{Z}_{k|k-1}^{*\left(l\right)}& S_{R_k}\end{array}\right)$$

(49)

$$P_{xz,k|k-1}={\chi }_{k|k-1}^{\left(l\right)}{Z}_{k|k-1}^{*\left(l\right)}{}^T$$

(50)

Obtain the multiple fading factor with (35)–(38) and adjust $S_{k|k-1}$ with multiple fading factors:

$$S_{k|k-1}-\sqrt{{\gamma }_k}{S}_{k|k-1}^{(l)}$$

(51)

The l in the upper right corner of the symbol indicates that this quantity has not been corrected by multiple fading factors.

Step 4: Measurement updating

Create cubature points:

$$X_{i,k|k-1}=S_{k|k-1}{\xi }_i+x_{k|k-1}$$

(52)

Propagate the cubature points through measurement model:

$$Z_{i,k|k-1}^{*}=h\left(X_{i,k|k-1}\right)$$

(53)

$$z_{k|k-1}={\displaystyle \sum _{i=1}^m{\omega }_i{Z}_{i,k|k-1}^{*}}$$

(54)

$$Z_{k|k-1}^{*}={\left(\sqrt{{\omega }_i}\left(Z_{i,k|k-1}-z_{k|k-1}\right)\right)}_{i=1}^m$$

(55)

$$S_{zz,k|k-1}=qr\left(\begin{array}{cc}{Z}_{k|k-1}^{*}& S_{R_k}\end{array}\right)$$

(56)

$${\chi }_{k|k-1}={\left(\sqrt{{\omega }_i}\left(X_{i,k|k-1}-x_{k|k-1}\right)\right)}_{i=1}^m$$

(57)

$$P_{xz,k|k-1}={\chi }_{k|k-1}{Z}_{k|k-1}^{*}{}^T$$

(58)

Obtain the Kalman gain:

$$W_k=P_{xz,k|k-1}/S_{zz,k|k-1}/S_{zz,k|k-1}{}^T$$

(59)

Update the state estimation:

$$x_k=x_{k|k-1}+W_k\left(z_k-z_{k|k-1}\right)$$

(60)

Obtain the square-root of noise covariance estimation:

$$S_k=qr\left(\begin{array}{cc}{\chi }_{k|k-1}-W_k{Z}_{k|k-1}^{*}& W_k{S}_{R_k}\end{array}\right)$$

(61)

Step 5: Estimation of the noise covariance

Obtain $J_q$ and $J_r$.

Compute the system noise covariance and the measurement noise covariance based on the sign of $J_q$ and $J_r$ using (28), (29) or (32), and (33).

4. Semi-Physical Simulation Results and Discussion

In order to assess the performance of the MSTASCKF during the in-flight alignment for GPS/MIMU under vibration, a semi-physical simulation is described in this section. A trajectory maintaining the IMU data and GPS data of typical short-range guided weapon was created, and an experiment to simulate the flight vibration was carried out to obtain simulated vibration noise data. Then, the vibration noise was added to the IMU data to simulate the vibration in-flight. The performance of MSTASCKF was compared with the traditional SCKF, which validated the efficiency of the two strategies added in the MSTASCKF.

4.1. Vibration Experiment

Guided weapons generally experience storage, transportation, and flight environments after their production cycle. During flight, the high-speed turbulent flow field formed around the projectile due to high-speed flight induces complex vibration, overload, high temperatures, and other environments [32]. The vibration environment has a greater impact on the structure of the projectile and may even cause deformation of the projectile or shedding of solder joints on the circuit board. The vibration of guided weapons during flight can be divided into two categories: 0–2000 HZ, low-frequency noise excited by vibration mainly transmitted by mechanical structures, and 10–10,000 HZ noise excited by frequency range. Among them, vibration excitation noise has an obvious effect on the mass-spring structure, and excitation noise mainly has an obvious effect on the plate and shell structure. Therefore, the main focus of this paper was the impact characteristics of the frequency range 0–2000 HZ by vibration excitation noise on the output of the inertial navigation system.

In order to obtain the output data of MIMU under the vibration environment, the following random vibration test was designed. The power spectrum density and frequency range of the vibration are as shown in the Figure 1. The experimental platform is shown in Figure 2, which is composed of a MIMU (MTI-1 from the XSENS company), a vibration table, and the tooling for the vibration table. The specification of the MIMU is listed in

Table 1. Specifications of MTI-1.

Parameters	Accelerometer	Gyroscope
Dynamic range	±16 g	±2000°/s
Update rate	100 Hz	100 Hz
Bias	3 mg	0.15°/s
Bias stability	0.03 mg	10°/h
Random walk	120 μg/√Hz	0.07°/s/√Hz

.

Since the MTI-1 MIMU cannot be sensitive to the Earth’s rotation angular rate, its gyro output under static conditions can be regarded as zero bias and random noise. Vibration noise can be obtained by subtracting the average value of the data at rest when the vibration table is turned on. Thus, the process of the experiment was arranged as in Figure 3 to obtain both the static output and the vibration output of the three axes. The processed vibration noise data in the experiment are shown in Figure 4.

4.2. Simulation Condition

Generally, the flight time of short-range guided weapons is within 60 s, and the flight distance is within 20 km. According to the trajectory characteristics of short-range guided weapons, the trajectory attitude and speed used in the semi-physical simulation are shown in the

Table 2. The specific value of the attitude and velocity used in the trajectory.

Time	Yaw Change	Pitch Change	Roll Change	Speed
1–3 s	5°	0°	0	200 m/s
4–17 s	−30°	−45°	0	135 m/s
18–30 s	25°	−45°	0	50 m/s

. The attitude and the velocity of the trajectory are shown in Figure 5.

The trajectory was set for 30 s, and the initial attitude angle was set as $\psi ={60}^{\circ },\theta ={45}^{\circ },\gamma =0^{\circ }$, representing the yaw, pitch, and roll, respectively. The yaw change was positive from north to east and negative from west to north, the pitch change was positive from the horizontal section of the projectile upward and negative from the downward direction, the hour hand was positive and the counterclockwise was negative, and the projectile speed was the projectile speed at the end of the current time period.

In the simulation, the three-axis gyro data were all added with a 100°/h bias. The speed error of the GPS was 0.1 m/s. Thus, the initial state covariance was set as follows.

$$\left\{\begin{array}{c}{P}_0=P_0\otimes I_3\\ P_0=diag\left(\left[P_l,P_{{\phi }^{i_b}},P_{\delta V^{i_b}},P_{\epsilon },P_{\nabla }\right]\right)\end{array}\right.$$

(62)

where $P_{l_j}={(1)}^2$, $P_{{\phi }_j^{i_b}}={\left(1e-4^{\prime }\right)}^2$, $P_{\delta V_j^{j_b}}={(0.1 \mathrm{m}/\mathrm{s})}^2$, $P_{{\epsilon }_j}={\left({100}^{°}/\mathrm{h}\right)}^2,P_{{\nabla }_j}={(3 \mathrm{mg})}^2$, $R=\mathrm{diag}\left({\left[\begin{array}{lll}0.1\hfill & 0.1\hfill & 0.1\hfill \end{array}\right]}^2\right)$.

4.3. Simulation Results under Vibration Noise

The in-flight alignment progresses were calculated using MSTASCKF and traditional SCKF, respectively. The attitude error is shown in Figure 6. The specific value of the estimated attitude error and its RMS of the proposed method and SCKF is shown in

Table 3. The attitude error and RMSE comparison between MSTASCKF and SCKF.

	MSTASCKF		SCKF
	Attitude Error	RMS	Attitude Error	RMS
Yaw	0.08°	1.13°	−2.85°	2.5°
Pitch	0.05°	0.14°	0.96°	0.99°
Roll	0.25°	5.35°	1.66°	6.12°

.

The figure shows that the MSTASCKF has an advantage in convergence speed in the alignment process. The estimation of yaw, pitch, and roll converged within 15 s and finally reached the accuracy of 0.08°, 0.05°, and 0.25°, respectively. The SCKF had a worse performance, which can be easily observed as it converged slower and only met the accuracy of −2.85°, 0.95°, 1.65° for the yaw, pitch, and roll under the vibration noise.

To further compare the stability of the MSTASCKF and SCKF, the RMS of the attitude is shown in the Figure 7.

We can see that the proposed method had a better RMS performance than the SCKF. The RMS values of the three attitudes of MSTASCKF and SCKF at the end of the alignment process were $\left[\begin{array}{ccc}{1.13}^{\circ }& {0.14}^{\circ }& {5.35}^{\circ }\end{array}\right]$ and $\left[\begin{array}{ccc}{2.5}^{\circ }& {0.99}^{\circ }& {6.12}^{\circ }\end{array}\right]$, respectively. The RMS of the yaw and roll was much larger than the pitch. This was caused by the estimation scheme, since the MSTASCKF and SCKF had the same trend. The first estimation of roll and pitch had such a large deviation that the subsequent RMS value was affected.

In fact, the main metrics of alignment performance was the final accuracy at the end of the alignment process rather than the accuracy during the process.

The attitude error is shown in the

Table 4. Thea attitude error comparison between MSTASCKF and SCKF when the alignment time was 10 s and 20 s.

	Attitude Error
	MSTASCKF		SCKF
Time	10 s	20 s	10 s	20 s
Yaw	0.29°	0.15°	−1.60°	−2.73°
Pitch	−0.02°	−0.08°	0.82°	1.25°
Roll	0.32°	0.19°	2.89°	2.05°

when the alignment time was limited to 10 s and 20 s. These data could also show that the MSTASCKF had an advantage in the convergence speed and the estimation stability.

4.4. Discussion

In this section, a semi-physical simulation for in-flight alignment under vibration noise was carried out to evaluate the performance of the proposed algorithm. Compared with conventional SCKF, it is apparent that the proposed method had a better performance not only in the convergence speed in the beginning of the process but also in the accuracy and steady state. For the proposed filter, the results are as follows.

We proposed a robust SCKF algorithm, which could significantly increase the in-flight alignment accuracy under the vibration noise.

Compared with conventional SCKF filter, the estimation performance was better. The attitude error was smaller and the RMS was also smaller. The convergence time of the proposed method was no longer than 10 s. Thus, the proposed method can meet the rapidity and accuracy requirements of in-flight alignment.

5. Conclusions

The rapid and accurate in-flight alignment has a vital impact on the performance of the guided weapon applied MIMU. To improve the in-flight alignment performance of MIMU/GPS system under flight vibration, this paper proposed a MSTASCKF filter based on the Sage–Husa noise estimator and multiple fading factors. The filter solved the instability problem of SCKF caused by the Sage–Husa estimator and improved the accuracy. The main work of this paper was as follows.

An experimental vibration environmental test was conducted to maintain the simulated vibration noise data during the flight and a semi-physical simulation was carried out to evaluate the performance of the proposed algorithm. The filtering method had an advantage in the convergence speed, accuracy, and stability compared to SCKF.