A Novel Grid SINS/DVL Integrated Navigation Algorithm for Marine Application

Abstract

Integrated navigation algorithms under the grid frame have been proposed based on the Kalman filter (KF) to solve the problem of navigation in some special regions. However, in the existing study of grid strapdown inertial navigation system (SINS)/Doppler velocity log (DVL) integrated navigation algorithms, the Earth models of the filter dynamic model and the SINS mechanization are not unified. Besides, traditional integrated systems with the KF based correction scheme are susceptible to measurement errors, which would decrease the accuracy and robustness of the system. In this paper, an adaptive robust Kalman filter (ARKF) based hybrid-correction grid SINS/DVL integrated navigation algorithm is designed with the unified reference ellipsoid Earth model to improve the navigation accuracy in middle-high latitude regions for marine application. Firstly, to unify the Earth models, the mechanization of grid SINS is introduced and the error equations are derived based on the same reference ellipsoid Earth model. Then, a more accurate grid SINS/DVL filter model is designed according to the new error equations. Finally, a hybrid-correction scheme based on the ARKF is proposed to resist the effect of measurement errors. Simulation and experiment results show that, compared with the traditional algorithms, the proposed navigation algorithm can effectively improve the navigation performance in middle-high latitude regions by the unified Earth models and the ARKF based hybrid-correction scheme.

1. Introduction

More and more significant scientific research and shipping industry are carried out in the polar region [1]. No matter in or heading to the polar region, navigation plays an important role to ensure the safety and reliability of vehicles in marine application, so it is significant to ensure the navigation performance in middle-high latitude regions [2]. With the latitude increasing, the global navigation satellite system (GNSS) would suffer more and more challenges such as ionosphere scintillation, multipath effect and lack of reference stations [3,4]. Although the strapdown inertial navigation system (SINS) is highly autonomous and self-contained [5,6], the SINS navigation output contains three kinds of periodic oscillation errors and accumulated errors [7,8,9]. To restrain the SINS errors, the integrated navigation system of SINS and other external navigation information is usually applied. The Doppler velocity log (DVL) is a good acoustic-based device in marine applications, which can provide three-dimensional velocities to restrain the errors of marine SINS [10,11,12]. Therefore, the SINS/DVL integration is a potential method for highly accurate marine navigation when the vehicles are in or heading to the polar region.

The traditional SINS based on north-oriented geographic frame performs well in middle latitude regions but loses its effectiveness in high latitude regions because of the meridian convergence [13]. The wander frame is a traditional solution in high latitude regions, however it cannot provide position and orientation information near the pole [13]. A grid frame is proposed and the grid SINS algorithm is designed in [14], which can solve the problem of meridian convergence by setting an available reference line. In the traditional SINS, the north-oriented geographic frame is used in middle latitude regions and the grid frame is used in high latitude regions, but the errors will occur during the switch between these two frames. In fact, the grid SINS can also work in middle latitude regions in theory [14,15]. Therefore, if the grid SINS based integrated navigation algorithm is designed properly, the grid frame will be a reasonable choice for navigation system without navigation frame switch when working in middle-high latitude regions. In [15,16,17], the traditional north-oriented SINS based integrated navigation algorithms have been applied. However, when working in high latitude regions, the navigation calculation error caused by meridian convergence is one of main SINS error sources, which will not be estimated and restrained by the integrated filter. Thus, the traditional north-oriented integrated navigation systems cannot work properly in high latitude regions. The grid SINS based integrated navigation algorithms are proposed for flight and the GNSS and star tracker are employed as external measurements [18,19]. However, for marine application, the GNSS and star tracker cannot achieve the long-distance signal transmission under water. The acoustic-based DVL is a proper choice for marine application in integrated navigation systems. In [18,19], the integrated filter models are designed based on a sphere Earth model, while the SINS mechanizations are based on a reference ellipsoid Earth model. The disunity of the Earth models between the SINS mechanization and the integrated filter model will bring the principle errors and decrease the navigation accuracy, especially in middle latitude regions. Thus, it is necessary to choose a unified accurate Earth model and design an accurate grid SINS/DVL integrated filter model.

The accurate filter model, filter algorithm and correction scheme are important factors to ensure the navigation accuracy. The Kalman filter (KF) is a well-known filter method for integrated navigation applications [20]. However, the performance of KF depends on many factors such as filter models and noise characteristics [20,21]. Besides, the traditional KF is susceptible to measurement outliers [22,23]. It is noted that the uncertain measurement errors from acoustic-based sensors (such as DVL) are unavoidable, so an adaptive filter method is a proper choice for the SINS/DVL integrated system [24,25,26,27]. Finally, in traditional integrated navigation systems, the output-correction and feedback-correction are generally used as the correction schemes of the system, which will both impact the navigation system outcome through the correction. The output-correction integrated system cannot correct the SINS inner errors, so the accuracy of the filter would decrease sharply when the output-correction integrated system works for a long time. The feedback-correction system can correct the SINS inner errors at the update frequency of the integrated filter, but the filter errors will be also introduced into the SINS. In the traditional algorithms, the filter algorithms and correction schemes are researched independently. However, it would be better if the filter method and correction scheme are considered as a whole in the design to achieve a better navigation performance considering the characteristics of the DVL measurement errors.

The main contribution of this paper is to propose a novel marine grid SINS/DVL integrated navigation algorithm which can improve the performance of navigation system when vehicles working in or heading to the polar region. In this paper, the grid SINS mechanization with the reference ellipsoid Earth model is introduced firstly. Then, the gird SINS error equations are derived based on the SINS mechanization. According to the new grid SINS error equations, a dynamic filter model is designed and a modified accurate integrated navigation algorithm is proposed with the same Earth model. Besides, the DVL output errors are analyzed according to the DVL operating characteristic. Considering the DVL output errors, an adaptive robust Kalman filter (ARKF) is introduced, which can detect and reduce the influence of measurement errors by an adaptive factor. Then, a hybrid-correction scheme is designed based on the ARKF with a switching criterion and two correction channels. With the help of switching criterion, the proposed integrated navigation system can restrain navigation errors and improve the accuracy by choosing the proper correction channel. Finally, simulations and semi-physical experiments are conducted to validate the performance of the proposed integrated navigation algorithm.

2. Grid SINS Mechanization

The World Geodetic System 1984 (WGS-84) Earth model is widely used as the national geodetic reference system and is also chosen as the reference ellipsoid Earth model in this paper to design the grid SINS/DVL integrated navigation algorithm. The main frames used in this paper are the inertial frame (denoted as $i$), the Earth centered Earth fixed (ECEF) frame (denoted as $e$), the geographic frame (denoted as $g$), the grid frame (denoted as $G$) and the body frame (denoted as $b$).

2.1. Grid Frame

The grid frame is adopted to avoid the navigation errors caused by the meridian convergence. The mass center of the vehicle is chosen as the grid frame origin. As shown in Figure 1, when the mass center of the vehicle is located at point $P$, the origins of grid and geographic frames coincide with $P$; the local grid plane is the plane which passes through the point $P$ and parallels to the Greenwich plane. The $G$ frame is a right-handed coordinate system, and the axes are defined as follows.

The $o{y}_G$ axis is the grid north axis lying along the intersecting line of grid plane and local horizontal plane. The $o{x}_G$ axis is the grid East axis lying in the local horizontal plane and perpendicular to grid north axis. The $o{z}_G$ axis is the grid up axis perpendicular to the local horizontal plane and along the geographic frame z-axis.

The angle between the geographic north and grid north axis is $\sigma$. The latitude and longitude of point $P$ are defined as $\phi$ and $\lambda$, respectively. The transformation matrix between $e$, $g$ and $G$ frames can be described as:

$${\mathit{C}}_e^g= \left[\begin{array}{ccc}-\sin \lambda & \cos \lambda & 0\\ -\sin \phi \cos \lambda & -\sin \phi \sin \lambda & \cos \phi \\ \cos \phi \cos \lambda & \cos \phi \sin \lambda & \sin \phi \end{array}\right]$$

(1)

$${\mathit{C}}_g^G= \left[\begin{array}{ccc}\cos \sigma & -\sin \sigma & 0\\ \sin \sigma & \cos \sigma & 0\\ 0& 0& 1\end{array}\right]$$

(2)

The $\sigma$ can be obtained by:

$$\sin \sigma =\sin \lambda \sin \phi /\sqrt{1-{\cos }^2\phi {\sin }^2\lambda }$$

(3)

$$\cos \sigma =\cos \lambda /\sqrt{1-{\cos }^2\phi {\sin }^2\lambda }$$

(4)

2.2. Grid SINS Mechanization

The differential equations of SINS attitude, velocity and position in $G$ frame are

$${\dot{\mathit{C}}}_b^G={\mathit{C}}_b^G({\mathit{\omega }}_{Gb}^b\times )$$

(5)

$${\dot{\mathit{V}}}^G={\mathit{C}}_b^G{\mathit{f}}^b-\left(2{\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G\right)\times {\mathit{V}}^G+{\mathit{g}}^G$$

(6)

$${\dot{\mathit{R}}}^e={\mathit{C}}_G^e{\mathit{V}}^G= {({\mathit{C}}_e^G)}^T{({\mathit{C}}_G^G)}^T{\mathit{V}}^G$$

(7)

where ${\mathit{C}}_b^G$ represents the calculated attitude matrix of SINS from $b$ to $G$ frame; ${\mathit{\omega }}_{Gb}^b$ is the angular velocity of $G$ frame which is relative to $b$ frame and is expressed in $b$ frame; $({\mathit{\omega }}_{Gb}^b\times )$ is the anti-symmetric matrix of ${\mathit{\omega }}_{Gb}^b$; ${\mathit{V}}^G$ is the velocity of vehicles expressed in $G$ frame; ${\mathit{f}}^b$ is the specific force measured by SINS accelerometers; ${\mathit{\omega }}_{ie}^G$ is the angular velocity of Earth rotation expressed in $G$ frame; ${\mathit{\omega }}_{eG}^G$ is the angular velocity of $e$ frame which is relative to $G$ frame and is expressed in $G$ frame; ${\mathit{g}}^G$ is the projection of local gravity acceleration expressed in $G$ frame; ${\mathit{R}}^e$ is the position coordinates expressed in $e$ frame; and ${\mathit{C}}_G^e$ is the transformation matrix from $G$ to $e$ frame. ${\mathit{\omega }}_{Gb}^b$ can be obtained by:

$${\mathit{\omega }}_{Gb}^b= {\mathit{\omega }}_{ib}^b-{\mathit{C}}_G^b{\mathit{\omega }}_{iG}^G$$

(8)

$${\mathit{\omega }}_{iG}^G= {\mathit{\omega }}_{ie}^G+ {\mathit{\omega }}_{eG}^G$$

(9)

$${\mathit{\omega }}_{ie}^G = {\mathit{C}}_g^G{\mathit{\omega }}_{ie}^g$$

(10)

where ${\mathit{\omega }}_{ib}^b$ is the output vector of SINS gyroscopes and ${\mathit{\omega }}_{ie}^g$ is the angular velocity vector of Earth expressed in $g$ frame.

The parameter $\kappa$ is defined as:

$$\kappa =\frac{\sin \lambda \cos \phi }{\sqrt{1-{\cos }^2\phi {\sin }^2\lambda }}$$

(11)

Then, ${\mathit{\omega }}_{eG}^G$ can be described as:

$${\mathit{\omega }}_{eG}^G=\left[\begin{array}{c}{\omega }_{eGx}^G\\ {\omega }_{eGy}^G\\ {\omega }_{eGz}^G\end{array}\right]=\left[\begin{array}{cc}\frac{1}{{\tau }_f}& -\frac{1}{R_y}\\ \frac{1}{R_x}& -\frac{1}{{\tau }_f}\\ \frac{\kappa }{{\tau }_f}& -\frac{\kappa }{R_y}\end{array}\right]\left[\begin{array}{c}{v}_E^G\\ v_N^G\end{array}\right]$$

(12)

where ${R_x}^{-1}$ is the local Earth curvature along the grid East axis. ${R_y}^{-1}$ is the local Earth curvature along the grid north axis. ${{\tau }_f}^{-1}$ is the local Earth twist rate. They can be described as:

$$\{\begin{array}{l}\frac{1}{R_x}=\frac{{\sin }^2\sigma }{R_{Mh}}+\frac{{\cos }^2\sigma }{R_{Nh}}\\ \frac{1}{R_y}=\frac{{\cos }^2\sigma }{R_{Mh}}+\frac{{\sin }^2\sigma }{R_{Nh}}\\ \frac{1}{{\tau }_f}=\left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right)\sin \sigma \cos \sigma \end{array}$$

(13)

where $R_{Mh}$ and $R_{Nh}$ are the local radiuses of Earth in the meridian circle and the prime vertical, respectively.

Because $G$ frame has a local horizontal plane, ${\mathit{g}}^G$ can be described as:

$${\mathit{g}}^G={\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^{\mathrm{T}}$$

(14)

where $g$ is the gravitational acceleration.

The position transformation from geodetic latitude, longitude, and altitude to ECEF coordinate is described as:

$$\{\begin{array}{l}x=R_{Nh}\cos \phi \cos \lambda \\ y=R_{Nh}\cos \phi \sin \lambda \\ z=[R_N(1-e^2)+h]\sin \phi \end{array}$$

(15)

The existing studies choose the north-oriented geographic frame as the navigation frame in middle latitude regions and choose the grid frame in high latitude regions. When vehicles navigate from middle latitude regions to high latitude regions, the switch of navigation frame is simulated and the attitude and velocity curves at the switch instant are shown in Figure 2.

As shown in Figure 2, the navigation curves jump when navigation frame switch occurs. Because the frame switch is a non-successive change, when a frame switch happens, some sudden changes will occur in the navigation system. To ensure the navigation accuracy when vehicles work in middle-high latitude regions, it is necessary to unify the navigation frame and improve the performance of grid frame based navigation system.

3. Error Equations of Grid SINS and DVL

In this section, new grid SINS error models are established and the error equations of DVL are derived. In the grid SINS, the main error sources are the gyroscope drifts (denoted as $\delta {\mathit{\omega }}_{ib}^b$) and the accelerometer biases (denoted as $\delta f^b$). The attitude velocity and position errors are denoted as $\mathit{\varphi }={\left[\begin{array}{ccc}{\varphi }_x& {\varphi }_y& {\varphi }_z\end{array}\right]}^{\mathrm{T}}$ $\delta {\mathit{V}}^G={\left[\begin{array}{ccc}\delta V_x^G& \delta V_y^G& \delta V_z^G\end{array}\right]}^{\mathrm{T}}$ and $\delta \mathit{P}={\left[\begin{array}{ccc}\delta \phi & \delta \lambda & \delta h\end{array}\right]}^{\mathrm{T}}$, respectively. Because the grid SINS calculates position in ECEF coordinate, the position error in ECEF can be denoted as $\delta {\mathit{R}}^e={\left[\begin{array}{ccc}\delta x& \delta y& \delta z\end{array}\right]}^{\mathrm{T}}$.

3.1. Earth Models and Parameters

It is known that the Earth is an irregular sphere and presents as an oval shape. In the navigation system, the Earth model should be described with special equations. There are three common Earth models in navigation, i.e., the geoid sphere, the reference ellipsoid and the sphere model. The geoid sphere is the most accurate one among the three models, but it is hard to be expressed by equations. The reference ellipsoid is close to geoid sphere, the deflections of the normal direction are less than $3^{″}$ and the errors of radius are less than 150 m. The sphere is the most imprecise of the three models, and is normally used in theoretical analysis. Therefore, the reference ellipsoid Earth model is the common one employed in the practical integrated navigation.

In the traditional grid SINS/DVL integrated navigation system, the sphere and the reference ellipsoid are both used as Earth models. When the Earth is modeled as a reference ellipsoid, the normal of reference ellipsoid model is called as the geographic vertical, and the geographic latitude $\phi$ is defined as the angle between geographic vertical and equatorial plane. When the Earth is modeled as a sphere, the connecting line between point $P$ and Earth center is called the geocentric vertical, and the geocentric latitude $\widehat{\phi }$ is the angle between geocentric vertical and equatorial plane. The reference ellipsoid Earth model and two kinds of latitudes are shown in Figure 3. $R_e$ and $R_p$ are the two semi-axes of the Earth.

In SINS, the latitude is defined as geographic latitude. If the point $P$ on the Earth is expressed by geographic and geocentric latitude, respectively, the latitude error caused by the disunity of the Earth models is defined as $\Delta \phi =\phi -\widehat{\phi }$, which can be described as:

$$\Delta \phi \approx e\sin 2\phi$$

(16)

The maximum deviation between geographic and geocentric latitude occurs in middle-latitude regions, which can be up to ${11}^{\prime }$. The latitude error will bring decrease of the integrated navigation system accuracy especially in middle latitude regions. To improve the integrated navigation accuracy, it is necessary to unify the Earth models of the SINS mechanization and the integrated filter model.

In the grid SINS based on the reference ellipsoid Earth model, the local curvatures and local twist rate of Earth are described as $\frac{1}{R_x^c}=\frac{1}{R_x}+\delta \frac{1}{R_x}$, $\frac{1}{R_y^c}=\frac{1}{R_y}+\delta \frac{1}{R_y}$ and $\frac{1}{{\tau }_f^c}=\frac{1}{{\tau }_f}+\delta \frac{1}{{\tau }_f}$. Neglecting the second order small quantity and according to Equation (13), the errors of Earth curvatures and twist rate in $G$ frame can be obtained as:

$$\{\begin{array}{l}\delta \frac{1}{R_x}= 2\sin \sigma \cos \sigma \left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right)\delta \sigma \\ \delta \frac{1}{R_y}=-2\sin \sigma \cos \sigma \left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right)\delta \sigma \\ \delta \frac{1}{{\tau }_f}=-\left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right)({\sin }^2\sigma -{\cos }^2\sigma )\delta \sigma \end{array}$$

(17)

Deriving the first order differential of Equation (4), $\delta \sigma$ can be obtained by:

$$\delta \sigma = \left[\begin{array}{ccc}\frac{\sin \phi }{1-{\cos }^2\phi {\sin }^2\lambda }& \frac{\sin \lambda \cos \lambda \cos \phi }{1-{\cos }^2\phi {\sin }^2\lambda }& 0\end{array}\right]\left[\begin{array}{c}\delta \phi \\ \delta \lambda \\ \delta h\end{array}\right] = {\mathit{C}}_1\delta \mathit{P}$$

(18)

Deriving the first order differential of Equation (15), $\delta \mathit{P}$ can be calculated as:

$$\delta \mathit{P}= \frac{1}{R_{Nh}}\left[\begin{array}{ccc}-\sin \phi \cos \lambda & -\sin \phi \sin \lambda & \cos \phi \\ -\frac{\sin \lambda }{\cos \phi }& \frac{\cos \lambda }{\cos \phi }& 0\\ R_{Nh}\cos \phi \cos \lambda & R_{Nh}\cos \phi \sin \lambda & R_{Nh}\sin \phi \end{array}\right]\left[\begin{array}{c}\delta x\\ \delta y\\ \delta z\end{array}\right] = {\mathit{C}}_{R2P}\delta {\mathit{R}}^e$$

(19)

Then, the $\delta \sigma$ can be obtained by Equations (18) and (19):

$$\delta \sigma = {\mathit{C}}_1{\mathit{C}}_{R2P}\delta {\mathit{R}}^e = {\mathit{C}}_{R2\sigma }\delta {\mathit{R}}^e$$

(20)

According to Equations (17) and (20), the error equations of the Earth curvatures and twist rate in $G$ frame can be calculated as:

$$\{\begin{array}{l}\delta \frac{1}{R_x} = 2\sin \sigma \cos \sigma \left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right){\mathit{C}}_{R2\sigma }\delta {\mathit{R}}^e\\ \delta \frac{1}{R_y} = -2\sin \sigma \cos \sigma \left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right){\mathit{C}}_{R2\sigma }\delta {\mathit{R}}^e\\ \delta \frac{1}{{\tau }_f} = -\left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right)({\sin }^2\sigma -{\cos }^2\sigma ){\mathit{C}}_{R2\sigma }\delta {\mathit{R}}^e\end{array}$$

(21)

3.2. ${\mathit{\omega }}_{eG}^G$ Error Equations

According to the grid SINS mechanization, the accuracy of ${\mathit{\omega }}_{eG}^G$ will also be influenced by Earth model. When the Earth is considered as a sphere, then $1/R_x=1/R_{eh}$, $1/R_y=1/R_{eh}$, and $1/{\tau }_f^c=0$. $R_{eh}$ is the local radius of Earth. According to Equation (12), the error of ${\mathit{\omega }}_{eG}^G$ can be described as:

$$\begin{array}{cc}\hfill \delta {\mathit{\omega }}_{eG\_ball}^G& = {\mathit{\omega }}_{eG\_ball}^{G^c}-{\mathit{\omega }}_{eG\_ball}^G\hfill \\ \hfill & =\left[\begin{array}{c}-\frac{1}{R_{eh}}\delta v_{G_N}\\ \frac{1}{R_{eh}}\delta v_{G_E}\\ -\frac{1}{R_{eh}}\kappa \delta v_{G_N}+\frac{v_{G_N}}{R_{eh}}\frac{\kappa }{\cot \phi }\delta \phi -\frac{v_{G_N}}{R_{eh}}\kappa \delta \sigma \end{array}\right]\hfill \end{array}$$

(22)

When the Earth is considered as a reference ellipsoid, according to the Equations (12) and (21), the error of ${\mathit{\omega }}_{eG}^G$ can be obtained by:

$$\begin{array}{cc}\hfill \mathit{\delta }{\omega }_{eG}^G& = {\mathit{\omega }}_{eG}^{G^c}-{\mathit{\omega }}_{eG}^G\hfill \\ \hfill & =\left[\begin{array}{c}-\frac{1}{R_y}\delta v_{G_N}\\ \frac{1}{R_x}\delta v_{G_E}\\ -\frac{1}{R_y}\kappa \delta v_{G_N}+\left(\frac{v_{G_N}}{R_y}-\frac{v_{G_E}}{{\tau }_f}\right)\frac{\kappa }{\cot \phi }\delta \phi -\left(\frac{v_{G_N}}{R_y}-\frac{v_{G_E}}{{\tau }_f}\right)\kappa \cot \sigma \delta \sigma \end{array}\right] + \left[\begin{array}{c}\frac{1}{{\tau }_f}\delta v_{G_E}+v_{G_E}\delta \frac{1}{{\tau }_f}-v_{G_N}\delta \frac{1}{R_y}\\ -\frac{1}{{\tau }_f}\delta v_{G_N}-v_{G_N}\delta \frac{1}{{\tau }_f}+v_{G_E}\delta \frac{1}{R_x}\\ \frac{1}{{\tau }_f}\kappa \delta v_{G_E}+v_{G_E}\kappa \delta \frac{1}{{\tau }_f}-v_{G_N}\kappa \delta \frac{1}{R_y}\end{array}\right]\hfill \end{array}$$

(23)

It can be seen that the $\delta {\mathit{\omega }}_{eG}^G$ equation has advantages compared to the $\delta {\mathit{\omega }}_{eG\_ball}^G$ equation. The influence from local Earth parameters and Earth parameter errors is considered and the influence from velocity and velocity errors is also presented more comprehensively in the $\delta {\mathit{\omega }}_{eG}^G$ equation than in the $\delta {\mathit{\omega }}_{eG\_ball}^G$ equation. In this case, the error of ${\mathit{\omega }}_{eG}^G$ can be described more accurately by Equation (23), and Equation (23) will provide a better accuracy when it is used in the grid SINS/DVL integrated navigation system.

3.3. Grid SINS Attitude Error Equation

Quaternion algorithm is usually used in the implementation of attitude update; the quaternion error differential equation in grid frame can be described as:

$$\delta \dot{\mathit{Q}}=-\frac{1}{2}\delta {\tilde{\mathit{\omega }}}_{ib}^G\otimes \delta \mathit{Q}-\frac{1}{2}({\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G)\otimes \delta \mathit{Q} +\frac{1}{2}\delta \mathit{Q}\otimes ({\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G+\delta {\mathit{\omega }}_{ie}^G+\delta {\mathit{\omega }}_{eG}^G)$$

(24)

where $\mathit{Q}$ is the quaternion. The attitude error $\mathit{\varphi }={\left[\begin{array}{ccc}{\varphi }_x& {\varphi }_y& {\varphi }_z\end{array}\right]}^{\mathrm{T}}$ is considered as a small angle vector, and the quaternion $\mathit{Q}$ can be described as:

$$\delta \mathit{Q}=1+\frac{\mathit{\varphi }}{2}$$

(25)

$$\delta \dot{\mathit{Q}}=1+\frac{\dot{\mathit{\varphi }}}{2}$$

(26)

According to Equations (25) and (26), and neglecting the second order small quantity, Equation (24) can be written as:

$$\dot{\mathit{\varphi }}=\mathit{\varphi }\times {\mathit{\omega }}_{iG}^G+\delta {\mathit{\omega }}_{ie}^G+\delta {\mathit{\omega }}_{eG}^G-\delta {\tilde{\mathit{\omega }}}_{ib}^G$$

(27)

Besides, Equation (23) can be rewritten as:

$$\delta {\mathit{\omega }}_{eG}^G = {\mathit{C}}_2\delta \mathit{P}+{\mathit{C}}_3\delta \mathit{V}+{\mathit{C}}_4\delta \sigma$$

(28)

where ${\mathit{C}}_2$, ${\mathit{C}}_3$ and ${\mathit{C}}_4$ can be expressed as:

$${\mathit{C}}_2= \left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ \left(\frac{v_{G_N}}{R_y}-\frac{v_{G_E}}{{\tau }_f}\right)\frac{\kappa }{\cot \phi }& 0& 0\end{array}\right]$$

(29)

$${\mathit{C}}_3= \left[\begin{array}{ccc}\frac{1}{{\tau }_f}& -\frac{1}{R_y}& 0\\ \frac{1}{R_x}& -\frac{1}{{\tau }_f}& 0\\ \frac{\kappa }{{\tau }_f}& -\frac{\kappa }{R_y}& 0\end{array}\right]$$

(30)

$${\mathit{C}}_4= \left[\begin{array}{c}{v}_{G_N}2\sin \sigma \cos \sigma \left(\frac{1}{R_M}-\frac{1}{R_N}\right)\\ v_{G_E}2\sin \sigma \cos \sigma \left(\frac{1}{R_M}-\frac{1}{R_N}\right)\\ v_{G_N}\kappa 2\sin \sigma \cos \sigma \left(\frac{1}{R_M}-\frac{1}{R_N}\right)\end{array}\right] + \left[\begin{array}{c}-v_{G_E}\left(\frac{1}{R_M}-\frac{1}{R_N}\right)({\sin }^2\sigma -{\cos }^2\sigma )\\ v_{G_N}\left(\frac{1}{R_M}-\frac{1}{R_N}\right)({\sin }^2\sigma -{\cos }^2\sigma )\\ -v_{G_E}\kappa \left(\frac{1}{R_M}-\frac{1}{R_N}\right)({\sin }^2\sigma -{\cos }^2\sigma )-\left(\frac{v_{G_N}}{R_y}-\frac{v_{G_E}}{{\tau }_f}\right)\kappa \cot \sigma \end{array}\right]$$

(31)

According to Equation (10), the angular velocity error of Earth $\delta {\mathit{\omega }}_{ie}^G$ can be obtained by:

$$\begin{array}{l}\delta {\mathit{\omega }}_{ie}^G={\mathit{\omega }}_{ie}^{G^c}-{\mathit{\omega }}_{ie}^G\\ ={\omega }_{ie}\left[\begin{array}{c}-\cos \phi \cos \sigma \\ -\cos \phi \sin \sigma \\ 0\end{array}\right]\delta \sigma + {\omega }_{ie}\left[\begin{array}{ccc}\sin \phi \sin \sigma & 0& 0\\ -\sin \phi \cos \sigma & 0& 0\\ \cos \phi & 0& 0\end{array}\right]\left[\begin{array}{c}\delta \phi \\ \delta \varphi \\ \delta h\end{array}\right] = {\mathit{C}}_5\delta \sigma + {\mathit{C}}_6\delta \mathit{P}\end{array}$$

(32)

The attitude error equation can be calculated by Equations (27), (28) and (32):

$$\dot{\mathit{\varphi }}=-{\mathit{\omega }}_{iG}^G\times \mathit{\varphi }+{\mathit{C}}_{V\_\varphi }\delta \mathit{V}+{\mathit{C}}_{R\_\varphi }\delta {\mathit{R}}^e-\delta {\tilde{\mathit{\omega }}}_{ib}^G$$

(33)

where ${\mathit{C}}_{V\_\varphi }={\mathit{C}}_3$ and ${\mathit{C}}_{R\_\varphi }=({\mathit{C}}_6+ {\mathit{C}}_2){\mathit{C}}_{R2P}+({\mathit{C}}_5+ {\mathit{C}}_4){\mathit{C}}_{R2\sigma }$.

3.4. Grid SINS Velocity Error Equation

Based on the $G$ frame, the actual velocity differential equation can be described as:

$$\begin{array}{cc}\hfill {\dot{\widehat{\mathit{V}}}}^G& ={\widehat{\mathit{C}}}_b^G{\tilde{\mathit{f}}}^b-\left(2{\widehat{\mathit{\omega }}}_{ie}^G+{\widehat{\mathit{\omega }}}_{eG}^G\right)\times {\widehat{\mathit{V}}}^G+{\widehat{\mathit{g}}}^G\hfill \\ \hfill & = {\mathit{C}}_G^{G^c}{\mathit{C}}_b^G({\mathit{f}}^b+\delta {\mathit{f}}^b)+({\mathit{g}}^G+\delta {\mathit{g}}^G)-\left(2({\mathit{\omega }}_{ie}^G+\delta {\mathit{\omega }}_{ie}^G)+({\mathit{\omega }}_{eG}^G+\delta {\mathit{\omega }}_{eG}^G\right)\times ({\mathit{V}}^G+\delta {\mathit{V}}^G)\hfill \end{array}$$

(34)

According to Equations (6), (28), (32) and (34), neglecting the second order small quantity and error of local gravity acceleration, the velocity error can be calculated as:

$$\delta {\dot{\mathit{V}}}^G= {\mathit{f}}^G\times \mathit{\varphi }+{\mathit{C}}_{V\_V}\delta {\mathit{V}}^G+{\mathit{C}}_{R\_V}\delta {\mathit{R}}^e+{\mathit{C}}_b^G\delta {\mathit{f}}^b$$

(35)

where ${\mathit{C}}_{R\_V}= ({\mathit{V}}^G\times )(2{\mathit{C}}_6+{\mathit{C}}_2){\mathit{C}}_{R2P}+({\mathit{V}}^G\times )(2{\mathit{C}}_5+{\mathit{C}}_4){\mathit{C}}_{R2\sigma }$ and ${\mathit{C}}_{V\_V}= ({\mathit{V}}^G\times ){\mathit{C}}_3-(2{\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G)\times$.

3.5. Grid SINS Position Error Equation

Based on the ECEF frame, the actual position differential equation of the grid SINS can be written as:

$${\dot{\widehat{\mathit{R}}}}^e={\mathit{C}}_{G^C}^e{\widehat{\mathit{V}}}^G={\mathit{C}}_G^e(I+\delta {\mathit{\theta }}^G)({\mathit{V}}^G+\delta {\mathit{V}}^G)$$

(36)

where $\delta {\mathit{\theta }}^G$ is the error angle between $G$ and $G^c$ frames.

According to Equations (7) and (36), the position error can be calculated as:

$$\delta {\dot{\mathit{R}}}^e={\mathit{C}}_G^e\delta {\mathit{V}}^G-{\mathit{C}}_G^e({\mathit{V}}^G\times )\delta {\mathit{\theta }}^G$$

(37)

As shown in Figure 4, when a vehicle is located at point $P$, the local grid frame is denoted as $P{x}_G{y}_G{z}_G$; the true values of latitude, longitude and angle between the true north and grid north are denoted as $\phi$, $\lambda$ and $\sigma$, respectively; the calculated grid frame is denoted as $P^c{x}_{G^c}{y}_{G^c}{z}_{G^c}$ and the calculated values are denoted as ${\phi }^c$, ${\lambda }^c$ and ${\sigma }^c$, respectively.

The local $G$ frame can be obtained by a three-time rotation from $G^c$ frame. The error $\delta {\mathit{\theta }}^G$ can be presented as:

$$\begin{array}{cc}\hfill \delta {\mathit{\theta }}^G& = \left[\begin{array}{ccc}-\cos \sigma & -\cos \phi \sin \sigma & 0\\ -\sin \sigma & \cos \phi \cos \sigma & 0\\ 0& \sin \phi & 0\end{array}\right]\left[\begin{array}{c}\delta \phi \\ \delta \lambda \\ \delta h\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]\delta \sigma \hfill \\ \hfill & ={\mathit{C}}_7\delta \mathit{P}+{\mathit{C}}_8\delta \sigma =({\mathit{C}}_7{\mathit{C}}_{R2P}+{\mathit{C}}_8{\mathit{C}}_{R2\sigma })\delta {\mathit{R}}^e= {\mathit{C}}_{R2\delta \theta }\delta {\mathit{R}}^e\hfill \end{array}$$

(38)

According to Equation (38), Equation (37) can be rewritten as:

$$\delta {\dot{\mathit{R}}}^e = {\mathit{C}}_{V\_R}\delta {\mathit{V}}^G+{\mathit{C}}_{R\_R}\delta {\mathit{R}}^e$$

(39)

where ${\mathit{C}}_{V\_R}= {\mathit{C}}_G^e$ and ${\mathit{C}}_{R\_R}=-{\mathit{C}}_G^e({\mathit{V}}^G\times ){\mathit{C}}_{R2\delta \theta }$.

3.6. DVL Error Equation

The DVL measures the Doppler frequency shift, and then provides three-dimensional velocity measurements. In general, the DVL has two modes of operation, i.e., the bottom lock mode and the bottom unlock mode. When the DVL is used to provide the velocity of the vehicle related to the seafloor, it operates in bottom lock mode. The main error sources of the DVL contain the sound velocity errors and the frequency shift biases. In the DVL output, the sound velocity errors mainly cause the scale factor errors and the frequency shift biases mainly lead to the random velocity errors. The DVL output in the DVL body frame (donated as $m$ frame) can be presented as:

$${\tilde{\mathit{V}}}_{DVL}^m=(\mathit{I}+\delta \mathit{K}){\mathit{V}}_{DVL}^m+\delta {\mathit{V}}_{DVL}^m$$

(40)

where ${\mathit{V}}_{DVL}^m$ is the actual velocity in $m$ frame; $\delta \mathit{K}$ is the scale factor error; and $\delta {\mathit{V}}_{DVL}^m$ is the random velocity error.

When the DVL output is translated to navigation frame, the main DVL output errors also include the installation error $\mathit{\psi }$ and the attitude error $\mathit{\phi }$. The DVL output in navigation frame (i.e., $G$ frame) can be obtained by:

$${\tilde{\mathit{V}}}_{DVL}^G=[\mathit{I}-\mathit{\phi }\times ]{\mathit{C}}_b^G[\mathit{I}-\mathit{\psi }\times ]{\mathit{C}}_m^b{\tilde{\mathit{V}}}_{DVL}^{\mathrm{m}}$$

(41)

Neglecting the second order small quantity, the DVL output in $G$ frame can be rewritten as:

$${\tilde{\mathit{V}}}_{DVL}^G={\mathit{C}}_b^G{\mathit{C}}_m^b{\mathit{V}}_{DVL}^m+\delta \mathit{K}{\mathit{C}}_b^G{\mathit{C}}_m^b{\mathit{V}}_{DVL}^m+{\mathit{C}}_b^G{\mathit{C}}_m^b\delta {\mathit{V}}_{DVL}^m+{\mathit{C}}_b^G{\mathit{C}}_m^b({\mathit{V}}_{DVL}^m\times )\mathit{\phi }+{\mathit{C}}_b^G{\mathit{C}}_m^b({\mathit{V}}_{DVL}^m\times )\mathit{\psi }$$

(42)

In the practical application, due to some special marine environments, the DVL may fail to maintain bottom lock and lead to the following three consequences: firstly, the DVL output will contain noises with uncertain statistical characteristic, which are mostly large-amplitude errors and intensity mutation errors; secondly, the DVL output will provide velocities related to the ocean currents, which are shown as constant velocity errors; and, finally, no velocity information will be provided by the DVL. When the DVL is not able to provide vehicle velocities, the navigation system will change from the integrated mode to the independent inertial navigation mode. In this paper, the first two situations will be discussed. Both kinds of DVL output errors contain uncertain noises with unknown characteristics and are hard to model accurately.

4. Design of the Grid SINS/DVL Integrated Navigation Algorithms

The grid SINS can ensure the reliability and accuracy independently in short term. However, due to the errors of inertial measurement unit (IMU), the navigation accuracy will decrease sharply when the grid SINS works independently for a long time. To ensure the navigation accuracy, an integration of grid SINS and DVL is adopted.

The traditional grid frame integrated navigation systems have different Earth models between the mechanization and the filter model, which will bring the principle error and decrease the navigation accuracy.

Besides, the traditional integrated navigation systems are designed by the traditional KF. The KF is an optimal state estimator based on the assumption that the system dynamic model and noise statistic characteristics are known precisely. However, the KF would not be optimal when used in practice due to the uncertain measurement noises and outliers.

Finally, the traditional integrated system with the output-correction or the feedback-correction has its own shortages. The output-correction corrects the SINS output errors directly. The traditional integrated system with the output-correction can converge with a fast speed when the measurement contains mutation errors, but the SINS inner errors are not corrected and still accumulate with time. Due to the accumulated error, when working for long hours the filter state equation will not match the actual SINS working state and the filter accuracy will decrease or even diverge. The ships or submarines always work for long hours in or heading to the polar region, so the output-correction is hardly suited for the navigation task. In contrast, the feedback-correction can correct the SINS inner errors with the same update frequency of the filter, but the filter output will be involved in SINS calculation. As a result, the filter errors become one of the SINS error sources. When the measurement contains unexpected mutation errors, the feedback-correction system will converge slowly compared to the output-correction and the filter estimation results will contain serious errors. If the serious errors are introduced into the SINS, the navigation accuracy will decrease seriously and the influence will exist for a long time. In practice, the uncertain DVL output errors are unavoidable, so the feedback-correction is not a proper correction method.

To overcome the disadvantages of the traditional system discussed above and improve the navigation performance for marine applications, a novel grid SINS/DVL integrated navigation algorithm is designed with the unified Earth model and an ARKF based hybrid-correction scheme is proposed in the following sections.

4.1. Dynamic Model

The dynamic models of the integrated navigation system include grid SINS model and DVL model. Assuming that constant errors (denoted as ${\mathit{\epsilon }}_c^b$ and ${\nabla }_c^b$) and random errors (denoted as ${\mathit{\epsilon }}_w^b$ and ${\nabla }_w^b$) exist in gyroscope and accelerometer outputs, the gyroscope and accelerometer output errors can be denoted as:

$$\delta {\tilde{\mathit{\omega }}}_{ib}^b= {\mathit{\epsilon }}_c^b+{\mathit{\epsilon }}_w^b$$

(43)

$$\delta {\tilde{\mathit{f}}}^b={\mathit{\nabla }}_c^b+{\mathit{\nabla }}_w^b$$

(44)

The attitude error $\mathit{\varphi }$, velocity error $\delta {\mathit{V}}^G$, position error in ECEF frame $\delta {\mathit{R}}^e$, drifts of gyroscope ${\mathit{\epsilon }}^b$ and accelerometers ${\mathit{\nabla }}^b$ are chosen as the states of the grid SINS to be estimated, i.e., ${\mathit{x}}_{SINS}={\left[\begin{array}{ccccc}\mathit{\varphi }& \delta {\mathit{V}}^G& \delta {\mathit{R}}^e& {\mathit{\epsilon }}_c^b& {\mathit{\nabla }}_c^b\end{array}\right]}^{\mathrm{T}}$. According to the derived error equations of grid SINS in Section 3, the differential equations of grid SINS states can be described as:

$$\{\begin{array}{l}\dot{\mathit{\varphi }} =-{\mathit{\omega }}_{iG}^G\times \mathit{\varphi }+{\mathit{C}}_{V\_\varphi }\delta \mathit{V}+{\mathit{C}}_{R\_\varphi }\delta {\mathit{R}}^e-{\mathit{C}}_b^G{\mathit{\epsilon }}^b\hfill \\ \delta {\dot{\mathit{V}}}^G = {\mathit{f}}^G\times \mathit{\varphi }+{\mathit{C}}_{V\_V}\delta {\mathit{V}}^G+{\mathit{C}}_{R\_V}\delta {\mathit{R}}^e+{\mathit{C}}_b^G{\nabla }^b\hfill \\ \begin{array}{l}\delta {\dot{\mathit{R}}}^e = {\mathit{C}}_{V\_R}\delta {\mathit{V}}^G+{\mathit{C}}_{R\_R}\delta {\mathit{R}}^e\\ {\dot{\mathit{\epsilon }}}_c^b = 0\end{array}\hfill \\ {\dot{\mathit{\nabla }}}_c^b = 0\hfill \end{array}$$

(45)

The DVL state is ${\mathit{x}}_{DVL}=\delta {\mathit{V}}_{DVL}^m$, and the random velocity error is assumed as the one-order Markov process:

$$\delta {\dot{\mathit{V}}}_{DVL}^m=-\delta {\mathit{V}}_{DVL}^m/{\tau }_V+{\mathit{w}}_V$$

(46)

where ${\tau }_V$ is the correlation time of Markov process and ${\mathit{w}}_V$ is the zero-mean Gaussian white noise.

Then, the dynamic model of the integrated system can be described by state equations as follow:

$$\left[\begin{array}{c}{\dot{\mathit{x}}}_{\mathit{SINS}}\\ {\dot{\mathit{x}}}_{\mathit{DVL}}\end{array}\right]=\left[\begin{array}{cc}{\mathit{F}}_{SINS}& 0_{15\times 3}\\ 0_{3\times 15}& {\mathit{F}}_{DVL}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{SINS}\\ {\mathit{x}}_{DVL}\end{array}\right]+\left[\begin{array}{cc}{\mathit{B}}_{SINS}& 0_{15\times 3}\\ 0_{3\times 6}& {\mathit{B}}_{DVL}\end{array}\right]\left[\begin{array}{c}{\mathit{w}}_{SINS}\\ {\mathit{w}}_{DVL}\end{array}\right]$$

(47)

where $\mathit{F}$ is the system matrix; $\mathit{B}$ is the noise transition matrix; and $\mathit{w}$ is the system noise. ${\mathit{w}}_{SINS}={\left[\begin{array}{cc}{\mathit{\epsilon }}_w^b& {\mathit{\nabla }}_w^b\end{array}\right]}^{\mathrm{T}}$, ${\mathit{w}}_{DVL}= {\mathit{w}}_V$, and

$${\mathit{F}}_{SINS}=\left[\begin{array}{ccccc}-{\mathit{\omega }}_{iG}^G\times & {\mathit{C}}_{V\_\varphi }& {\mathit{C}}_{R\_\varphi }& -{\mathit{C}}_b^G& 0_{3\times 3}\\ {\mathit{f}}^G\times & {\mathit{C}}_{V\_V}& {\mathit{C}}_{R\_V}& 0_{3\times 3}& {\mathit{C}}_b^G\\ 0_{3\times 3}& {\mathit{C}}_{V\_R}& {\mathit{C}}_{R\_R}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

(48)

$${\mathit{B}}_{SINS}= \left[\begin{array}{cc}-{\mathit{C}}_b^G& 0_{3\times 3}\\ 0_{3\times 3}& {\mathit{C}}_b^G\\ 0_{9\times 3}& 0_{9\times 3}\end{array}\right]$$

(49)

$${\mathit{F}}_{DVL}= \left[\begin{array}{ccc}-1/{\tau }_{Ve}& 0& 0\\ 0& -1/{\tau }_{Vn}& 0\\ 0& 0& -1/{\tau }_{Vu}\end{array}\right]$$

(50)

$${\mathit{B}}_{DVL}= {\mathit{I}}_{3\times 3}$$

(51)

where ${\mathit{C}}_{V\_\varphi }={\mathit{C}}_3$, ${\mathit{C}}_{R\_\varphi }=({\mathit{C}}_6+{\mathit{C}}_2){\mathit{C}}_{R2P}+({\mathit{C}}_5+{\mathit{C}}_4){\mathit{C}}_{R2\sigma }$, ${\mathit{C}}_{R\_v}= ({\mathit{V}}^G\times )(2{\mathit{C}}_6+{\mathit{C}}_2){\mathit{C}}_{R2P}+({\mathit{V}}^G\times )(2{\mathit{C}}_5+{\mathit{C}}_4){\mathit{C}}_{\mathit{R}2\sigma }$, ${\mathit{C}}_{v\_v}=({\mathit{V}}^G\times ){\mathit{C}}_3-(2{\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G)\times$, and ${\mathit{C}}_{V\_R}={\mathit{C}}_G^e$, ${\mathit{C}}_{R\_R}=-{\mathit{C}}_G^e({\mathit{V}}^G\times ){\mathit{C}}_{R2\delta \theta }$.

4.2. Observation Model

Both the grid SINS and DVL can output vehicle velocity. The velocity error between grid SINS and DVL is chosen as the system observation. Besides, the DVL installation error and scale factor error are compensated by calibration and neglected in this paper. According to Equation (42), the DVL output in $G$ frame can be obtained by:

$${\tilde{\mathit{V}}}_{DVL}^G={\mathit{V}}^G+({\mathit{V}}_{DVL}^G\times )\mathit{\phi }+\delta {\mathit{V}}_{DVL}^G$$

(52)

where ${\mathit{V}}^G$ is the actual vehicle velocity in the $G$ frame and $\delta {\mathit{V}}_{DVL}^G$ is the random velocity error of DVL.

The grid SINS output velocity can be described as:

$${\tilde{\mathit{V}}}_{SINS}^G={\mathit{V}}^G+\delta {\mathit{V}}_{SINS}^G$$

(53)

where $\delta {\mathit{V}}_{SINS}^G$ is the SINS output error in $G$ frame.

The observation model of integrated system can be given by:

$$\mathit{Z}={\tilde{\mathit{V}}}_{SINS}^G-{\tilde{\mathit{V}}}_{DVL}^G=\mathit{H}\mathit{x}+\mathit{\nu }$$

(54)

where the observation matrix $\mathit{H}$ can be described as $\mathit{H}=\left[\begin{array}{ccc}{\mathit{V}}_{DVL}^G\times & {\mathit{I}}_{3\times 3}& 0_{12\times 3}\end{array}\right]$, and $\mathit{\nu }$ is the system measurement noise.

4.3. The Adaptive Robust Kalman Filter

Because the DVL output errors with imprecise or unknown statistics are hard to be modeled, an ARKF is employed to restrain the uncertain measurement errors and improve the filter accuracy. The adaptive factor is used in the ARKF algorithm to adjust the filter algorithm. The ARKF can detect and reduce the measurement mutation random errors.

The ARKF is based on M-estimation, and the filter steps are described as the follows [28,29]:

$${\widehat{\mathit{X}}}_{k,k-1}={\mathit{\Phi }}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}$$

(55)

$${\mathit{P}}_{k/k-1}={\mathit{\Phi }}_{k,k-1}{\mathit{P}}_{k-1}{\mathit{\Phi }}_{{}^{{}_{k,k-1}}}^T+{\mathit{\Gamma }}_{k-1}{\mathit{Q}}_{k-1}{\mathit{\Gamma }}_{k-1}^T$$

(56)

$${\mathit{K}}_k={\mathit{P}}_{k/k-1}{\mathit{H}}_k^T{\left({\mathit{H}}_k{\mathit{P}}_{k/k-1}{\mathit{H}}_k^T+{\overline{\mathit{R}}}_k\right)}^{-1}$$

(57)

$${\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k/k-1}+{\mathit{K}}_k({\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_{k/k-1})$$

(58)

$${\mathit{P}}_k=\left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{P}}_{k/k-1}{\left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right)}^T+{\mathit{K}}_k{\overline{\mathit{R}}}_k{\mathit{K}}_k^T$$

(59)

The institute of geodesy and geophysics (IGG) III function of the adaptive factor ${\alpha }_i$ is obtained by [28,29]:

$${\alpha }_i=\{\begin{array}{cc}1& \left|\Delta {{\overline{V}}^i}_{k,k-1}\right|\le c_0\\ \frac{c_0}{\left|\Delta {{\overline{V}}^i}_{k,k-1}\right|}{\left(\frac{c_1-\left|\Delta {{\overline{V}}^i}_{k,k-1}\right|}{c_1-c_0}\right)}^2& c_0\le \left|\Delta {{\overline{V}}^i}_{k,k-1}\right|\le c_1\\ 0& \left|\Delta {{\overline{V}}^i}_{k,k-1}\right|>c_1\end{array}$$

(60)

where $\left|\Delta {{\overline{V}}^i}_{k,k-1}\right|$ is the standardized predicted residual of measurement: $\left|\Delta {{\overline{V}}^i}_{k,k-1}\right|=\left|{\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_{k/k-1}\right|/{\sigma }_R$; and ${\sigma }_R$ is the standard deviation of measurement noise. $\left|\Delta {\overline{V}}^i{}_{k,k-1}\right|$ can reflect the measurement accuracy to some extent. $c_0$ and $c_1$ are constant thresholds of $\left|\Delta {{\overline{V}}^i}_{k,k-1}\right|$ and $1.0<c_0<1.5$, $3.0<c_1<4.5$. When the measurement contains small errors, ${\alpha }_i=1$ and the measurement is involved in filter update with original value. When the measurement contains unpredicted measurement errors, $0<{\alpha }_i<1$ and the weight of measurement is reduced. When the measurement contains large unpredicted measurement errors, ${\alpha }_i=0$ and the measurement is rejected to participate in the filter update.

The equivalent covariance of the measurement noise is related to the DVL measurement, and the equivalent covariance ${\overline{\mathit{R}}}_k$ can be obtained by [28,29]:

$${\overline{\mathit{R}}}_k=\mathit{R}/{\alpha }_i$$

(61)

When the ARKF is applied to the grid SINS/DVL integrated navigation system, measurement outliers will be isolated or weighted less to enhance the accuracy of navigation system.

4.4. The ARKF Based Hybrid-Correction Scheme

In this paper, considering the grid SINS/DVL integrated system character, an ARKF based hybrid-correction scheme is proposed to improve the navigation accuracy.

The ARKF based hybrid-correction scheme contains three parts, i.e., an output-correction channel, a feedback-correction channel and a two-order switching criterion. The ARKF based hybrid-correction integrated navigation structure diagram is shown in Figure 5.

A certain correction period is given by the empirical value. As shown in Figure 6, a correction period contains two stages, i.e., an output-correction stage and a feedback-correction stage. The output-correction stage is nine times as long as the filter update period, and is always at the beginning of every correction period. During the output-correction stage, the output-correction operates at the same frequency with the filter update. The feedback-correction stage is as long as the filter update period, and is always at the end of every correction period. Besides, the feedback-correction operates at most one time during every correction period.

The two-order switch criterion is used to choose the output-correction channel or the feedback-correction channel, which is shown in Figure 7.

As shown in Figure 7, the first order switch criterion is a time criterion. In the output-correction stage, the output errors will be restrained by the output-correction to ensure the navigation output accuracy. Then the SINS inner errors will be corrected by the feedback-correction in the feedback-correction stage, which will ensure the filter state equations to match the actual system properly. Besides, the feedback correction frequency of the hybrid-correction scheme is lower than the filter update frequency, so the filter errors of hybrid-correction system have less influence to the grid SINS performance than the traditional feedback-correction system. Although the hybrid-correction scheme with time switch criterion is able to improve navigation performance to some extent, a part of filter errors will still be feedback into the grid SINS, especially when the DVL output errors contain large amplitude or mutation. To further resist the filter errors, the ARKF with an adaptive factor ${\alpha }_i$ is used and a second order switch criterion is designed as a threshold criterion. The ARKF adaptive factor ${\alpha }_i$ is used to detect and resist the unpredicted measurement errors. During the feedback-correction stage, if the DVL operates properly and ${\alpha }_i\ge threshold$, the correction channel will change from the output-correction channel to the feedback-correction channel to correct the grid SINS inner errors; if the DVL output measurement contains large noise and ${\alpha }_i<threshold$, the correction channel will not change from the output-correction channel to the feedback-correction channel, so that the filter errors will not be introduced into the grid SINS.

5. Experiment Results and Discussions

To validate the necessity and advantages of the filter model with unified Earth model, the ARKF algorithm and the ARKF based hybrid-correction scheme, the simulations and experiments are conducted in middle and high latitude regions respectively.

To estimate the performances of the designed algorithm, the sea state and vehicle motion status are also considered. The sea state is set as moderate condition, and the motion status includes static, motion with constant velocity, motion with constant acceleration, climbing and turning a corner. The main IMU and motion parameters of the grid SINS trajectory generator are described as follows.

Firstly, the gyroscopes drifts are set as 0.03° per hour and the random errors of gyroscopes are set up as zero-mean Gaussian white noises. The accelerometer biases are set as 5 × 10⁻⁵ and the random errors of accelerometers are set up as zero-mean Gaussian white noises.

Secondly, the attitude of the vehicle is set as a sine function to simulate the influence of the moderate sea state. The amplitude and period (denoted as amplitude/period) of pitch angle, roll angle and yaw angle are set as $1°/9 \mathrm{s}$, $2°/6 \mathrm{s}$ and $3°/8 \mathrm{s}$, respectively.

Thirdly, the acceleration of the vehicle is set as $0.2 \mathrm{m}/{\mathrm{s}}^2$ and the vehicle has the maximum speed $5 \mathrm{m}/\mathrm{s}$. The middle latitude and longitude of the initial position $P_1$ is set as $(45° \mathrm{N}, 126° \mathrm{E})$. The high latitude and longitude of the initial position $P_2$ is set as $(75° \mathrm{N}, 126° \mathrm{E})$.

Finally, the DVL output errors are set up according to the different experiment requirements, which will be illustrated in the following sections.

The simulation experiments compare different filter models, correction schemes and filter algorithms.

5.1. Simulation Results and Discussions

5.1.1. Filter Models

There are two integrated filer models based on two different grid SINS error equations discussed in this section. The first model assumes the Earth as a sphere and the second model proposed in this paper assumes the Earth as a reference ellipsoid. Other parameters of the two methods are as follows: the DVL output errors are set up as the zero-mean Gaussian white noises. The KF is chosen as the filter algorithm. The output-correction is chosen as correction scheme and the simulation time is set up to 8 h.

It is noted that the integrated navigation system based on the two filter models can both restrain the errors of grid SINS, but the proposed model can unify the Earth model of the SINS mechanization and the integrated filter. The simulation experiment is to compare the navigation performance of integrated filter models and choose the better one.

The navigation errors in middle latitude regions are depicted in Figure 8 and

Table 1. Integrated navigation position errors in the middle latitude regions.

Parameters	Model	Maximum	RMS
Latitude/(′)	Sphere	0.8088	0.3710
	Reference ellipsoid	0.6774	0.3106
Longitude/(′)	Sphere	3.6926	1.2787
	Reference ellipsoid	2.9876	1.0331
Height/(km)	Sphere	1.8792	1.5127
	Reference ellipsoid	1.6732	1.2953

.

The navigation errors in high latitude regions are depicted in Figure 9 and

Table 2. Integrated navigation position errors in the high latitude regions.

Parameters	Model	Maximum	RMS
Latitude/(′)	Sphere	0.1968	0.0894
	Reference ellipsoid	0.1708	0.0776
Longitude/(′)	Sphere	5.0615	1.9602
	Reference ellipsoid	4.4108	1.7159
Height/(km)	Sphere	1.5175	1.3707
	Reference ellipsoid	1.4780	1.2854

.

As shown in Figure 8 and Figure 9, compared with the grid SINS, the oscillation and accumulated errors of the grid SINS output have been restrained by the integrated navigation algorithm. Both grid SINS/DVL integrated navigation methods have a higher accuracy than the grid SINS. Moreover, the integrated navigation system with unified reference ellipsoid Earth model has higher accuracy, less accumulated position error and smaller oscillated error amplitude, compared to the traditional integrated algorithm with sphere based filter model.

As shown in Table 1 and Table 2, when the Earth model is changed from the sphere to the reference ellipsoid, the accuracies of latitude, longitude and height are improved 16.28%, 19.21% and 14.37% (RMS), and 16.25%, 19.09% and 10.96% (maximum error), respectively, in middle latitude regions, and the accuracies of latitude, longitude and height are improved 13.20%, 12.46% and 6.22% (RMS), and 13.21%, 12.86% and 2.61% (maximum error), respectively, in high latitude regions. When choosing the ellipsoid based dynamic model, the position accuracy improves more in middle latitude regions than in high latitude regions. Because the Earth model influences the position accuracy more in middle latitude regions than in high latitude regions, and the latitude would influence the accuracy of other navigation parameters in the navigation system.

The simulation results show that the proposed reference ellipsoid based filter model can provide a better accuracy than the traditional filter model because of the unity of Earth models.

5.1.2. Correction Schemes

There are three correction schemes discussed in this section: the output-correction, the feedback-correction and the hybrid-correction. All three correction schemes can be used in the grid SINS/DVL integrated navigation system to resist SINS navigation errors. The former two schemes are traditional methods and the third scheme is the one proposed in this paper. The simulation experiment is to compare the performance of the three correction schemes.

In this simulation, the DVL output errors are set up as zero-mean Gaussian white noises. The reference ellipsoid based model is chosen as the integrated filter model which has been discussed above. The KF is chosen as the filter algorithm, which means the hybrid-correction only have the first order time criterion. The simulation time is set up to 24 h.

The navigation errors in middle latitude regions are depicted in Figure 10.

The navigation errors in high latitude regions are depicted in Figure 11.

As shown in Figure 10 and Figure 11, if the integrated system works less than 10 h, the output-correction system can maintain a certain accuracy. However, if the integrated system works for long hours, the output-correction system navigation errors will be accumulated and even larger than grid SINS independently, because the SINS inner errors are not corrected and the filter model does not match the actual SINS state. In contrast, the filter of the feedback-correction system would not diverge, as the SINS inner errors are corrected after the filter estimation. However, the feedback-correction system is sensitive to DVL output errors, although the DVL errors are set as zero-mean Gaussian white noises.

The navigation error curves of the hybrid-correction system are shown in Figure 10 and Figure 11. The hybrid system has an output-correction channel, so the navigation output errors are corrected. The output correction frequency is nearly the same as filter update frequency, and the navigation output accuracy is maintained in short term. Besides, the hybrid system also has a feedback-correction channel, so the inner errors of SINS are corrected periodically and the navigation system will maintain a proper accuracy even working for long hours. The feedback channel of the hybrid system operates at a lower frequency than the filter update, so the DVL output errors of the hybrid-correction system have less influence to the integrated navigation accuracy than the feedback-correction system.

Therefore, the grid SINS/DVL integrated navigation system with the hybrid-correction scheme has a higher navigation accuracy and better performance than traditional correction schemes.

5.1.3. KF and ARKF Algorithms

The simulation experiments mentioned above all assume that DVL output errors are zero-mean Gaussian white noises, and the filter algorithm is the KF. However, when the DVL operates in practice, the DVL output may contain different kinds of errors, such as the normal DVL output noises, the unpredicted large random errors, the velocity of the ocean current, etc. In this case, the KF will not work in optimal state. To illustrate the filter algorithm, the DVL output errors in trajectory generators are adjusted to simulate the errors in practice.

There are two integrated filter algorithms discussed in this simulation experiment. The reference ellipsoid based filter model is chosen as the integrated model which has been discussed above. The filter algorithms are the KF and ARKF, respectively, so the correction schemes are the KF based hybrid-correction and the ARKF based hybrid-correction, respectively. When the DVL fails to maintain bottom lock in short time, the velocity of ocean current will be considered as a constant. The DVL output errors are shown in Figure 12.

When the DVL works properly, the errors are set up as zero-mean Gaussian white noises. When the DVL fails to maintain bottom lock during $1.0 \mathrm{h}<t<1.01 \mathrm{h}$ and $2.0 \mathrm{h}<t<2.02 \mathrm{h}$, the constant errors $2 \mathrm{m}/\mathrm{s}$ and $-2 \mathrm{m}/\mathrm{s}$ are set to simulate the situation that the DVL provides velocity relative to the ocean current. During $3.0 \mathrm{h}<t<3.1 \mathrm{h}$, the errors with amplitudes about $1.5 \mathrm{m}/\mathrm{s}$ are set to simulate the situation that the DVL provides velocity with large random noises.

The errors of the grid SINS/DVL integrated navigation in middle latitude regions are depicted in Figure 13.

The errors of the grid SINS/DVL integrated navigation in high latitude regions are depicted in Figure 14.

As shown in Figure 13 and Figure 14, the hybrid-correction grid SINS/DVL integrated navigation system has different navigation performance based on different filter algorithms.

During the first one hour, the DVL works properly, the KF and ARKF have the same performance.

If the KF is chosen as the filter algorithm, large oscillation errors appear in the velocity information when the DVL fails to maintain bottom lock. At the same time, large oscillation errors and accumulated errors appear in the position information. The KF is sensitive to measurement noises and unable to restrain measurement outliers. The DVL constant errors and large amplitude random errors will increase the filter errors. When the large filter errors are introduced into the grid SINS through feedback channel, it will lead to the decrease of the navigation accuracy. Besides, the DVL constant errors have a larger influence to the navigation accuracy than large amplitude random errors.

If the ARKF is chosen as the filter algorithm, the oscillation amplitudes of velocity and position are much smaller than the KF based system and the longitude error barely accumulates with time. The reason is that the ARKF employs an adaptive factor to detect and reduce the influence of measurement uncertain errors. Meanwhile, when the adaptive factor detects a large measurement error, the switching criterion of the ARKF based hybrid-correction can reject the feedback-correction channel to avoid filter errors to be introduced into the grid SINS. Therefore, the ARKF based hybrid-correction system has a higher accuracy and stronger robustness than the traditional KF based hybrid-correction integration.

5.2. Semi-Physical Experiment Results and Discussions

Due to the geographic restriction of the field experiment especially the polar experiment, a semi-physical experiment is conducted in this paper. The key point of the semi-physical simulation experiment is to get the actual sensor output errors, including the IMU output errors and the DVL output errors. The actual sensor errors can reflect the actual error characteristic of the sensors. Meanwhile, the grid based SINS trajectory generator can provide the ideal sensor output data. Then, a set of semi-physical experiment sensor output data can be obtained by adding the actual sensor errors to the ideal sensor output. The semi-physical data can reflect the performance of the navigation algorithm more properly when it is used in experiments.

5.2.1. The Output Error Extraction from IMU

A turntable experiment is designed to extract the IMU output errors. An IMU is installed on a high-precision three-axis turntable shown in Figure 15. The main parameters of the three-axis turntable and IMU are shown in

Table 3. The main parameters of the three-axis turntable.

	Outer Axis	Middle Axis	Inner Axis	Unit
Angular position accuracy	±3/1	±3/1.5	±3/1	arc-sec
Minimum angular rate	±0.001	±0.001	±0.001	${}^{\circ }/\mathrm{s}$
Maximum angular rate	±180	±250	±400	${}^{\circ }/\mathrm{s}$
Angular rate accuracy and stability	5 × 10−5	5 × 10−5	5 × 10−5	${}^{\circ }/\mathrm{s}$
Angular rate resolution	0.0001	0.0001	0.0001	${}^{\circ }/\mathrm{s}$

and

Table 4. The main parameters of the IMU.

	Constant Bias	Random Bias
Gyroscope	<0.005 ${}^{\circ }/\mathrm{s}$	<0.005 ${}^{\circ }/\mathrm{s}$
Accelerometer	<7 × 10−5 g	<5 × 10−5 g

.

The IMU actual output data, i.e., ${\tilde{\mathit{\omega }}}_{ib}^b$ and ${\tilde{\mathit{f}}}_{ib}^b$, along with the turntable’s movement are collected. Meanwhile, the turntable’s movement data are used in the trajectory generator to output the ideal IMU measurement data, i.e., ${\mathit{\omega }}_{ib}^b$ and ${\mathit{f}}_{ib}^b$. Then, the actual output errors of the IMU can be obtained from the difference of the IMU actual output data and the ideal IMU output data.

The actual output of the gyroscope and accelerometer can be presented as:

$$\{\begin{array}{l}{\tilde{\mathit{\omega }}}_{ib}^b={\mathit{\omega }}_{ib}^b+\delta {\mathit{\omega }}_{ib}^b\\ {\tilde{\mathit{f}}}_{ib}^b={\mathit{f}}_{ib}^b+\delta {\mathit{f}}_{ib}^b\end{array}$$

(62)

Then, the actual IMU output error can be obtained by:

$$\{\begin{array}{l}\delta {\mathit{\omega }}_{ib}^b={\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\omega }}_{ib}^b\\ \delta {\mathit{f}}_{ib}^b={\tilde{\mathit{f}}}_{ib}^b-{\mathit{f}}_{ib}^b\end{array}$$

(63)

In the semi-physical integrated navigation experiment, the IMU error data $\delta {\mathit{\omega }}_{ib}^b$ and $\delta {\mathit{f}}_{ib}^b$ gained from the above turntable experiment can reflect the actual error characteristic of the gyroscopes and accelerometers.

5.2.2. The Output Error Extraction from DVL

A ship-mounted experiment is carried out in the Dalian sea area. The DVL error characteristic is susceptible to the vehicle motion status and the working environment, and a set of DVL actual output errors is extracted according to this experiment.

A high precision SINS/GNSS integrated navigation system and a DVL are mounted on a ship and both can provide ship velocity, respectively. The main parameters of the SINS/GNSS navigation system and DVL are shown in

Table 5. The main parameters of the SINS/GNSS integrated navigation system.

Velocity Accuracy (m/s)		Time Accuracy	Data Rate
Horizontal	Vertical	20 ns	20 Hz
0.015	0.010

and

Table 6. The main parameters of the DVL.

Velocity Accuracy	Velocity Range	Data Rate
±1.15% ± 0.2 cm/s	±17.0 m/s	12 Hz max

.

The ship trajectory in this experiment is shown in Figure 16. During the experiment, the ship has different motions, including the acceleration, deceleration, corner operation and turning tail.

The output velocity of the SINS/GNSS integrated navigation system is donated as ${\tilde{\mathit{V}}}_{SINS/GNSS}$ and the DVL output velocity is donated as ${\tilde{\mathit{V}}}_{DVL}$. After the level arm compensation and calibration, both provide the velocity of the vehicle which is expressed in the navigation frame, i.e., ${\tilde{\mathit{V}}}_{SINS/GNSS}^n$ and ${\tilde{\mathit{V}}}_{DVL}^n$. The output velocity of the SINS/GNSS integrated navigation system is much more accurate than the DVL and is considered as the reference velocity $\mathit{V}$ of the actual vehicle, so the DVL output can be expressed as:

$${\tilde{\mathit{V}}}_{DVL}^n=\mathit{V}+\delta {\mathit{V}}_{DVL}$$

(64)

Then, the DVL output errors can be calculated by:

$$\delta {\mathit{V}}_{DVL}={\tilde{\mathit{V}}}_{DVL}^n-\mathit{V}$$

(65)

The DVL output errors are shown in Figure 17.

As shown in Figure 17, the DVL output contains three kinds of errors, i.e., the noise with minor amplitude when operate properly, the noises with large amplitude and constant velocity errors when failed to maintain bottom lock.

The DVL error data $\delta {\mathit{V}}_{DVL}$ gained from the ship-mounted experiment can reflect the actual DVL error characteristic when it is used in the semi-physical integrated navigation experiment.

5.2.3. Semi-Physical Grid SINS/DVL Integrated Navigation Experiments

The ideal sensor output data have been generated by the grid trajectory generator, including the gyroscopes output ${\mathit{\omega }}_{ib}^b$, the accelerometer output ${\mathit{f}}_{ib}^b$ and the DVL output $\mathit{V}$. Meanwhile, the actual noises $\delta {\mathit{\omega }}_{ib}^b$ $\delta {\mathit{f}}_{ib}^b$ and $\delta {\mathit{V}}_{DVL}$ have also been extracted from the turntable experiment and the ship-mounted experiment. Therefore, a set of semi-physical experiment sensor output data (i.e., ${\tilde{\mathit{\omega }}}_{ib}^b$ ${\tilde{\mathit{f}}}_{ib}^b$ and ${\tilde{\mathit{V}}}_{DVL}$) can be obtained by adding the actual sensor noises to the ideal sensor output.

There are three different kinds of grid SINS/DVL integrated navigation algorithm compared in this experiment. The former two algorithms are traditional methods. These two traditional algorithms choose the sphere as the Earth model in KF filter models, which means that the integrated system operates with different Earth models between the grid SINS mechanization and the integrated filter. The first traditional algorithm chooses the output-correction scheme and the KF, and the second traditional algorithm chooses the feedback-correction scheme and the KF. The third algorithm is the ARKF based hybrid-correction grid SINS/DVL integrated navigation algorithm proposed in this paper, which chooses the reference ellipsoid as the Earth model to unify the Earth model of the grid SINS mechanization and the integrated filter.

The errors of the grid SINS/DVL integrated navigation in middle latitude regions are depicted in Figure 18.

The errors of grid SINS/DVL integrated navigation in high latitude regions are depicted in Figure 19.

As shown in Figure 18 and Figure 19, during the 8 h experiment, the traditional KF output-correction grid SINS/DVL integration algorithm has the lowest overall accuracy. The navigation outcomes are diverging; the Schuler oscillation is not completely suppressed; and the oscillation amplitude increases gradually. The reason is that the SINS inner errors are not corrected by the integration algorithm.

Compared with the traditional output-correction, the KF feedback-correction algorithm corrects the SINS inner errors in every filter period and the Schuler oscillation is completely suppressed. The attitude and position accuracies are higher than the output-correction system. Due to the weakness of the position observability [30], the position errors are hard to be restrained totally and still accumulate with time. Besides, the KF feedback-correction system is sensitive to the DVL output errors. When the DVL has large noises, the feedback-correction scheme even has less horizontal velocity accuracy than the output-correction scheme.

The proposed ARKF hybrid-correction system has the best performance among the three methods. Compared with the traditional KF output-correction system, the ARKF hybrid-correction system has a higher navigation accuracy. Compared with the traditional KF feedback-correction system, the proposed ARKF hybrid-correction system has the similar attitude accuracy but higher velocity and position accuracy and stronger robustness. The reasons are as follows: firstly, the unified Earth model decreases the principle errors; secondly, the navigation output errors are corrected by the output-correction channel of the hybrid-correction; thirdly, the SINS inner errors are corrected periodically by the feedback-correction channel of the hybrid-correction to ensure the navigation accuracy when working for long hours; fourthly, the ARKF filter can detect and adjust the filter algorithm to restrain the uncertain DVL output errors; and, finally, when the DVL output contains large noises, the residual filter errors after ARKF are rejected with the help of the ${\alpha }_i$ threshold. As a result, the proposed grid SINS/DVL integrated navigation algorithm has a best performance than the traditional methods for marine application.

6. Conclusions

A novel grid SINS/DVL integrated navigation algorithm is proposed to improve the performance of integrated navigation systems in middle-high latitude regions. The grid SINS error equations based on reference ellipsoid are firstly derived, and a more accurate integrated navigation filter model is designed with the unified Earth model to improve the integrated navigation accuracy. To resist the effects of measurement errors, the ARKF is employed and the ARKF based hybrid-correction scheme is proposed, by which the navigation performance is further improved and ensured for a long working time. The results of simulation and semi-physical experiments show that the proposed grid SINS/DVL integrated algorithm can effectively improve the navigation performance even working for long hours both in middle and high latitude regions. In the future, more kinds of external measurements will be employed to improve the integrated navigation accuracy and reliability.