An Alignment Method Based on KF-ASMUKF Hybrid Filtering for Ship’s SINS under Mooring Conditions

Abstract

To solve the problem that the ship’s strapdown inertial navigation system (SINS) alignment accuracy decreases with the increase of the nonlinear filtering state dimension under mooring conditions, a method based on Kalman filter (KF) and Adaptive scale mini-skewness single line sampling Unscented Kalman Filter (ASMUKF) hybrid filtering algorithm is proposed in this paper. Three improvements are made as the following: (1) adopt a new sampling strategy. To obtain the ASMUKF filtering algorithm, scale mini-skewness single line sampling is used to replaced the traditional symmetrical sampling method and an adaptive scale factor is adapted into the Unscented Kalman Filter (UKF) to correct the real-time transformation sampling process; (2) the improved ASMUKF algorithm is combined with KF to form KF-ASMUKF hybrid filtering model; (3) the hybrid filtering model is divided into linear and nonlinear parts. The linear filtering part adopts the KF filtering model and the nonlinear filtering part adopts the ASMUKF model. Then, the calculation steps of the hybrid filtering algorithm is designed in this paper. The simulation and experimental results show that the hybrid filtering algorithm proposed has certain advantages over the traditional algorithm, and it can reduce the ship’s SINS calculation amount and improve alignment accuracy under mooring conditions.

1. Introduction

Strapdown inertial navigation system (SINS) has the advantages of not relying on external information and good autonomy. It is one of the most important navigation methods for maritime carriers [1,2,3]. But, the error of SINS continues to expand over time, and it must be aided by other navigation methods for a long time to navigate. Thus, SINS is used to an integrated navigation system with other auxiliary equipment [4,5,6]. With the wide application of strapdown inertial navigation system on ships, the initial alignment of the strapdown inertial navigation systems plays an important role in the maneuverability of ships. For ship alignment, auxiliary external information should be used, such as a reference for position and speed information given by the global navigation satellite system [7,8,9,10]. The ship’s strapdown inertial navigation system (SINS) must complete initial alignment before entering navigation. About the problem of initial alignment, how to complete the initial alignment quickly and well under mooring conditions is the key to ensuring accurate navigation of the ship [11,12,13,14].

At present, the error propagation model and filtering algorithm are two important issues for studying the initial alignment of strapdown inertial navigation system [15]. The SINS has a large angular motion under mooring conditions. The interference angular velocity caused by the swing is much larger than the rotation angular velocity of the earth, which makes it impossible to extract the angular velocity component of the earth rotation from the gyro output, so the error model of the ship’s SINS is nonlinear [16,17,18,19]. In [20], Sun et al. proposed an inertial system initial alignment algorithm based on hidden Markov model-Kalman filter (HMM-KF). The method can suppress the high-frequency random disturbance of the initial alignment of the ship under the mooring condition and improve the azimuth accuracy under mooring condition, but the method has low alignment accuracy for large misalignment angles. In [21], Zhao et al. proposed a second-order extended Kalman filter algorithm based on second-order nonlinear measurement, which can perform initial alignment under arbitrary posture conditions. Although the method improves the alignment accuracy, it also has a large amount of calculation. In [22], Wang et al. used Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) for initial alignment with large misalignment angles. The results show that in the case of large misalignment angles, Unscented Kalman Filter (UKF) has higher estimation accuracy than Extended Kalman Filter (EKF). But the alignment accuracy is not high in a complex environment. In [23], Gao et al. proposed an improved ACKF-KF method, which can complete the initial alignment well in the case of unknown measurement noise. But the method does not work well for the alignment of ships at sea. In [24], Yan et al. proposed an alignment algorithm based on Gauss-Hermite filter (Gauss-Hermite filter) ship strapdown inertial navigation system inertial system, without coarse alignment to quickly achieve the initial alignment of Strapdown Inertial Navigation System (SINS). However, it is difficult to determine the optimal filtering parameters of the method, which affect the alignment accuracy. In [25], Pei et al. proposed a fast initial self-alignment method for ships based on the Constrained Matrix Kalman filter (CMKF). This method can shorten the alignment time while ensuring accuracy. But the method is not effective under the condition of large misalignment angles. In [26], Rong et al. proposed an adaptive filtering ship SINS mooring alignment algorithm based on Complementary Ensemble Empirical Mode Decomposition (CEEMD), which eliminates the effect of sensor measurement noise on accuracy and improves alignment accuracy. But the amount of calculation is relatively large.

In this paper, a hybrid filter algorithm based on KF-ASMUKF is proposed. It is to solve the problem that the initial alignment accuracy of SINS decreases and the amount of calculation complexity caused by the increase of the nonlinear filter state dimension. Firstly, this paper adopts a new sampling strategy. It uses scale mini-skewness single line sampling instead of a traditional symmetrical sample. Meanwhile, introduce an adaptive scale factor into the UKF algorithm to get the ASMUKF filtering algorithm. Secondly, the improved ASMUKF filter algorithm and KF are combined to form a KF-ASMUKF hybrid filter model. Then, the hybrid filter model is decomposed into linear and nonlinear parts. The linear filter part adopts the KF filter model, the nonlinear filter part adopts the ASMUKF filter model, and the calculation steps of the hybrid filter algorithm are designed. Finally, the simulated rocking test and the mooring test are designed to verify the method’s effectiveness.

The rest of this paper is organized as follows. Section 2 defines the standard inertial coordinate system. Section 3 introduces the error model of the ship’s large misalignment strapdown inertial navigation system. Section 4 designs the improved UKF algorithm and constructs an alignment model based on the KF-ASMUKF hybrid filter algorithm and its steps. Section 5 designs simulation and offshore alignment test to verify the effectiveness of the method. Section 6 gives the conclusion.

2. Description of Common Coordinate System

The different reference coordinate used in this article are shown in Figure 1, and their respective definitions are as follows:

Geocentric Inertial Coordinate System ($o{x}_i{y}_i{z}_i$.): the origin o is the center of the earth, $o{z}_i$ point to the rotation axis of the earth, $o{x}_i$ and $o{y}_i$ form a right-handed coordinate system with $o{z}_i$ in the equatorial plane.

Earth coordinate system ($o{x}_e{y}_e{z}_e$): the origin o is the center of the earth, $o{z}_e$ is along the axis of the earth, $o{x}_e$ and $o{y}_e$ are on the intersection of the equatorial plane with the prime meridian plane, forming a right-handed coordinate system with $o{z}_e$.

Navigation coordinate system ($o{x}_n{y}_n{z}_n$): the origin o is the center of the carrier, $o{z}_n$ points upward along the local geographic vertical line, $o{x}_n$ points east along the local latitude and $o{y}_n$ points north along the local meridian.

Carrier coordinate system ($o{x}_b{y}_b{z}_b$): the origin o is the center of the carrier, $o{x}_b$ points to the right along the horizontal axis of the carrier, $o{y}_b$ points forward along the longitudinal axis of the carrier, $o{z}_b$ points up along the vertical axis of the carrier.

3. SINS Error Model with Large Misalignment

Select the east north up (ENU) as the navigation coordinate system of the ship’s strapdown navigation system, and set its attitude differential equation at time t as:

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

(1)

${\omega }_{nb}^b(t)$ is the projection of the angular velocity of the carrier system relative to the navigation system on the system at time t. ${\omega }_{nb}^b\times$ is the antisymmetric matrix of ${\omega }_{nb}^b$, $C_n^b(t)$ is the attitude equation of the ship carrier coordinate system relative to the navigation system at time t. According to the chain rule, the attitude equation can be decomposed into the following parts:

$$C_b^n(t)=C_{n^{\prime }}^n(t)C_b^{n^{\prime }}(t)$$

(2)

Due to the existence of various errors, there is an error angle between the calculated navigation system $n^{\prime }$ of the ship carrier and the real navigation system n, which can be expressed as:

$$\phi =\left[\begin{array}{ccc}{\phi }_E& {\phi }_N& {\phi }_U\end{array}\right]$$

(3)

The n coordinate system can get the $n^{\prime }$ coordinate system by rotating three times around the ENU in three directions, and its rotation matrix can be expressed as:

$$C_n^{n^{\prime }}=\left[\begin{array}{ccc}\begin{array}{c}cos{\phi }_{N}cos{\phi }_U\hfill \\ -sin{\phi }_{N}sin{\phi }_{E}sin{\phi }_U\hfill \end{array}& \begin{array}{c}cos{\phi }_{N}sin{\phi }_U\hfill \\ +sin{\phi }_{N}sin{\phi }_{E}cos{\phi }_U\hfill \end{array}& -sin{\phi }_{N}cos{\phi }_E\\ -cos{\phi }_{E}sin{\phi }_U& cos{\phi }_{E}cos{\phi }_U& sin{\phi }_E\\ \begin{array}{c}sin{\phi }_{N}cos{\phi }_U\hfill \\ +cos{\phi }_{N}sin{\phi }_{E}sin{\phi }_U\hfill \end{array}& \begin{array}{c}sin{\phi }_{N}sin{\phi }_U\hfill \\ -cos{\phi }_{N}sin{\phi }_{E}cos{\phi }_U\hfill \end{array}& cos{\phi }_{N}cos{\phi }_E\end{array}\right]$$

(4)

$C_n^{n^{\prime }}(t)$ can be obtained according to the relationship between the equivalent rotation vector and the direction matrix:

$$C_n^{n^{\prime }}(t)={(C_{n^{\prime }}^n(t))}^T\approx {\left\{[I+(\phi \times )](t)\right\}}^T=[I-(\phi \times )](t)$$

(5)

In Formula (5), I is the identity matrix, $\phi \times$ is the antisymmetric matrix of $\phi$.

Formula (1) can be further decomposed into:

$$\begin{array}{c}{\dot{C}}_b^n(t)=C_b^n(t)({\omega }_{nb}^b\times )(t)\hfill \\ =C_b^n(t)[({\omega }_{ib}^b-{\omega }_{in}^b)\times ](t)\hfill \\ =C_b^n(t)({\omega }_{ib}^b\times )(t)-C_b^n(t)({\omega }_{in}^b\times )(t)C_b^n(t)C_n^b(t)\hfill \\ =C_b^n(t)({\omega }_{ib}^b\times )(t)-({\omega }_{in}^n\times )(t)C_b^n(t)\hfill \end{array}$$

(6)

${\omega }_{ib}^b$ is the angular velocity information output by the gyro. ${\omega }_{ib}^b\times$ is its antisymmetric matrix.

$${\omega }_{in}^n(t)={\omega }_{ie}^n(t)+{\omega }_{en}^n(t)$$

(7)

$${\omega }_{ie}^n(t)={\left[\begin{array}{ccc}0\hfill & {\omega }_{ie}(t)cosL\hfill & {\omega }_{ie}(t)sinL\hfill \end{array}\right]}^T$$

(8)

$${\omega }_{en}^n(t)={[\begin{array}{ccc}\frac{V_N(t)}{R_M+h}& \frac{V_E(t)}{R_N+h}& \frac{V_E(t)}{R_N+h}\end{array}tanL]}^T$$

(9)

In Formula (9), $V_N$ is the northerly velocity of the carrier in motion. $V_E$ is the easterly velocity. $R_M$ is the radius of curvature of the meridian circle. $R_N$ is the radius of curvature of the earth’s unitary rotation. h is the geographic height, and L is the geographic latitude.

Due to the existence of various errors in actual measurement, Formula (6) can be expressed as:

$${\dot{C}}_b^{n^{\prime }}(t)=C_b^{n^{\prime }}(t)({\tilde{\omega }}_{ib}^b\times )(t)-({\tilde{\omega }}_{in}^n\times )(t)C_b^{n^{\prime }}(t)$$

(10)

In Formula (10), ${\tilde{\omega }}_{ib}^b(t)={\omega }_{ib}^b(t)+\delta {\omega }_{ib}^b(t)$. ${\tilde{\omega }}_{in}^n(t)={\omega }_{in}^n(t)+\delta {\omega }_{in}^n(t)$. $\delta {\omega }_{ib}^b(t)$ is the gyroscope measurement error at time t. $\delta {\omega }_{ib}^b(t)={\epsilon }^b$. $\delta {\omega }_{in}^n(t)$ is the calculation error of the navigation system at time t.

$$\delta {\omega }_{in}^n(t)=\delta {\omega }_{ie}^n(t)+\delta {\omega }_{en}^n(t)$$

(11)

$$\delta {\omega }_{ie}^n(t)={\left[\begin{array}{ccc}0& -{\omega }_{ie}^n(t)sinL\delta L& -{\omega }_{ie}^n(t)cosL\delta L\end{array}\right]}^T$$

(12)

$$\delta {\omega }_{en}^n(t)=\left[\begin{array}{c}-\frac{\delta V_N(t)}{R_M+h}+\frac{\delta h{V}_N(t)}{{(R_M+h)}^2}\\ \frac{\delta V_E(t)}{R_N+h}-\frac{\delta h{V}_E(t)}{{(R_N+h)}^2}\\ \delta L({\omega }_{ie}(t)cosL+\frac{V_E(t){sec}^{2}L}{R_N+h})+\frac{\delta V_E(t)}{R_N+h}tanL-\frac{\delta h{V}_E(t)tanL}{{(R_N+h)}^2}\end{array}\right]$$

(13)

Differentiate Formula (5) to get:

$$(-\dot{\phi }\times )(t)C_b^n(t)+(I-\phi \times )(t){\dot{C}}_b^n(t)=C_b^{n^{\prime }}(t)({\tilde{\omega }}_{ib}^b\times )(t)-({\tilde{\omega }}_{in}^n\times )(t)C_b^{n^{\prime }}(t)$$

(14)

Substituting Formula (2) and Formula (6) into Formula (14) can be obtained:

$$\begin{array}{c}(-\dot{\phi }\times )(t)C_b^n(t)+(I-\phi \times )(t){\dot{C}}_b^n(t)\hfill \\ =C_b^{n^{\prime }}(t)({\tilde{\omega }}_{ib}^b\times )(t)-({\tilde{\omega }}_{in}^n\times )(t)C_b^{n^{\prime }}(t)\hfill \\ =(I-\phi \times )(t)C_b^n(t)[({\omega }_{ib}^b+\delta {\omega }_{ib}^b)\times ](t)\hfill \\ -[({\omega }_{in}^n+\delta {\omega }_{in}^n)\times ](t)(I-\phi \times )(t)C_b^n(t)\hfill \end{array}$$

(15)

Multiply both sides of Formula (12) to the right by two at the same time. Ignoring the small second-order amount in the equation to get:

$$\begin{array}{c}(\dot{\phi }\times )(t)=[(\phi \times {\omega }_{in}^n)\times ](t)+(\delta {\omega }_{in}^n\times )(t)-(\delta {\omega }_{ib}^b\times )(t)\hfill \\ =[(\phi \times {\omega }_{in}^n+\delta {\omega }_{in}^n-\delta {\omega }_{ib}^b)\times ](t)\hfill \end{array}$$

(16)

The differential equation of the carrier error angle on the navigation system n can be expressed as:

$$\dot{\phi }(t)=(I-C_n^{n^{\prime }}){\omega }_{in}^n(t)+C_b^n\delta {\omega }_{in}^b(t)-C_b^n\delta {\omega }_{ib}^b(t)$$

(17)

By Formula (17) combined with Formulas (7)–(9), the attitude error component of the carrier in the (ENU) direction can be obtained:

$$\begin{array}{c}{\dot{\phi }}_E(t)=({\omega }_{ie}(t)sinL+\frac{V_E(t)tanL}{R_N+h}){\phi }_N\hfill \\ -({\omega }_{ie}(t)cosL+\frac{V_E(t)}{R_N+h}){\phi }_U-\frac{\delta V_N(t)}{R_M+h}+\frac{\delta h{V}_N(t)}{{(R_M+h)}^2}-{\epsilon }_E\hfill \end{array}$$

(18)

$$\begin{array}{c}{\dot{\phi }}_N(t)=-({\omega }_{ie}(t)sinL+\frac{V_E(t)tanL}{R_N+h}){\phi }_E-\frac{V_N(t){\phi }_U}{R_M+h}\hfill \\ -\delta L{\omega }_{ie}(t)sinL+\frac{\delta V_E(t)}{R_N+h}-\frac{\delta h{V}_E(t)}{{(R_N+h)}^2}-{\epsilon }_N\hfill \end{array}$$

(19)

$$\begin{array}{c}{\dot{\phi }}_U(t)=({\omega }_{ie}(t)cosL+\frac{V_E(t)}{R_N+h}){\phi }_E+\frac{V_N(t){\phi }_N}{R_M+h}\hfill \\ +\delta L({\omega }_{ie}(t)cosL+\frac{V_E(t){sec}^{2}L}{R_N+h})+\frac{\delta V_E(t)tanL}{R_N+h}-\frac{\delta h{V}_E(t)tanL}{{(R_N+h)}^2}-{\epsilon }_E\hfill \end{array}$$

(20)

The velocity equation of the carrier in the navigation system can be expressed as:

$${\dot{V}}^n(t)=C_b^n(t)f^b(t)-(2{\omega }_{ie}^n(t)+{\omega }_{en}^n(t))V_{en}^n(t)-g^n(t)$$

(21)

In the actual calculation process, due to various errors, the speed equation can be expressed as:

$${\tilde{\dot{V}}}^n(t)=C_b^{n^{\prime }}(t){\tilde{f}}^b(t)-\left(2{\tilde{\omega }}_{ie}^n(t)+{\tilde{\omega }}_{en}^n(t)\right){\tilde{V}}_{en}^n(t)-{\tilde{g}}^n(t)$$

(22)

The velocity error equation can be obtained by Formulas (21) and (22):

$$\begin{array}{cc}\hfill \delta {\dot{V}}^n(t)& ={\tilde{\dot{V}}}^n(t)-{\dot{V}}^n(t)\hfill \\ \hfill & =\left(C_b^{n^{\prime }}(t){\tilde{f}}^b(t)-C_b^n(t)f^b(\mathrm{t})\right)-\left[\left(2{\tilde{\omega }}_{ie}^n(t)+{\tilde{\omega }}_{en}^n(t)\right)\times {\tilde{\dot{V}}}^n(t)\right.\hfill \\ \hfill & \left.-\left(2{\omega }_{ie}^n(t)+{\omega }_{en}^n(t)\right)\times {\dot{V}}^n(t)\right]+\left({\tilde{g}}^n(t)-g^n(t)\right)\hfill \end{array}$$

(23)

In Formula (23), ${\tilde{f}}^b=f^b+\delta f^b$, ${\tilde{g}}^n=g^n+\delta g^n$, substituting Formulas (2) and (5) into Formula (23) and ignoring the second-order small quantities, we can further obtain:

$$\begin{array}{c}\delta {\dot{V}}^n(t)=[(I-\phi \times )C_b^n(t)(f^b(t)+\delta f^b(t))-C_b^n(t)f^b(t)]\hfill \\ -\{[2({\omega }_{ie}^n(t)+\delta {\omega }_{ie}^n(t))+({\omega }_{en}^n(t)+\delta {\omega }_{ie}^n(t))]\times (V^n(t)+\delta V^n(t))\hfill \\ -(2{\omega }_{ie}^n(t)+{\omega }_{en}^n(t))\times V^n(t)+\delta g^n(t)\}\hfill \\ \approx f^n(t)\times \phi +V^n(t)\times (2\delta {\omega }_{ie}^n(t)+\delta {\omega }_{en}^n(t))\hfill \\ -(2{\omega }_{ie}^n(t)+{\omega }_{en}^n(t))\times \delta V^n(t)+\delta f^b(t)+\delta g^n(t)\hfill \end{array}$$

(24)

In Formula (24), $\delta f^b$ is the accelerometer’s zero bias error. $\delta f^b={\nabla }^b$, $\delta g^n$ is the gravity acceleration deviation. $\delta g^n\approx 0$, from the Formula (24), we can get the speed error component of the carrier in the (EN) direction at time t:

$$\begin{array}{c}\delta {\dot{V}}_E(t)={\phi }_U{f}_N(t)-{\phi }_N{f}_U(t)+\frac{\delta V_E(t)(V_N(t)tanL-V_U(t))}{R_N+h}+\delta V_N(t)(2{\omega }_{ie}(t)sinL\hfill \\ +\frac{V_E(t)tanL}{R_N+h})-\delta V_U(t)(2{\omega }_{ie}(t)cosL+\frac{V_E(t)}{R_N+h})+\delta L(t)[2{\omega }_{ie}(t)(V_U(t)sinL\hfill \\ +V_N(t)cosL)+\frac{V_E(t)V_N(t)}{R_N+h}]+\delta h(t)\frac{V_E(t)V_U(t)-V_E(t)V_N(t)tanL}{R_N+h}+{\nabla }_E\hfill \end{array}$$

(25)

$$\begin{array}{c}\delta {\dot{V}}_N(t)=-{\phi }_U{f}_E(t)+{\phi }_E{f}_U(t)-2\delta V_E(t)({\omega }_{ie}(t)sinL+\frac{V_E(t)tanL}{R_N+h})\hfill \\ -\frac{\delta V_N(t)V_U(t)}{R_M+h}-\frac{\delta V_U(t)V_N(t)}{R_M+h}-\delta L(t)(2V_E(t){\omega }_{ie}(t)cosL\hfill \\ +\frac{{V_E}^2(t){sec}^{2}L}{R_N+h})+\delta h(t)[\frac{V_N(t)V_U(t)}{{(R_M+h)}^2}+\frac{{V_E}^2(t)tanL}{{(R_N+h)}^2}]+{\nabla }_N\hfill \end{array}$$

(26)

4. KF-ASMUKF Hybrid Filtering Algorithm

4.1. ASMUKF Filtering Algorithm

The traditional UKF algorithm mainly processes the statistics of the state through unscented transformation (UT), and obtains a set of sigma points according to the statistical characteristics of the state variables that need to be predicted. Then calculates the statistical factors of the sampling points after these nonlinear transformations, get the mean and covariance estimates [18]. But the traditional UKF has the problem of nonlocal sampling. This article adopts a new sampling strategy to obtain new sigma points by scaling them to solve the nonlocal sampling problem, suppress filter divergence and improve its accuracy.

The state estimation model for the nonlinear system can be expressed as:

$$\begin{array}{c}{x}_t=f(x_{t-1})+Q_{t-1}\hfill \\ y_t=h(x_{t-1})+R_{t-1}\hfill \end{array}$$

(27)

In Formula (27), $x_t$ and $y_t$ represent state quantity and quantity measurement respectively. $Q_{t-1}$ and $R_{t-1}$ represent system noise and measurement noise, respectively.

New sigma sampling method:

$$l_i=\left\{\begin{array}{cc}\overline{x}& i=0\\ \overline{x}-(\sqrt{P_x})l_i& i=1,\cdots ,n\\ \overline{x}+(\sqrt{P_x})l_i& i=1,\cdots ,2n\end{array}\right.$$

(28)

The weights when calculating the mean and variance are:

$$\left\{\begin{array}{cc}{W}_i^m=\frac{W_0}{{\alpha }^2}+(1-\frac{1}{{\alpha }^2}),& i=0\\ W_i^m=\frac{1-W_0}{2^n{\alpha }^2},& i=1,2\\ W_i^m=\frac{2^{i-2}-W_1}{{\alpha }^2},& i=3,4,\cdots ,n\end{array}\right.$$

(29)

$$\left\{\begin{array}{c}{W}_i^c=W_i^m+(1-{\alpha }^2+\beta ),i=0\\ W_i^c=W_1^m,i\ne 0\end{array}\right.$$

(30)

In the Formula (30), $\alpha$ is the scale parameter, the value range is generally between [0, 1]. $W_i$ is the weight of the i sampling point in the minimum skewness simplex sampling. $W_i^m$ and $W_i^c$ are the sampling, respectively The weighted value used for the mean and covariance in the transformation.

(1) Time update

According to Formula (28), all sampling points can be passed:

$${\zeta }_{t,i}=f(l_{t-1,i}),i=1,2,...n$$

(31)

State prediction and its covariance can be expressed as:

$${\widehat{x}}_{t/t-1}=\sum _{i=0}^{2n}{W}_i^m{\zeta }_{t-1}$$

(32)

$$P_{x,t/t-1}=\sum _{i=0}^{2n}{W}_i^c({\zeta }_{t-1}-{\widehat{x}}_{t/t-1}){({\zeta }_{t-1}-{\widehat{x}}_{t/t-1})}^T+Q_{t-1}$$

(33)

(2) Measurement update

According to Formula (31), it can be concluded that all sampling points are transmitted during the measurement update process:

$${\zeta }_i=h(l_i),i=1,2,...2n$$

(34)

The mean, variance, and covariance of the measured information after transmission can be expressed as:

$${\widehat{y}}_t=\sum _{i=0}^{2n}{W}_i^m{\zeta }_{t,i}$$

(35)

$$P_{y,t}=\sum _{i=0}^{2n}{W}_i^c({\zeta }_{t,i}-{\widehat{y}}_t){({\zeta }_{t-1,i}-{\widehat{y}}_t)}^T+R_t$$

(36)

$$P_{xy,t}=\sum _{i=0}^{2n}{W}_i^c({\zeta }_{t,i}-{\widehat{x}}_{t/t-1}){({\zeta }_{t,i}-{\widehat{y}}_t)}^T$$

(37)

Then, the mean value and variance of the state quantity at time t can be expressed as:

$$K_t=P_{xy,t}{(P_{y,t})}^{-1}$$

(38)

$${\widehat{x}}_{t/t}={\widehat{x}}_{t/t-1}+K_t(y_t-{\widehat{y}}_t)$$

(39)

$$P_{x,t/t}=P_{x,t/(t-1)}-K_t{P}_{y,t}{(K_t)}^T$$

(40)

The system estimate can be obtained as its variance, then:

$$P_{t/t}=E[(x_{t/t}-{\widehat{x}}_{t/t}){(x_{t/t}-{\widehat{x}}_{t/t})}^T]$$

(41)

Formula (41) shows that the distance between $x_{t/t}$ and ${\widehat{x}}_{t/t}$. When the mini-skewness single-line sampling transformation is performed for the n time, the distance ${\widehat{x}}_{t/t}$ from the sigma sampling point to the center point should satisfy $d_n\le d_t$:

$$\left\{\begin{array}{c}{\alpha }_{t+1/t}{d}_{max}=d_{t/t}\\ d_{max}=max\left\{d_i\right\},i\in \mathbb{R}\\ d_i=\sqrt{{(l_i)}^T{U}^{T}U{l}_i}\end{array}\right.$$

(42)

In Formula (42), U is the number of sampling points. $\mathbb{R}$ is the index set of sampling points. According to Formula (42), the scale correction coefficient of the minimum skewness single-line sampling transformation can be calculated ${\alpha }_{t+1/t}$. $\alpha$ is corrected every time the mini-skewness single-line sampling transformation is performed. Finally, the ASMUKF algorithm can be obtained. Based on the above analysis and derivation, the flow of the ASMUKF algorithm can be derived, as shown in

Table 1. Algorithm flow based on ASMUKF.

Algorithm: ASMUKF filtering algorithm

Input:$x_0$, R, Q

Output:$x_{t/t}$

(1) Initialization: ${\widehat{x}}_0$, ${\widehat{x}}_0=E\left[x_0\right]$, $P_0=E\left[\left(x_0-{\widehat{x}}_0\right){\left(x_0-{\widehat{x}}_0\right)}^T\right]$

(2) For $t\in [1,n]$

(3) Calculate Sigma points and the weights of the mean and variance: $l_i$, $W_i^m$, $W_i^c$

(4) Time update: ${\zeta }_{t,i}=f\left(l_{t-1,i}\right)$, ${\widehat{x}}_{t/t-1}$, $P_{x,t/t-1}$

(5) Measurement update: ${\widehat{y}}_t$, $P_{y,t}$, $P_{xy,t}$

(6) Filter update: $K_t$, ${\widehat{x}}_{t/t}$, $P_{x,t/t}$

(7) Calculate filter update sampling correction coefficient: ${\alpha }_{t+1/t}$, ${\alpha }_{t+1/t}=d_{t/t}/d_{max}$. Return to step (2)

(8) Return: $x_{t/t}\leftarrow {\widehat{x}}_{t/t}$

(9) End for

:

4.2. Mooring Alignment Model Based on KF-ASMUKF Hybrid filter

Research the ship’s mooring alignment and establish the East-North-UP(ENU) space coordinate system. Then the 11-dimensional linear state vector is:

$$X(t)={[\begin{array}{cccc}\delta V& \phi & {\epsilon }^b& {\nabla }^b\end{array}]}^T$$

(43)

Combining Equations (17) and (24) can get the error equation:

$$\left\{\begin{array}{c}\dot{X}(t)=f[X(t),t]+G(t)w(t)\\ Z(t)=H(t)X(t)+v(t)\end{array}\right.$$

(44)

In Formula (44), $f[X(t),t]$ is a nonlinear state function:

$$f[X(t),t]=\left[\begin{array}{c}(I-C_n^{n^{\prime }}){\omega }_{in}^n(t)+C_b^n\delta {\omega }_{in}^b(t)-C_b^n\delta {\omega }_{ib}^b(t)\\ \begin{array}{c}{f}^n(t)\times \phi +V^n(t)\times (2\delta {\omega }_{ie}^n(t)+\delta {\omega }_{en}^n(t))\hfill \\ -(2{\omega }_{ie}^n(t)+{\omega }_{en}^n(t))\times \delta V^n(t)+\delta f^b(t)+\delta g^n(t)\hfill \end{array}\\ 0_{6\times 1}\end{array}\right]$$

(45)

$G(t)$ is the control matrix of noise, $w(t)$ is the noise matrix, where:

$$G(t)=\left[\begin{array}{cc}{C}_b^{n^{\prime }}& 0_{3\times 3}\\ 0_{3\times 3}& -C_b^{n^{\prime }}\\ 0_{5\times 3}& 0_{5\times 3}\end{array}\right]w(t)={[\begin{array}{cc}{\epsilon }_w^b& {\nabla }_w^b\end{array}]}^T$$

(46)

Observed measurement is the speed error of GPS and SINS. $H(t)=\left[{\begin{array}{cc}{I}_{3\times 3}& 0\end{array}}_{3\times 8}\right]$. $v(t)$ is the measurement noise vector

Formulas (44) and (45) show that in the nonlinear filter equations of the system, only the attitude error angle is in the nonlinear form, while the velocity error and position error appear in the system equation in linear form. Therefore, the model decomposition method can be used to decompose the filter model into a nonlinear part and a linear part. For the nonlinear part, the ASMUKF filtering model is used, and for the linear part, the KF filtering model is used. It can ensure accuracy and reduce the calculation amount of the filtering algorithm. It can also further improve the real-time performance of the algorithm.

Select the linear part state vector $X^{lin}(t)={\left[\begin{array}{ccc}\delta v^n& {\epsilon }^b& {\nabla }^b\end{array}\right]}^T$. Using SINS to calculate the difference between the speed and the GPS output speed under the condition of static base as the measurement of the linear part. Then, the filter model after linear part discretization:

$$\left\{\begin{array}{c}{X}^{lin}(t+1)=F^{lin}(t)X^{lin}(t)+{\Xi }^{lin}{X}^{non}(t)+G^{lin}(t)w^{lin}(t)\\ Z^{lin}(t+1)=H^{lin}(t+1)X^{lin}(t+1)+v^{lin}(t+1)\end{array}\right.$$

(47)

In Formula (47), $F^{lin}(t)$ is the transition matrix of the linear part, $F^{lin}(t)=\left[\begin{array}{cc}{0}_{3\times 6}& C_b^{n^{\prime }}\\ 0_{6\times 3}& 0_{6\times 3}\end{array}\right]$. $\Xi (t)$ is the input control matrix, ${\Xi }^{lin}={\left[\begin{array}{cc}{C}_{n^{\prime }}^n{g}^n& 0_{1\times 6}\end{array}\right]}^T$. $G^{lin}(t)$ is the linear input noise matrix, $G^{lin}(t)={\left[\begin{array}{cc}{C}_b^{n^{\prime }}& 0_{6\times 3}\end{array}\right]}^T$. $H^{lin}(t)$ is the measurement matrix of the linear part, $H^{lin}(t)=\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 6}\end{array}\right]$. $w^{lin}(t)$ is the process noise of the linear part, $w^{lin}(t)\sim N(0,Q^{lin}(t))$. $v^{lin}(t+1)$ is the measurement noise, $v^{lin}(t+1)\sim N(0,R^{lin}(t+1))$.

The nonlinear part of state equation $X^{non}(t)={\left[\phi \right]}^T$ can be separated by Formula (44). The filter model after discretization of the nonlinear part:

$$\left\{\begin{array}{c}{X}^{non}(t+1)=f^{non}(X(t),t)+{\Xi }^{non}{\widehat{X}}^{lin}(t)+G^{non}(t)w^{non}(t)\\ Z^{non}(t+1)=H^{non}(t+1)X^{non}(t+1)+v^{non}(t+1)\end{array}\right.$$

(48)

In Formula (48), $f^{non}(X(t),t)$ is the nonlinear function of state $X(t)$, and

$$f^{non}(X(t),t)=(I-C_n^{n^{\prime }}){\omega }_{in}^n(t)+C_b^n\delta {\omega }_{in}^b(t)-C_b^n\delta {\omega }_{ib}^b(t),$$

$B^{non}$ is the input control matrix, ${\Xi }^{non}=\left[\begin{array}{ccc}{0}_{3\times 3}& -C_b^{n^{\prime }}& 0_{3\times 3}\end{array}\right]$. ${\widehat{X}}^{lin}(t)$ is the optimal estimation of the state of the linear part, it is also the control input of the nonlinear filter. $G^{non}(t)$ is the process noise control matrix of the nonlinear system, $G^{non}(t)=\left[-C_b^{n^{\prime }}\right]$. $w^{non}(t)$ is the noise vector, $w^{non}(t)\sim N(0,Q{(t)}^{non})$. $H^{non}(t+1)$ is the measurement matrix of the nonlinear system, $H^{non}(t)=\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 6}\end{array}\right]$. $v^{non}(t+1)$ is the measurement noise, $v^{non}(t+1)\sim N(0,R^{non}(t+1))$.

Based on the above analysis and derivation, the flow chart of the hybrid KF-ASMUKF alignment algorithm can be obtained, as shown in Figure 2:

Step 1: initialize the system, and apply the ASMUKF algorithm to the nonlinear partial filtering model at time $t+1$, as shown in Formula (48).

Step 2: through calculation, the best estimation of east-, north-, and up-misalignment angles can be obtained at time t.

Step 3: select the ASMUKF algorithm to perform filter estimation, ASMUKF one-step prediction and update refer to Formulas (31)–(42).

Step 4: obtain all state variable estimates according to filtering $\widehat{x}(t)={\left[\begin{array}{cc}{\widehat{x}}^{lin}(t)& {\widehat{x}}^{non}(t)\end{array}\right]}^T$ and the value observed $Z^{lin}(t+1)$ at time $t+1$, then KF is applied to the linear partial filtering model as Formula (47), the estimation ${\widehat{x}}^{lin}(t+1)$ of the optimal state variable of the linear part at time it can be obtained. Finally, return to Step 1 to start the next cycle of filtering.

5. Experimental Results and Analysis

5.1. Simulation Test

To verify the effectiveness of the algorithm in this paper. We set up a swing experiment platform based on a fiber optic gyro strapdown inertial navigation system on the three-axis turntable in the laboratory to simulate the marine environment of the ship. The test equipment is shown in Figure 3.

The fiber optic strapdown inertial navigation system consists of three fiber optic gyroscopes with an accuracy of 0.01°/h and three flexible accelerometers with an offset of $5\times {10}^{-4}$ g. The sampling frequency is 100 Hz. The three-axis turntable simulates the motion of the ship. The three-axis is set to sinusoidal signal. The local latitude and longitude is 24.6188° and the longitude is 118.0399°. the motion model for assessing the simulated ship’s pitch angle $\theta$, roll angle $\gamma$ and azimuth angle $\psi$:

$$\begin{array}{c}\theta ={\theta }_{m}sin(\pi t/5)\hfill \\ \gamma ={\gamma }_{m}sin(\pi t/6)\hfill \\ \psi ={\psi }_{m}sin(\pi t/8)\hfill \end{array}$$

(49)

In Formula (49), ${\theta }_m=4^{\circ }$, the swing period is 4 s, ${\gamma }_m=6^{\circ }$, the swing period is 6 s and ${\psi }_m={20}^{\circ }$, the swing period is 10 s.

Assuming that the system has completed coarse alignment, the hybrid filtering algorithm proposed in this article is used for fine alignment. The alignment results are shown in Figure 4, Figure 5 and Figure 6:

From Figure 4, Figure 5 and Figure 6, in the fine alignment stage of the KF-ASMUKF based hybrid filter algorithm, the pitch angle, roll angle, and heading angle converge faster, and the error is also small. The three attitude angle errors are about the first. After three minutes, it began to converge to a more accurate range.

To further verify the correctness of the proposed hybrid filtering algorithm, four ship simulation mooring alignment tests were carried out in this paper. During the simulation test, the coarse alignment was 5 min. The fine alignment was 10 min, and the integrated navigation system was switched to integrated navigation after the alignment was completed. To facilitate the comparison of different filtering algorithms, the data after the fine alignment are intercepted respectively using KF, UKF, and KF-ASMUKF, then they perform the initial alignment in the mooring state. The alignment results are shown in Figure 7, Figure 8 and Figure 9 and

Table 2. Attitude error results of mooring alignment under different filtering algorithms (simulated environment).

	Pitch Error (${}^{\prime }$)		Roll Error (${}^{\prime }$)		Heading Error (${}^{\prime }$)
	Mean	Std	Mean	Std	Mean	Std
KF	2.594	0.106	−3.752	0.125	7.342	0.235
UKF	2.284	0.062	−3.423	0.064	6.158	0.073
KF-ASMUKF	1.065	0.043	−2.251	0.032	4.599	0.041

:

According to Figure 7, Figure 8 and Figure 9 and Table 2, KF-ASMUKF can be used for SINS fine alignment. The attitude misalignment angle can quickly converge in a short time. The average of pitch error is less than 1.5${}^{\prime }$, the average of roll misalignment error is less than 2.5${}^{\prime }$, and the average heading error is less than 5${}^{\prime }$. The convergence process of the attitude misalignment angle estimation is stable and they have good stability. In addition, in this simulation environment, the estimated errors and standard deviations of the three attitude misalignment angles based on the KF-ASMUKF hybrid filter algorithm have certain advantages over KF and UKF, which verifies the effectiveness of the algorithm proposed in this paper.

5.2. Mooring Alignment Test on the Sea

To further verify the effectiveness of the algorithm proposed in this paper, the ship’s alignment test under mooring conditions was carried out in Yantai, China. The ship used in this test is shown in Figure 10:

The test ship is equipped with a high-precision laser SINS integrated navigation system, and a self-made fiber optic SINS integrated navigation system, as shown in Figure 11:

On the test ship, the self-made fiber optic SINS-GPS integrated navigation system and the high-precision integrated navigation system are fixed using aluminum alloy plates. The self-made SINS can achieve a fast and precise alignment method, the high-precision integrated navigation system is used as the attitude reference datum. GPS is used to output speed and position information, and a stable power supply is used to power the inertial navigation system. Before the on-site power-on test, each group of SINS integrated navigation systems is accurately calibrated.

In this paper, four ship mooring alignment tests were carried out. During the ship test, the coarse alignment was 5 min and the fine alignment was 10 min. After the alignment was completed, it was converted to integrated navigation. To facilitate the comparison of different filtering algorithms, the data at the moment after the fine alignment are intercepted. The KF, UKF, and KF-ASMUKF based filtering algorithms are used to perform the initial alignment of the ship in the mooring state. The estimated attitude error results of different filtering algorithms are shown in

Table 3. Attitude error results of mooring alignment under different filtering algorithms (real environment).

	Pitch Error (${}^{\prime }$)		Roll Error (${}^{\prime }$)		Heading Error (${}^{\prime }$)
	Mean	Std	Mean	Std	Mean	Std
KF	5.615	0.023	5.522	0.224	12.276	0.429
UKF	4.477	0.022	4.450	0.251	10.246	0.392
KF-ASMUKF	3.053	0.015	3.332	0.101	8.868	0.285

and Figure 12, Figure 13 and Figure 14:

According to Table 3 and Figure 12, Figure 13 and Figure 14, the proposed hybrid filtering algorithm based on KF-ASMUKF performs the initial alignment of ship’s SINS under mooring conditions. This method can make the three attitude misalignment angles fast in a short time. Convergence and the error are smaller than the KF algorithm and the UKF algorithm. The average of pitch error is 3.053${}^{\prime }$, the average of roll error is 3.332${}^{\prime }$, the average of heading error is 8.868${}^{\prime }$, which meets the alignment accuracy requirements of the ship’s SINS in the medium-precision mooring conditions. The estimation convergence process of the three attitude misalignment angles is stable, and they also have good stability. In addition, the estimated errors and standard deviations of the three attitude misalignment angles obtained based on the KF-ASMUKF hybrid filter algorithm have certain advantages over KF and UKF, which further verifies the effectiveness of the proposed algorithm.

6. Conclusions

Aiming at the problem of ship alignment under large azimuth misalignment under mooring conditions, a KF-ASMUKF hybrid filtering method is proposed. This paper deduced the ASMUKF algorithm, constructed its filter model, then combined it with the KF filter algorithm to form a KF-ASMUKF hybrid filter model through which the linear and nonlinear parts of the ship’s SINS alignment process were processed. This method reduces the computational complexity of the algorithm, improves the real-time performance of the filtering algorithm, and accelerates the convergence speed of mooring alignment under large azimuth misalignment angles. A simulation test of mooring alignment was carried out on a three-axis turntable, and an on-site mooring alignment test was carried out in Yantai. The experimental results show that this method improves the ship SINS alignment accuracy and reduces the amount of calculation in the ship SINS alignment process. It meets the accuracy requirements of medium-precision marine SINS, and lays the foundation for the later ship alignment and integrated navigation.

In addition, some future work needs to be improved and expanded:

(1) At present, the algorithm is carried out under relatively good sea conditions. In the future, the algorithm will be verified in a harsh environment.

(2) The algorithm was only tested for alignment in a mooring environment, and the effectiveness of the alignment algorithm will be verified during travel in the future.