Polar Grid Navigation Algorithm for Unmanned Underwater Vehicles

Abstract

To solve the unavailability of a traditional strapdown inertial navigation system (SINS) for unmanned underwater vehicles (UUVs) in the polar region, a polar grid navigation algorithm for UUVs is proposed in this paper. Precise navigation is the basis for UUVs to complete missions. The rapid convergence of Earth meridians and the serious polar environment make it difficult to establish the true heading of the UUV at a particular instant. Traditional SINS and traditional representation of position are not suitable in the polar region. Due to the restrictions of the complex underwater conditions in the polar region, a SINS based on the grid frame with the assistance of the OCTANS and the Doppler velocity log (DVL) is chosen for a UUV navigating in the polar region. Data fusion of the integrated navigation system is realized by a modified fuzzy adaptive Kalman filter (MFAKF). By neglecting the negative terms, and using T-S fuzzy logic in the adaptive regulation of the noise covariance, the proposed filter algorithm can improve navigation accuracy. Simulation and experimental results demonstrate that the polar grid navigation algorithm can effectively navigate a UUV sailing in the polar region.

1. Introduction

Unmanned underwater vehicles (UUVs) have been widely used in underwater resource exploration, military reconnaissance, marine surveying, and marine mapping [1,2]. Without the operation of humans, UUVs can carry out the missions autonomously. With the rapid development of underwater navigation technology, humans can explore polar underwater resources through UUVs.

Precise navigation is the premise and basis for UUV sailing. A large number of navigation algorithms have been proposed for UUVs. According to the principle, UUV navigation can be divided into acoustic navigation, visual navigation, inertial navigation, radio navigation, satellite navigation, and so on. Inertial navigation is the most widely used because of its high autonomy. Filtering methods are another important factor that affects navigation accuracy. Filters are widely used in data fusion, such as the Kalman filter (KF), extended Kalman filter (EKF) [3,4], unscented Kalman filter (UKF) [5], and adaptive Kalman filter (AKF). A number of successful tests have also been conducted on these navigation strategies, such as the off-line verification of the UKF-based navigation algorithm for the Typhoon AUV [6]. Cooperative navigation is an effective strategy for efficiently performing tasks while reducing costs. It is also a significant approach for solving the navigation problem in middle-depth of underwater conditions [7]. Algorithms proposed to realize the cooperative navigation include tetrahedral configuration [8], measurement distribution framework [9], and so on. Scholars have done a great deal of research for UUV navigating in the non-polar region and have made a large number of encouraging achievements.

Different from the navigation in the non-polar areas [10], some common navigation methods cannot be applied in the polar region. There are restrictions from the serious environment and the complex underwater conditions in the polar region. As radio signals cannot be spread underwater, Global Positioning System (GPS) is unavailable for UUVs. Meanwhile, the rapid convergence of Earth meridians makes it difficult to establish the true heading of the UUV in the polar region [11]. Therefore, a strapdown inertial navigation system (SINS), which is highly autonomous and stable, becomes the first choice for UUV navigating in the polar region [12]. However, there are some difficulties in the application of the traditional north-oriented SINS algorithm in the polar region, such as calculation overflow and errors increasing sharply. Due to the rapid convergence of Earth meridians, small measurement errors would lead to significant calculation errors. To solve these problems, a transversal SINS algorithm is proposed to replace the traditional SINS algorithm in the polar region [13]. Although a system reset is used to restrain the error drifts, there are principle errors in the transversal SINS algorithm. The transversal SINS ignores the ellipse of the Earth.

Therefore, it is important to choose an available form of SINS for UUVs navigating in the polar region. The polar grid navigation algorithm, which has already helped large aircraft flying though the polar region [14], is the best choice. Selecting the Greenwich meridian as the reference line, the polar grid navigation algorithm can solve the problems caused by the meridians’ convergence [15]. The definition of the grid frame has been given in [16,17] briefly. However, the detailed definition and the derivation of the SINS are not proposed in these papers. The error of approximating a large circle to a straight line in the grid frame is discussed in [18]. The attitude differential equation based on the grid frame is derived in [19]. Although some aspects of the navigation algorithm based on the grid frame are analyzed in the above papers, the complete error model and the filter model are not established. Additionally, these papers did not combine with the specific objects and some specific characters were not taken into consideration. Only in [14,20] was the navigation algorithm based on the grid frame combined with a large aircraft. Different from the flight environment of large aircraft, which has high speed, high precision, and auxiliary forms of a variety of external information, the underwater environment of the UUV is more complex with a lack of radio signal, low visibility, and high requirements of concealment. This navigation algorithm cannot be used on UUVs directly. Humans can help to make decisions during aircraft flights, while UUVs sail alone without the help of humans. The underwater environment is more complex than that of air. For example, large aircraft are affected by wind while UUVs are affected by ocean waves, ocean currents, etc. Therefore, an integrated navigation algorithm is needed to improve the navigation accuracy. Based on the analysis above, the polar grid navigation algorithm for UUVs is proposed considering the underwater environment and the characters of UUV.

In order to improve the navigation accuracy, the OCTANS and the Doppler velocity log (DVL) are chosen as the assistants for the long-endurance navigation of UUVs in the polar region. Combined with the SINS, a SINS/OCTANS/DVL integrated navigation system is constituted. Meanwhile, data fusion of the integrated navigation system is realized by a modified fuzzy adaptive Kalman filter (MFAKF).

In this paper, a polar grid navigation algorithm for UUVs is proposed. Based on the grid frame, the SINS is used for navigating the UUV in the polar region. With the assistance of the OCTANS and DVL, a SINS/OCTANS/DVL integrated navigation system is established. The main contribution of this paper is the derivation of the complete error model and filter model of the polar grid navigation based on the characters of the UUV and the proposed modified AKF to improve the navigation accuracy of the UUV. The following sections are arranged as follows: Error equations and filter models are deduced in Section 2 and Section 3, respectively; In Section 4, a modified fuzzy adaptive Kalman filter is proposed; The results of simulations and experiments are expressed in Section 5; In Section 6, the results and error sources are discussed; Finally, the conclusions are drawn in Section 7.

2. Error Equations of the UUV in Polar Grid Navigation

Frames are important for navigation. The transformation relations among frames can reflect motions of the UUV. In this paper, i frame, e frame, g frame, G frame, b frame, n frame, o frame, and m frame represent the inertial frame, Earth-centered Earth-fixed frame, geographic frame, grid frame, body frame of the UUV, navigation frame, body frame of the OCTANS, and the body frame of DVL, respectively. The meridians’ convergence in high-latitude areas has no influence on the grid frame. Therefore, the grid frame (G frame) is chosen as the navigation frame (n frame).

Error equations of the UUV consist of the attitude error equation, velocity error equation, and position error equation. The main error sources of the SINS are from inertial measurement unit (IMU), which is composed of gyroscopes and accelerometers. Their influence on navigation can be obtained from the following error equations. In addition, error models of the OCTANS and DVL are also established in this section.

2.1. Grid Frame (G Frame)

In Figure 1, point $P$ represents the position of the UUV. The grid planes are the planes that are parallel with the Greenwich plane. The grid plane of point $P$ is the grid plane passes through point $P$. The grid north axis lies along the intersection of the grid plane and the tangent plane of the earth passing point $P$. The grid up axis and the geographic up axis coincide. The grid east axis, grid north axis, and grid up axis constitute the right-handed frame that is the grid frame. There is an angle $\sigma$ between the grid north axis and the geographic north axis.

Figure 1. Definition of the grid frame.

As the grid up axis coincides with the geographic up axis, the grid frame can be obtained from the geographic frame by rotating $\sigma$ around the up axis [15]. Therefore, the transform relations among the $g$ frame, $G$ frame, and $e$ frame can be described as:

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

(1)

$${\mathit{C}}_{\mathrm{e}}^g=\left[\begin{array}{ccc}-\sin \lambda & \cos \lambda & 0\\ -\sin L\cos \lambda & -\sin L\sin \lambda & \cos L\\ \cos L\cos \lambda & \cos L\sin \lambda & \sin L\end{array}\right],$$

(2)

$${\mathit{C}}_{\mathrm{e}}^G={\mathit{C}}_{\mathrm{e}}^g{\mathit{C}}_g^G=\left[\begin{array}{ccc}-\mathrm{c}\sigma \mathrm{s}\lambda +\mathrm{s}\sigma \mathrm{c}\lambda \mathrm{s}L& \mathrm{c}\sigma \mathrm{c}\lambda +\mathrm{s}\sigma \mathrm{s}L\mathrm{s}\lambda & -\mathrm{s}\sigma \mathrm{c}L\\ -\mathrm{s}\sigma \mathrm{s}\lambda -\mathrm{c}\sigma \mathrm{s}L\mathrm{c}\lambda & \mathrm{s}\sigma \mathrm{c}\lambda -\mathrm{c}\sigma \mathrm{s}L\mathrm{s}\lambda & \mathrm{c}\sigma \mathrm{c}L\\ \mathrm{c}L\mathrm{c}\lambda & \mathrm{c}L\mathrm{s}\lambda & \mathrm{s}L\end{array}\right],$$

(3)

The angle $\sigma$ can be expressed as:

$$\sin \sigma =\frac{\sin \lambda \sin L}{\sqrt{1-{\cos }^{2}L{\sin }^2\lambda }},$$

(4)

$$\cos \sigma =\frac{\cos \lambda }{\sqrt{1-{\cos }^{2}L{\sin }^2\lambda }},$$

(5)

The one-order increment of Equation (5) can be written as:

$$\delta \sigma =\frac{\sin L}{1-{\cos }^{2}L{\sin }^2\lambda }\delta \lambda +\frac{\sin \lambda cos\lambda cosL}{1-{\cos }^{2}L{\sin }^2\lambda }\delta L,$$

(6)

By substituting Equations (4)–(5) into Equation (3), ${\mathit{C}}_{\mathrm{e}}^G$ can be rewritten as:

$${\mathit{C}}_e^G=\left[\begin{array}{ccc}\frac{-{\mathrm{c}}^{2}L\mathrm{s}\lambda \mathrm{c}\lambda }{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}& \sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }& \frac{-\mathrm{s}\lambda \mathrm{s}L\mathrm{c}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}\\ \frac{-\mathrm{s}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}& 0& \frac{\mathrm{c}\lambda \mathrm{c}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}\\ \mathrm{c}L\mathrm{c}\lambda & \mathrm{c}L\mathrm{s}\lambda & \mathrm{s}L\end{array}\right],$$

(7)

Due to the rapid convergence of Earth meridians in high-latitude areas, small position errors will lead a large divergence in longitude. Thus, latitude and longitude are not suitable to describe the position of the UUV in high-latitude areas. In this paper, the coordinate in $e$ frame ${\mathit{R}}^e\left(x,y,z\right)$ is chosen to express the position of the UUV. Latitude and longitude of the position can be obtained from ${\mathit{R}}^e\left(x,y,z\right)$:

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

(8)

$$x^2+y^2={\left(R_{Nh}\cos L\right)}^2,$$

(9)

Then,

$$\cos L=\frac{\sqrt{x^2+y^2}}{R_{Nh}},$$

(10)

$$\sin L=\pm \sqrt{1-\frac{x^2+y^2}{R_{Nh}^2}},$$

(11)

$$\sin \lambda =\frac{y}{R_{Nh}\cos L}=\frac{y}{\sqrt{x^2+y^2}},$$

(12)

$$\cos \lambda =\frac{x}{R_{Nh}\cos L}=\frac{x}{\sqrt{x^2+y^2}},$$

(13)

The longitude $\lambda$ can be calculated from Equations (12) and (13) directly, while the calculation of latitude $L$ needs the approximate solution [21].

Ignoring the ellipse of the earth, the radius of the earth is approximate to $R_{Mh}\approx R_{Nh}\approx R_{eh}$.

$$\{\begin{array}{l}x=R_{eh}\cos L\cos \lambda \\ y=R_{eh}\cos L\sin \lambda \\ z=R_{eh}\sin L\end{array},$$

(14)

The one-order increment of Equation (14) can be described as:

$$\{\begin{array}{l}\delta x=-R_{eh}\mathrm{s}L\mathrm{c}\lambda \delta L-R_{eh}\mathrm{c}L\mathrm{s}\lambda \delta \lambda +\mathrm{c}L\mathrm{c}\lambda \delta h\\ \delta y=-R_{eh}\mathrm{s}L\mathrm{s}\lambda \delta L+R_{eh}\mathrm{c}L\mathrm{c}\lambda \delta \lambda +\mathrm{c}L\mathrm{s}\lambda \delta h\\ \delta z=R_{eh}\mathrm{c}L\delta L+\mathrm{s}L\delta h\end{array},$$

(15)

$$\delta {\mathit{R}}^e=\left[\begin{array}{c}\delta x\\ \delta y\\ \delta z\end{array}\right]=\left[\begin{array}{ccc}-R_{eh}\sin L\cos \lambda & -R_{eh}\cos L\sin \lambda & \cos L\cos \lambda \\ -R_{eh}\sin L\sin \lambda & R_{eh}\cos L\mathrm{c}\lambda & \cos L\sin \lambda \\ R_{eh}\cos L& 0& \sin L\end{array}\right]\left[\begin{array}{c}\delta L\\ \delta \lambda \\ \delta h\end{array}\right],$$

(16)

Thus,

$$\delta \mathit{P}=\left[\begin{array}{c}\delta L\\ \delta \lambda \\ \delta h\end{array}\right]=\frac{1}{R_{eh}}\left[\begin{array}{ccc}-\sin L\mathrm{c}\lambda & -\sin L\sin \lambda & \cos L\\ -\frac{\sin \lambda }{\cos L}& \frac{\cos \lambda }{\cos L}& 0\\ R_{eh}\cos L\cos \lambda & R_{eh}\cos L\sin \lambda & R_{eh}\sin L\end{array}\right]\delta {\mathit{R}}^e,$$

(17)

2.2. Attitude Error Equation

Due to the restriction of the computer and algorithm, there are errors between the ideal G frame and the actual G frame. The actual G frame in practical application of the SINS is defined as the G’ frame. The attitude differential equation of the UUV in ideal conditions can be expressed as:

$${\dot{\mathit{C}}}_b^G={\mathit{C}}_b^G\left[{\mathbf{\omega }}_{ib}^b\times \right]-\left[{\mathbf{\omega }}_{iG}^G\times \right]{\mathit{C}}_b^G,$$

(18)

where $\left[{\mathbf{\omega }}_{iG}^G\times \right]$ is the anti-symmetric matrix of ${\mathbf{\omega }}_{iG}^G$, which can be expressed as:

$$\left[{\mathbf{\omega }}_{iG}^G\times \right]=\left[{\mathbf{\omega }}_{ie}^G\times \right]+\left[{\mathbf{\omega }}_{eG}^G\times \right],$$

(19)

$${\mathbf{\omega }}_{ie}^G={\mathit{C}}_g^G{\mathbf{\omega }}_{ie}^g=\left[\begin{array}{ccc}\cos \sigma & -\sin \sigma & 0\\ \sin \sigma & \cos \sigma & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ {\omega }_{ie}\cos L\\ {\omega }_{ie}\sin L\end{array}\right]=\left[\begin{array}{c}-{\omega }_{ie}\cos L\sin \sigma \\ {\omega }_{ie}\cos L\cos \sigma \\ {\omega }_{ie}\sin L\end{array}\right],$$

(20)

$${\mathbf{\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 }_{fG}}& -\frac{1}{R_{yG}}\\ \frac{1}{R_{xG}}& -\frac{1}{{\tau }_{fG}}\\ \frac{K_G}{{\tau }_{fG}}& -\frac{K_G}{R_{yG}}\end{array}\right]\left[\begin{array}{c}{v}_{G_E}\\ v_{G_N}\end{array}\right],$$

(21)

where $\frac{1}{R_{xG}}=\frac{{\sin }^2\sigma }{R_{\mathit{M}h}}+\frac{{\cos }^2\sigma }{R_{Nh}}$ and $\frac{1}{R_{yG}}=\frac{{\cos }^2\sigma }{R_{Mh}}+\frac{{\sin }^2\sigma }{R_{Nh}}$ are the equivalent curvature; $\frac{1}{{\tau }_{fG}}=\left(\frac{1}{R_{Mh}}-\frac{1}{R_{Nh}}\right)\sin \sigma \cos \sigma$ is the twist rate of the ellipsoid at point P, and $K_G=\frac{\sin \lambda \cos L}{\sqrt{1-{\cos }^{2}L{\sin }^2\lambda }}=\frac{\sin \lambda \sin L\cos L}{\sin L\sqrt{1-{\cos }^{2}L{\sin }^2\lambda }}=\frac{\cos L}{\sin L}\sin \sigma =\cot L\sin \sigma$. Ignoring the ellipse of the Earth and the influence of distortion, ${\mathbf{\omega }}_{eG}^G$ can be simplified as:

$${\mathbf{\omega }}_{eG}^G=\left[\begin{array}{cc}0& -\frac{1}{R_{eh}}\\ \frac{1}{R_{eh}}& 0\\ 0& -\frac{K_G}{R_{eh}}\end{array}\right]\left[\begin{array}{c}{v}_{G_E}\\ v_{G_N}\end{array}\right]=\left[\begin{array}{c}-\frac{v_{GN}}{R_{eh}}\\ \frac{v_{GE}}{R_{eh}}\\ -\frac{v_{GN}}{R_{eh}}\cot L\mathrm{s}\sigma \end{array}\right],$$

(22)

Due to the errors in practical application, the attitude differential equation of UUV can be written as:

$${\dot{\mathit{C}}}_b^{G^{\prime }}={\mathit{C}}_b^{G^{\prime }}\left[{\widehat{\mathbf{\omega }}}_{ib}^b\times \right]-\left[{\widehat{\mathbf{\omega }}}_{iG}^G\times \right]{\mathit{C}}_b^{G^{\prime }},$$

(23)

where:

$${\widehat{\mathbf{\omega }}}_{ib}^b={\mathbf{\omega }}_{ib}^b+\delta {\mathbf{\omega }}_{ib}^b,$$

(24)

$${\widehat{\mathbf{\omega }}}_{iG}^G={\widehat{\mathbf{\omega }}}_{ie}^G+{\widehat{\mathbf{\omega }}}_{eG}^G,$$

(25)

$${\widehat{\mathbf{\omega }}}_{ie}^G={\mathbf{\omega }}_{ie}^G+\delta {\mathbf{\omega }}_{ie}^G,$$

(26)

$${\widehat{\mathbf{\omega }}}_{eG}^G={\mathbf{\omega }}_{eG}^G+\delta {\mathbf{\omega }}_{eG}^G,$$

(27)

where $\delta {\mathbf{\omega }}_{ib}^b$, $\delta {\mathbf{\omega }}_{ie}^G$, and $\delta {\mathbf{\omega }}_{eG}^G$ are the errors of ${\mathbf{\omega }}_{ib}^b$, ${\mathbf{\omega }}_{ie}^G$, and ${\mathbf{\omega }}_{eG}^G$, respectively. $\delta {\mathbf{\omega }}_{ie}^G$ and $\delta {\mathbf{\omega }}_{eG}^G$ can be defined as:

$$\delta {\mathbf{\omega }}_{ie}^G={\widehat{\mathbf{\omega }}}_{ie}^G-{\mathbf{\omega }}_{ie}^G={\mathbf{\omega }}_{ie}\left[\begin{array}{c}-\cos L\cos \sigma \\ -\cos L\sin \sigma \\ 0\end{array}\right]\delta \sigma +{\omega }_{ie}\left[\begin{array}{c}\sin L\sin \sigma \\ -\sin L\cos \sigma \\ \cos L\end{array}\right]\delta L,$$

(28)

$$\delta {\mathbf{\omega }}_{eG}^G={\widehat{\mathbf{\omega }}}_{eG}^G-{\mathbf{\omega }}_{eG}^G=\left[\begin{array}{ccc}0& 0& \frac{v_{GN}}{R_{eh}^2}\\ 0& 0& -\frac{v_{GE}}{R_{eh}^2}\\ \frac{v_{GN}\sin \sigma }{R_{eh}{\sin }^{2}L}& 0& \frac{v_{GN}\cot L\sin \sigma }{R_{eh}^2}\end{array}\right]\left[\begin{array}{c}\delta L\\ \delta \lambda \\ \delta h\end{array}\right]+\left[\begin{array}{ccc}0& -\frac{1}{R_{eh}}& 0\\ \frac{1}{R_{eh}}& 0& 0\\ 0& -\frac{\cot L\mathrm{s}\sigma }{R_{eh}}& 0\end{array}\right]\left[\begin{array}{c}\delta v_{GE}\\ \delta v_{GN}\\ \delta v_{GU}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -\frac{v_{GN}\cot L\cos \sigma }{R_{eh}}\end{array}\right]\delta \sigma ,$$

(29)

The velocity error can be described as $\delta {\mathit{V}}^G={\left[\delta v_{GE}\delta v_{GN}\delta v_{GU}\right]}^{\mathrm{T}}$. Then, based on Equation (6), $\delta {\mathbf{\omega }}_{ie}^G$ and $\delta {\mathbf{\omega }}_{eG}^G$ can be rewritten as:

$$\delta {\mathbf{\omega }}_{ie}^G={\mathit{C}}_{\omega iep}\delta \mathit{P},$$

(30)

$$\delta {\mathbf{\omega }}_{eG}^G={\mathit{C}}_{\omega eGp}\delta \mathit{P}+{\mathit{C}}_{\omega eGv}\delta {\mathit{V}}^G,$$

(31)

where:

$${\mathit{C}}_{\omega iep}={\omega }_{ie}\left[\begin{array}{ccc}\sin L\sin \sigma -\cot L\cos L{\cos }^2\sigma \sin \sigma & -\frac{\cot L\cos \sigma {\sin }^2\sigma }{{\mathrm{s}}^2\lambda }& 0\\ -\sin L\cos \sigma -\cot L\mathrm{c}L{\sin }^2\sigma \cos \sigma & -\frac{\cot L{\sin }^3\sigma }{{\sin }^2\lambda }& 0\\ \mathrm{c}L& 0& 0\end{array}\right],$$

(32)

$${\mathit{C}}_{\omega eGp}=\left[\begin{array}{ccc}0& 0& \frac{v_{GN}}{R_{eh}^2}\\ 0& 0& -\frac{v_{GE}}{R_{eh}^2}\\ \frac{v_{GN}\mathrm{s}\sigma }{R_{eh}(1-c^{2}L{c}^2\sigma )}& -\frac{v_{GN}\mathrm{c}L\mathrm{c}\sigma }{R_{eh}(1-c^{2}L{\mathrm{s}}^2\lambda )}& \frac{v_{GN}\cot L\mathrm{s}\sigma }{R_{eh}^2}\end{array}\right],$$

(33)

$${\mathit{C}}_{\omega eGv}=\left[\begin{array}{ccc}0& -\frac{1}{R_{eh}}& 0\\ \frac{1}{R_{eh}}& 0& 0\\ 0& -\frac{\cot L\sin \sigma }{R_{eh}}& 0\end{array}\right],$$

(34)

By substituting Equation (17) into Equations (30)–(31) and neglecting the small high-order terms, $\delta {\mathbf{\omega }}_{ie}^G$ and $\delta {\mathbf{\omega }}_{eG}^G$ can be rewritten as:

$$\delta {\mathbf{\omega }}_{ie}^G={\mathit{C}}_{\omega ieR}\delta {\mathit{R}}^e,$$

(35)

$$\delta {\mathbf{\omega }}_{eG}^G={\mathit{C}}_{\omega eGR}\delta {\mathit{R}}^e+{\mathit{C}}_{\omega eGv}\delta {\mathit{V}}^G,$$

(36)

where:

$${\mathit{C}}_{\omega ieR}=\frac{{\omega }_{ie}}{R_{eh}{\left(1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda \right)}^{\frac{3}{2}}}\cdot \left[\begin{array}{ccc}2{\mathrm{c}}^{2}L\mathrm{s}Ls\lambda c\lambda & -\mathrm{s}L\left[{\mathrm{c}}^2\lambda +{\mathrm{s}}^2\lambda \cdot \left({\mathrm{s}}^{2}L-{\mathrm{c}}^{2}L\right)\right]& {\mathrm{s}}^{2}L\mathrm{c}L\mathrm{s}\lambda \\ {\mathrm{s}}^{2}L& 0& -\mathrm{s}L\mathrm{c}L\mathrm{c}\lambda \\ -\mathrm{s}L\mathrm{c}L\mathrm{c}\lambda & -\mathrm{s}L\mathrm{c}L\mathrm{s}\lambda & {\mathrm{c}}^{2}L\end{array}\right],$$

(37)

$${\mathit{C}}_{\omega eGR}=\frac{1}{R_{eh}^2}\cdot \left[\begin{array}{ccc}{v}_{GN}cLc\lambda & v_{GN}\mathrm{c}L\mathrm{s}\lambda & v_{GN}\mathrm{s}L\\ -v_{GE}cLc\lambda & -v_{GE}\mathrm{c}L\mathrm{s}\lambda & -v_{GE}\mathrm{s}L\\ \frac{2v_{GN}{\mathrm{c}}^{2}L\mathrm{s}\lambda \mathrm{c}\lambda }{{\left(1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda \right)}^{\frac{3}{2}}}& -v_{GN}\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }& \frac{2v_{GN}\mathrm{s}L\mathrm{c}L\mathrm{s}\lambda }{{\left(1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda \right)}^{\frac{3}{2}}}\end{array}\right],$$

(38)

where $s(\cdot )$ and $c(\cdot )$ represent $\sin (\cdot )$ and $\cos (\cdot )$, respectively.

The attitude error matrix can be defined as:

$$\mathrm{\Delta }\mathit{C}={\mathit{C}}_b^{G^{\prime }}-{\mathit{C}}_b^G,$$

(39)

$$\mathrm{\Delta }\mathit{C}=\left(I-{\mathit{C}}_{G^{\prime }}^G\right){\mathit{C}}_b^{G^{\prime }},$$

(40)

By substituting Equation (23) into Equation (40), the derivative of $\mathrm{\Delta }\mathit{C}$ can be expressed as:

$$\mathrm{\Delta }\dot{\mathit{C}}={\mathit{C}}_b^{G^{\prime }}\left[{\widehat{\mathbf{\omega }}}_{ib}^b\times \right]-\left[{\widehat{\mathbf{\omega }}}_{iG}^G\times \right]{\mathit{C}}_b^{G^{\prime }}-{\mathit{C}}_{G^{\prime }}^G{\mathit{C}}_b^{G^{\prime }}\left[{\widehat{\mathbf{\omega }}}_{ib}^b\times \right]+{\mathit{C}}_{G^{\prime }}^G\left[{\widehat{\mathbf{\omega }}}_{iG}^G\times \right]{\mathbf{C}}_b^{G^{\prime }}-{\dot{\mathit{C}}}_{G^{\prime }}^G{\mathit{C}}_b^{G^{\prime }},$$

(41)

By substituting Equations (18) and (23), $\mathrm{\Delta }\dot{\mathit{C}}$ can be rewritten as:

$$\mathrm{\Delta }\dot{\mathit{C}}={\mathit{C}}_b^{G^{\prime }}\left[{\widehat{\mathbf{\omega }}}_{ib}^b\times \right]-\left[{\widehat{\mathbf{\omega }}}_{iG}^G\times \right]{\mathit{C}}_b^{G^{\prime }}-{\mathit{C}}_{G^{\prime }}^G{\mathit{C}}_b^{G^{\prime }}\left[{\mathbf{\omega }}_{ib}^b\times \right]+\left[{\mathbf{\omega }}_{iG}^G\times \right]{\mathit{C}}_{G^{\prime }}^G{\mathit{C}}_b^{G^{\prime }},$$

(42)

Combined Equation (41) and (42):

$${\mathit{C}}_{G^{\prime }}^G{\mathit{C}}_b^{G^{\prime }}\left[\delta {\mathbf{\omega }}_{ib}^b\times \right]+\left[{\mathbf{\omega }}_{iG}^G\times \right]{\mathit{C}}_{G^{\prime }}^G{\mathit{C}}_b^{G^{\prime }}-{\mathit{C}}_{G^{\prime }}^G\left[{\widehat{\mathbf{\omega }}}_{iG}^G\times \right]{\mathit{C}}_b^{G^{\prime }}+{\dot{\mathit{C}}}_{G^{\prime }}^G{\mathit{C}}_b^{G^{\prime }}=0,$$

(43)

Based on the similar transformation theory of matrix, Equation (43) can be simplified as:

$$\delta {\mathbf{\omega }}_{ib}^{G^{\prime }}+{\mathbf{\omega }}_{iG}^{G^{\prime }}-{\widehat{\mathbf{\omega }}}_{iG}^G+{\mathbf{\omega }}_{G{G}^{\prime }}^{G^{\prime }}=0,$$

(44)

Thus,

$${\mathbf{\omega }}_{G{G}^{\prime }}^{G^{\prime }}=\left(I-{\mathit{C}}_G^{G^{\prime }}\right){\mathbf{\omega }}_{iG}^G+\delta {\mathbf{\omega }}_{iG}^G-{\mathit{C}}_b^{G^{\prime }}\delta {\mathbf{\omega }}_{ib}^b,$$

(45)

The misalignment angle of the UUV between the $G$ frame and $G^{\prime }$ frame is ${\mathbf{\varphi }}^G={\left[{\varphi }_x^G{\varphi }_y^G{\varphi }_z^G\right]}^{\mathrm{T}}$. Thus, ${\mathbf{\omega }}_{G{G}^{\prime }}^{G^{\prime }}$ can be rewritten as:

$${\mathbf{\omega }}_{G{G}^{\prime }}^{G^{\prime }}=\left[\begin{array}{ccc}\mathrm{c}{\varphi }_y^G& 0& -\mathrm{s}{\varphi }_y^G\mathrm{c}{\varphi }_x^G\\ 0& 1& \mathrm{s}{\varphi }_x^G\\ \mathrm{s}{\varphi }_y^G& 0& \mathrm{c}{\varphi }_y^G\mathrm{c}{\varphi }_x^G\end{array}\right]\left[\begin{array}{c}{\dot{\varphi }}_x^G\\ {\dot{\varphi }}_y^G\\ {\dot{\varphi }}_z^G\end{array}\right]={\mathit{C}}_{\omega }{\dot{\mathbf{\varphi }}}^G,$$

(46)

where:

$${\mathit{C}}_{\omega }=\left[\begin{array}{ccc}\cos {\mathbf{\varphi }}_y^G& 0& -\sin {\mathbf{\varphi }}_y^G\cos {\mathbf{\varphi }}_x^G\\ 0& 1& \sin {\mathbf{\varphi }}_x^G\\ \sin {\mathbf{\varphi }}_y^G& 0& \cos {\mathbf{\varphi }}_y^G\cos {\mathbf{\varphi }}_x^G\end{array}\right],$$

(47)

Thus, based on Equation (45), ${\mathbf{\varphi }}^G$ can be obtained as:

$${\dot{\mathbf{\varphi }}}^G={\mathit{C}}_{\omega }{}^{-1}{\mathbf{\omega }}_{G{G}^{\prime }}^{G^{\prime }}={\mathit{C}}_{\omega }{}^{-1}\left(\left({\mathbf{\varphi }}^G\times \right){\mathbf{\omega }}_{iG}^G+\delta {\mathbf{\omega }}_{iG}^G-{\mathit{C}}_b^{G^{\prime }}\delta {\mathbf{\omega }}_{ib}^b\right),$$

(48)

Considering:

$$\delta {\mathbf{\omega }}_{ib}^b={\mathbf{\epsilon }}^b,$$

(49)

$$\delta {\mathbf{\omega }}_{iG}^G=\delta {\mathbf{\omega }}_{ie}^G+\delta {\mathbf{\omega }}_{eG}^G=\left({\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR}\right)\delta {\mathit{R}}^e+{\mathit{C}}_{\omega eGv}\delta {\mathit{V}}^G,$$

(50)

where ${\mathbf{\epsilon }}^b$ is the gyro drift that consists of the gyro constant drifts ${\mathbf{\epsilon }}_c^b$ and the gyro random drifts ${\mathbf{\epsilon }}_{\mathit{w}}^b$, and ${\mathbf{\epsilon }}_{\mathit{w}}^b$ can be set as zero-mean Gaussian white noise.

Therefore, the attitude error equation of UUV in $G$ frame can be rewritten as:

$${\dot{\mathbf{\varphi }}}^G={\mathit{C}}_{\omega }{}^{-1}(-({\mathbf{\omega }}_{iG}^G\times ){\mathbf{\varphi }}^G+({\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR})\delta {\mathit{R}}^e+{\mathit{C}}_{\omega eGv}\delta {\mathit{V}}^G-{\mathit{C}}_b^{G^{\prime }}{\mathbf{\epsilon }}^b),$$

(51)

2.3. Velocity Error Equation

The velocity differential equation of the UUV in ideal conditions can be described as:

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

(52)

where ${\mathit{f}}^b$ is the special force measured by the SINS.

Due to the errors from the IMU, the velocity differential equation of UUV in practical application can be expressed as:

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

(53)

and:

$$\delta {\mathit{V}}^G={\widehat{\mathit{V}}}^G-{\mathit{V}}^G,$$

(54)

$${\widehat{\mathit{C}}}_b^G={\mathit{C}}_G^{G^{\prime }}{\mathit{C}}_b^G=\left(I-{\mathbf{\varphi }}^G\times \right){\mathit{C}}_b^G,$$

(55)

$${\mathbf{\nabla }}^b={\widehat{\mathit{f}}}^b-{\mathit{f}}^b,$$

(56)

$$\delta {\mathbf{\omega }}_{ie}^G={\widehat{\mathbf{\omega }}}_{ie}^G-{\mathbf{\omega }}_{ie}^G,$$

(57)

$$\delta {\mathbf{\omega }}_{eG}^G={\widehat{\mathbf{\omega }}}_{eG}^G-{\mathbf{\omega }}_{eG}^G,$$

(58)

$$\delta {\mathit{g}}^G={\widehat{\mathit{g}}}^G-{\mathit{g}}^G,$$

(59)

where $\delta {\mathit{V}}^G$, ${\mathbf{\nabla }}^b$, $\delta {\mathbf{\omega }}_{ie}^G$, $\delta {\mathbf{\omega }}_{eG}^G$, and $\delta {\mathit{g}}^G$ are the errors of ${\mathit{V}}^G$, ${\mathit{f}}^b$, ${\mathbf{\omega }}_{ie}^G$, ${\mathbf{\omega }}_{eG}^G$, and ${\mathit{g}}^G$, respectively. ${\mathbf{\nabla }}^b$ is the accelerometer bias which consists of the accelerometer constant bias ${\mathbf{\nabla }}_c^b$ and the accelerometer random bias ${\mathbf{\nabla }}_w^b$., and ${\mathbf{\nabla }}_w^b$ can be set as zero-mean Gaussian white noise [22].

Substituting Equations (54)–(59) into Equation (53), then subtracting Equation (52) from Equation (53) and ignoring the second-order small terms, the velocity error equation can be described as:

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

(60)

Considering Equations (35) and (36), the velocity error equation of the UUV in the $G$ frame can be rewritten as:

$$\delta {\dot{\mathit{V}}}^G={\mathit{f}}^G\times {\mathbf{\varphi }}^G+\left[{\mathit{V}}^G\times {\mathit{C}}_{\omega eG\mathit{v}}-\left(2{\mathbf{\omega }}_{ie}^G+{\mathbf{\omega }}_{eG}^G\right)\times \right]\cdot \delta {\mathit{V}}^G+\left[{\mathit{V}}^G\times \left(2{\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR}\right)\right]\delta {\mathit{R}}^e+{\mathit{C}}_b^G{\mathbf{\nabla }}^b,$$

(61)

2.4. Position Error Equation

Since latitude and longitude are unsuitable for describing the position of the UUV in high-latitude areas, the coordinate in the $e$ frame ${\mathit{R}}^e\left(x,y,z\right)$ is chosen to describe the position. The position differential equation of the UUV in ideal conditions can be written as:

$${\dot{\mathit{R}}}^e={\mathit{C}}_G^e{\mathit{V}}^G,$$

(62)

Similar to the velocity differential equation, the position differential equation of UUV in actual condition can be described as:

$${\dot{\widehat{\mathit{R}}}}^e={\widehat{\mathit{C}}}_G^e{\widehat{\mathit{V}}}^G,$$

(63)

and:

$$\delta {\mathit{R}}^e={\widehat{\mathit{R}}}^e-{\mathit{R}}^e,$$

(64)

$$\delta {\mathit{V}}^G={\widehat{\mathit{V}}}^G-{\mathit{V}}^G,$$

(65)

$${\widehat{\mathit{C}}}_G^e={\mathit{C}}_{G^{″}}^e={\mathit{C}}_G^e{\mathit{C}}_{G^{″}}^G,$$

(66)

Due to the position deviation, the actual position of the UUV and the calculated position in the $G$ frame do not coincide. There are slight errors $\delta L$, $\delta \lambda$, and $\delta \sigma$ between them. The $G$ frame can be obtained by the following three-time rotations from the $G^{″}$ frame:

$$O{x}_{G^{″}}{y}_{G^{″}}{z}_{G^{″}}\underset{\delta L}{\overset{around{x}_{\mathit{g}}axis}{\to }}O{x}_{G^{″}}^{\prime }{y}_{G^{″}}^{\prime }{z}_{G^{″}}^{\prime }\underset{-\delta \lambda }{\overset{around{z}_{e}axis}{\to }}O{x}_{G^{″}}^{″}{y}_{G^{″}}^{″}{z}_{G^{″}}^{″}\underset{\delta \sigma }{\overset{around{\widehat{z}}_{G}axis}{\to }}O{x}_G{y}_G{z}_G,$$

(67)

The position deviation can be expressed as:

$$\delta \mathbf{\beta }=\delta L{\mathit{x}}_{\mathit{g}}-\delta \lambda {\mathit{z}}_e+\delta \sigma {\widehat{\mathit{z}}}_G,$$

(68)

Ignoring the second-order small terms and combining Equation (6), Equation (68) is transformed into the matrix form based on $G$ frame.

$$\delta {\mathbf{\beta }}^G=\left[\begin{array}{ccc}-\cos \sigma & -\cos L\sin \sigma & 0\\ -\sin \sigma & \cos L\cos \sigma & 0\\ -\frac{\cos L\cos \sigma \sin \sigma }{\sin L}& \frac{\cos \sigma \sin \sigma }{\sin \lambda \cos \lambda }+\sin L& 0\end{array}\right]\delta \mathit{P},$$

(69)

$$\delta {\mathbf{\beta }}^G={\mathit{C}}_{\beta R}\delta {\mathit{R}}^e,$$

(70)

where,

$${\mathit{C}}_{\beta R}=\frac{1}{R_{eh}}\left[\begin{array}{ccc}\frac{\mathrm{s}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}& 0& \frac{-c\lambda \mathrm{c}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}\\ \frac{\mathrm{s}\lambda c\lambda c^{2}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}& \sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }& \frac{-s\lambda \mathrm{s}L\mathrm{c}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}\\ \frac{cL\mathrm{s}L\mathrm{s}\lambda }{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }& 0& -\frac{c^{2}L\mathrm{s}\lambda c\lambda }{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }\end{array}\right],$$

(71)

Thus:

$${\widehat{\mathit{C}}}_G^e={\mathit{C}}_G^e{\mathit{C}}_{G^{\prime }}^G={\mathit{C}}_G^e\left(I+\delta {\mathbf{\beta }}^G\times \right),$$

(72)

Substituting Equations (64)–(66) and Equation (72) into Equation (63), then subtracting Equation (62) from Equation (63), the position error equation of the UUV in the grid frame can be obtained as:

$$\delta {\dot{\mathit{R}}}^e={\mathit{C}}_G^e\delta {\mathit{V}}^G-{\mathit{C}}_G^e\left({\mathit{V}}^G\times \right){\mathit{C}}_{\beta R}\delta {\mathit{R}}^e,$$

(73)

2.5. Error Model of OCTANS

The OCTANS is an all-in-one gyrocompass and motion sensor for diverse challenging applications. OCTANS consists of three fiber-optic gyroscopes and three quartz accelerometers. The OCTANS measures the attitude of UUV quickly and accurately. The errors of the OCTANS are caused by the inertially-sensitive elements, including gyroscope drifts and accelerometer bias. The accelerometer bias has little effect on the measurement results comparing with the gyroscope drifts. Therefore, the accelerometer bias can be ignored. The gyro drifts consist of gyro constant drifts and gyro random drifts. Therefore, the error model of OCTANS can be built as follows:

$${\mathbf{\epsilon }}_o^o={\mathbf{\epsilon }}_{\mathit{w}o}^o+{\mathbf{\epsilon }}_{co}^o,$$

(74)

where ${\mathbf{\epsilon }}_{co}^o$ and ${\mathbf{\epsilon }}_{wo}^o$ are the gyro constant drifts and gyro random drifts of the OCTANS in the $o$ frame, respectively. ${\mathbf{\epsilon }}_{wo}^o$ can be set as zero-mean Gaussian white noise and ${\mathbf{\epsilon }}_{co}^o$ can be expressed as:

$${\dot{\mathbf{\epsilon }}}_{co}^o=0,$$

(75)

The error model of the OCTANS expressed in the $G$ frame can be rewritten as:

$${\mathbf{\epsilon }}_o^G={\mathit{C}}_b^G{\mathit{C}}_o^b{\mathbf{\epsilon }}_o^o={\mathit{C}}_b^G{\mathit{C}}_o^b\left({\mathbf{\epsilon }}_{wo}^o+{\mathbf{\epsilon }}_{co}^o\right),$$

(76)

where ${\mathit{C}}_o^b$ is the direction cosine matrix from the $o$ frame to the $b$ frame. The installation error angles of the OCTANS are small enough to be ignored. Then, ${\mathit{C}}_o^b$ is approximate to ${\mathit{C}}_o^b=I$. Equation (76) can be rewritten as:

$${\mathbf{\epsilon }}_o^G={\mathit{C}}_b^{G}I{\mathbf{\epsilon }}_o^o={\mathit{C}}_b^{G}I\left({\mathbf{\epsilon }}_{wo}^o+{\mathbf{\epsilon }}_{co}^o\right)={\mathbf{\epsilon }}_{wo}^G+{\mathbf{\epsilon }}_{co}^G,$$

(77)

Considering ${\mathbf{\epsilon }}_{co}^o$ can be updated by Equation (75), the attitude of the UUV measured by the OCTANS in the $G$ frame can be obtained as:

$${\widehat{\mathsf{\Lambda }}}_o^G={\mathsf{\Lambda }}_o^G+{\mathbf{\epsilon }}_{wo}^G={\tilde{\mathsf{\Lambda }}}_o^G-{\mathbf{\epsilon }}_{co}^G,$$

(78)

where $\mathsf{\Lambda }={\left[\phi \theta \psi \right]}^{\mathrm{T}}$ represents the attitude of the UUV; $\phi$, $\theta$, and $\psi$ represent the roll angle, pitch angle, and yaw angle of the UUV, respectively; ${\widehat{\mathsf{\Lambda }}}_o^G$ is the attitude measured by the OCTANS after partial error compensation in $G$ frame; ${\tilde{\mathsf{\Lambda }}}_o^G$ is the attitude measured by the OCTANS before partial error compensation in the $G$ frame; ${\mathsf{\Lambda }}_o^G$ is the ideal attitude measured by the OCTANS in the $G$ frame, and ${\mathbf{\epsilon }}_{wo}^G$ is zero-mean Gaussian white noise.

2.6. Error Model of DVL

The DVL measures the velocity of the carrier by emitting ultrasonic waves to the seafloor. It is based on the Doppler effect. According to the principle of the DVL, it can provide the velocity of the UUV related to the seafloor. The accuracy of the DVL is affected by many error sources, such as installation errors, scale factor errors, frequency measurement errors, etc. [23]. To simplify the analysis, the errors of the DVL are considered to be composed of the scale factor errors, the random velocity errors, and the white noise. The output of the DVL in the body frame of the DVL (m frame), which can be expressed as:

$${\widehat{\mathit{V}}}_d^m=\left(1+\delta {\mathit{K}}_d\right){\mathit{V}}_d^m+\delta {\mathit{V}}_d^m+{\mathit{v}}_d^m,$$

(79)

where ${\widehat{\mathit{V}}}_d^m$ is the actual output velocity; ${\mathit{V}}_d^m$ is the ideal velocity; $\delta {\mathit{K}}_d$ is the scale factor error of the DVL; $\delta {\mathit{V}}_d^m$ is the random velocity error; and ${\mathit{v}}_d^m$ is zero-mean Gaussian white noise.

The scale factor error $\delta {\mathit{K}}_d$ is assumed as the random constant and a one-order Markov process can describe the random velocity error $\delta {\mathit{V}}_d^m$:

$$\{\begin{array}{l}\delta {\dot{\mathit{K}}}_d=0\\ \delta {\dot{\mathit{V}}}_d^m=-\delta {\mathit{V}}_d^m/{\tau }_v+{\mathit{w}}_v\end{array},$$

(80)

where ${\tau }_v$ is the correlation time of Markov process of $\delta {\mathit{V}}_d^m$ and ${\mathit{w}}_v$ is the white noise.

The output of the DVL projected in the $G$ frame can be expressed as:

$${\widehat{\mathit{V}}}_d^G={\mathit{C}}_b^G{\mathit{C}}_m^b{\widehat{\mathit{V}}}_d^m,$$

(81)

where ${\mathit{C}}_m^b$ is the direction cosine matrix from the $m$ frame to the $b$ frame. The installation error angles are small enough to be neglected. Therefore, ${\mathit{C}}_m^b$ is approximate to ${\mathit{C}}_m^b=\mathit{I}$.

As $\delta {\mathit{K}}_d$ and $\delta {\mathit{V}}_d^m$ can be updated by Equation (80), the velocity of the UUV measured by the DVL in the $G$ frame can be obtained as:

$${\widehat{\mathit{V}}}_d^G={\mathit{V}}_d^G+{\mathit{v}}_d^G={\mathit{C}}_b^G{\mathit{C}}_m^b{\mathit{V}}_d^m={\mathit{C}}_b^G\left[\left({\widehat{\mathit{V}}}_d^m-\delta {\mathit{V}}_d^m\right)/\left(1+\delta {\mathit{K}}_d\right)\right],$$

(82)

where ${\widehat{\mathit{V}}}_d^G$ is the velocity measured by the DVL after partial error compensation in the $G$ frame; ${\mathit{V}}_d^G$ is the ideal velocity of the UUV in the $G$ frame and ${\mathit{v}}_d^G$ is zero-mean Gaussian white noise.

3. Filter Models of the UUV in Polar Grid Navigation

3.1. Dynamic Model

Based on the analysis of Section 2, the attitude errors ${\mathbf{\varphi }}^G$, the velocity errors $\delta {\mathit{V}}^G$, the position errors $\delta {\mathit{R}}^e$, the gyro drifts ${\mathbf{\epsilon }}_c^b$, and the accelerometer bias ${\mathbf{\nabla }}_c^b$ are chosen as the states to be estimated of the SINS. The gyro constant drift of the OCTANS ${\mathbf{\epsilon }}_{co}^o$ is chosen as the state to be estimated of the OCTANS. The scale factor error of the DVL $\delta {\mathit{K}}_d$ and the random velocity error of the DVL $\delta {\mathit{V}}_d^m$ are chosen as the states to be estimated of the DVL. The states of dynamic model based on $G$ frame can be defined as:

$$\begin{gathered} \mathit{X}={\left[{\mathit{X}}_{SINS}^{\mathrm{T}}{\mathit{X}}_{OCTANS}^{\mathrm{T}}{\mathit{X}}_{DVL}^{\mathrm{T}}\right]}^{\mathrm{T}},{\mathit{X}}_{SINS}={\left[{\left({\mathbf{\varphi }}^G\right)}^{\mathrm{T}}{\left(\delta {\mathit{V}}^G\right)}^{\mathrm{T}}{\left(\delta {\mathit{R}}^e\right)}^{\mathrm{T}}{\left({\mathbf{\epsilon }}_c^b\right)}^{\mathrm{T}}{\left({\mathbf{\nabla }}_c^b\right)}^{\mathrm{T}}\right]}^{\mathrm{T}} \\ {\mathit{X}}_{OCTANS}={\left[{\left({\mathbf{\epsilon }}_{co}^o\right)}^{\mathrm{T}}\right]}^{\mathrm{T}},{\mathit{X}}_{DVL}={\left[{\left(\delta {\mathit{K}}_d\right)}^{\mathrm{T}}{\left(\delta {\mathit{V}}_d^m\right)}^{\mathrm{T}}\right]}^{\mathrm{T}} \end{gathered}$$

Based on the attitude error equation (Equation (51)), the velocity error equation (Equation (61)), the position error equation (Equation (73)), the error model of OCTANS (Equation (75)), the error model of DVL (Equation (80)), and assuming that ${\mathbf{\epsilon }}_r^b$ and ${\mathbf{\nabla }}_r^b$ are both zero-mean Gaussian white noise, the differential equations of the states can be described as:

$$\{\begin{array}{lll}{\dot{\mathbf{\varphi }}}^G& =& {\mathit{C}}_{\omega }{}^{-1}[-({\mathbf{\omega }}_{iG}^G\times ){\mathbf{\varphi }}^G+({\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR})\delta {\mathit{R}}^e+{\mathit{C}}_{\omega eGv}\delta {\mathit{V}}^G-{\mathit{C}}_b^{G^{\prime }}{\mathbf{\epsilon }}^b]\\ \delta {\dot{\mathit{V}}}^G& =& {\mathit{f}}^G\times {\mathbf{\varphi }}^G+\left[{\mathit{V}}^G\times {\mathit{C}}_{\omega eGv}-\left(2{\mathbf{\omega }}_{ie}^G+{\mathbf{\omega }}_{eG}^G\right)\times \right]\cdot \delta {\mathit{V}}^G+\left[{\mathit{V}}^G\times \left(2{\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR}\right)\right]\delta {\mathit{R}}^e+{\mathit{C}}_b^G{\mathbf{\nabla }}^b\\ \delta {\dot{\mathit{R}}}^e& =& {\mathit{C}}_G^e\delta {\mathit{V}}^G-{\mathit{C}}_G^e\left({\mathit{V}}^G\times \right){\mathit{C}}_{\phi R}\delta {\mathit{R}}^e\\ {\dot{\mathbf{\epsilon }}}^b& =& 0\\ {\dot{\mathbf{\nabla }}}^b& =& 0\\ {\dot{\mathbf{\epsilon }}}_{co}^o& =& 0\\ \delta {\dot{\mathit{K}}}_d& =& 0\\ \delta {\dot{\mathit{V}}}_d^m& =& -\delta {\mathit{V}}_d^m/{\tau }_v+{\mathit{w}}_v\end{array},$$

(83)

For simplification of illustration, the dynamic model of the UUV shown in Equation (83) is defined as Model 1. The typical dynamic model of the SINS shown in [24] is defined as Model 2. The dynamic model of the SINS shown in [13] is defined as Model 3. Model 1 and Model 3 are designed for navigation in the polar region, and Model 2 is designed for navigation in non-polar regions. Models 1, 2, and 3 are all based on the principle of traditional SINS. The main difference among Models 1, 2, and 3 is that the G frame is chosen as the navigation frame in Model 1, the g frame is chosen as the navigation frame in Model 2, and the transversal g frame is chosen as the navigation frame in Model 3. Therefore, the meridians’ convergence in high-latitude areas has no impact on Model 1 and 3, while it has an impact on Model 2. The dynamic model of Model 2 can be described as:

$$\{\begin{array}{l}{\dot{\mathbf{\varphi }}}^g={\mathbf{\varphi }}^g\times {\mathbf{\omega }}_{ig}^g+\delta {\mathbf{\omega }}_{ig}^g-{\mathit{C}}_b^g{\mathbf{\epsilon }}^b\\ \delta {\dot{\mathit{V}}}^g=-{\mathbf{\varphi }}^g\times {\mathit{f}}^b+\delta {\mathit{V}}^g\times \left(2{\mathbf{\omega }}_{ie}^g+{\mathbf{\omega }}_{eg}^g\right)+{\mathit{V}}^g\times \left(2\delta {\mathbf{\omega }}_{ie}^g+\delta {\mathbf{\omega }}_{eg}^g\right)+{\mathit{C}}_b^g{\mathbf{\nabla }}^b\\ \delta \dot{L}=\frac{\delta V_N^g}{R_M+h}-\delta h\frac{V_N^g}{{\left(R_M+h\right)}^2}\\ \delta \dot{\lambda }=\frac{\delta V_E^g}{R_N+h}\sec L+\delta L\frac{\delta V_E^g}{R_N+h}\tan L\sec L-\delta h\frac{V_E^g\sec L}{{\left(R_N+h\right)}^2}\\ \delta \dot{h}=\delta V_U^g\\ {\dot{\mathbf{\epsilon }}}^b=0\\ {\dot{\mathbf{\nabla }}}^b=0\\ {\dot{\mathbf{\epsilon }}}_{co}^o=0\\ \delta {\dot{\mathit{K}}}_d=0\\ \delta {\dot{\mathit{V}}}_d^m=-\delta {\mathit{V}}_d^m/{\tau }_v+{\mathit{w}}_v\end{array},$$

(84)

According to Equation (83), the dynamic models of the UUV based on the G frame can be expressed in vector form as:

$$\dot{\mathit{X}}=\mathit{A}\mathit{X}+\mathit{B}\mathit{W},$$

(85)

where $\mathit{A}$ is the system matrix; $\mathit{B}$ is the control matrix; $\mathit{W}$ is the system noise that can be regarded as independent Gaussian white noise ${\mathit{W}}_k\sim N\left(0,{\mathit{Q}}_k\right)$; and $\mathit{Q}$ is measurement noise covariance matrix.

$$\begin{gathered} \mathit{A}=\left[\begin{array}{lll}{\mathit{A}}_{15\times 15}^{SINS}& {\mathbf{0}}_{15\times 3}& {\mathbf{0}}_{15\times 6}\\ {\mathbf{0}}_{3\times 15}& {\mathit{A}}_{3\times 3}^{OCTANS}& {\mathbf{0}}_{3\times 6}\\ {\mathbf{0}}_{6\times 15}& {\mathbf{0}}_{6\times 3}& {\mathit{A}}_{6\times 6}^{DVL}\end{array}\right],{\mathit{A}}_{15\times 15}^{SINS}=\left[\begin{array}{lllll}{\mathit{A}}_1& {\mathit{A}}_2& {\mathit{A}}_3& {\mathit{A}}_4& {\mathbf{0}}_{3\times 3}\\ {\mathit{A}}_5& {\mathit{A}}_6& {\mathit{A}}_7& {\mathbf{0}}_{3\times 3}& {\mathit{A}}_8\\ {\mathbf{0}}_{3\times 3}& {\mathit{A}}_9& {\mathit{A}}_{10}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right],{\mathit{A}}_{3\times 3}^{OCTANS}=\left[{\mathbf{0}}_{3\times 3}\right],{\mathit{A}}_{6\times 6}^{DVL}=\left[\begin{array}{ll}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{A}}_{11}\end{array}\right] \\ \mathit{B}={\left[\begin{array}{lll}{\mathit{B}}_1& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{B}}_2& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{15\times 3}& {\mathbf{0}}_{15\times 3}& {\mathbf{0}}_{15\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& I_{3\times 3}\end{array}\right]}^{\mathrm{T}},\mathit{W}={\left[{\left({\mathbf{\epsilon }}_r^b\right)}^{\mathrm{T}}{\left({\mathbf{\nabla }}_r^G\right)}^{\mathrm{T}}{\left({\mathit{w}}_v\right)}^{\mathrm{T}}\right]}^{\mathrm{T}} \end{gathered}$$

where, ${\mathit{A}}_1={\mathit{C}}_{\omega }{}^{-1}(-({\mathbf{\omega }}_{iG}^G\times ))$, ${\mathit{A}}_2={\mathit{C}}_{\omega }{}^{-1}{\mathit{C}}_{\omega eGv}$, ${\mathit{A}}_3={\mathit{C}}_{\omega }{}^{-1}({\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR})$, ${\mathit{A}}_4=-{\mathit{C}}_{\omega }{}^{-1}{\mathit{C}}_b^{G^{\prime }}$, ${\mathit{A}}_5=\left({\mathit{f}}^G\times \right)$, ${\mathit{A}}_6=\left[{\mathit{V}}^G\times {\mathit{C}}_{\omega eGv}-\left(2{\mathbf{\omega }}_{ie}^G+{\mathbf{\omega }}_{eG}^G\right)\times \right]$, ${\mathit{A}}_7={\mathit{V}}^G\times \left(2{\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR}\right)$, ${\mathit{A}}_8={\mathit{C}}_b^G$, ${\mathit{A}}_9={\mathit{C}}_G^e$, ${\mathit{A}}_{10}=-{\mathit{C}}_G^e\left({\mathit{V}}^G\times \right){\mathit{C}}_{\beta R}$, ${\mathit{A}}_{11}=-1/{\tau }_v\cdot I_{3\times 3}$, ${\mathit{B}}_1={\mathit{C}}_{\omega }{}^{-1}{\mathit{C}}_b^{G^{\prime }}$, ${\mathit{B}}_2={\mathit{C}}_b^G$.

3.2. Observation Model

Due to the restriction of the complex underwater environment in the polar region and combining the characters of the UUV, OCTANS and DVL are chosen as the assistants to improve navigation accuracy of the UUV in the polar region. Thus, attitude and velocity errors are chosen as the states to be observed, which can be obtained from the OCTANS and DVL, respectively. Therefore, the observations of UUV are defined as:

$$\mathit{Z}={\left[\begin{array}{cc}{\left({\mathbf{\varphi }}^G\right)}^{\mathrm{T}}& {\left(\delta {\mathit{V}}^G\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

Based on the analysis in Section 2, the attitude error and the velocity error can be expressed as:

$$\mathit{Z}=\left[\begin{array}{c}{\widehat{\mathsf{\Lambda }}}_o^G-{\mathsf{\Lambda }}^G\\ {\widehat{\mathit{V}}}_d^G-{\mathit{V}}^G\end{array}\right]=\left[\begin{array}{c}{\mathbf{\varphi }}^G+{\mathbf{\epsilon }}_{wo}^G\\ \delta {\mathit{V}}^G+{\mathit{v}}_d^G\end{array}\right],$$

(86)

where ${\mathsf{\Lambda }}^G$ and ${\mathit{V}}^G$ are the attitude and velocity of UUV calculated by the SINS, respectively.

The observation model of UUV based on the $G$ frame can be expressed in vector form as:

$$\dot{\mathit{Z}}=\mathit{H}\mathit{X}+\mathit{V},$$

(87)

where $\mathit{H}$ is the observation matrix; $\mathit{V}={\left[\begin{array}{cc}{\left({\mathbf{\epsilon }}_{wo}^G\right)}^{\mathrm{T}}& {\left({\mathit{v}}_d^G\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$ is the measurement noise vector that is independent Gaussian white noise ${\mathit{V}}_k\sim N\left(0,{\mathit{R}}_k\right)$; and $\mathit{R}$ is the measurement noise covariance:

$$\mathit{H}={\left[\begin{array}{llllllll}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]}^{\mathrm{T}}$$

4. Filter Algorithm of the UUV in Polar Grid Navigation

The filter algorithm will affect the navigation accuracy. In this section, a modified fuzzy adaptive Kalman filter is proposed to realize the data fusion and to improve the accuracy of the system.

4.1. Modified Adaptive Kalman Filter

Based on the Equations (85) and (87), the discrete expression of the filter models are as follows:

$$\{\begin{array}{l}{\mathit{X}}_k={\mathsf{\Phi }}_{k,k-1}{\mathit{X}}_{k-1}+{\mathsf{\Gamma }}_{k,k-1}{\mathit{W}}_{k-1}\\ {\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_k+{\mathit{V}}_k\end{array},$$

(88)

where, ${\mathsf{\Phi }}_{k,k-1}$, ${\mathsf{\Gamma }}_{k,k-1}$ and ${\mathit{H}}_k$ are the discrete expression of $\mathit{A}$, $\mathit{B}$ and $\mathit{H}$, respectively.

The traditional adaptive Kalman filter (AKF) for a discrete system of the UUV can be described as [25]:

$${\mathit{X}}_{k,k-1}={\mathsf{\Phi }}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}+{\widehat{\mathit{q}}}_k,$$

(89)

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

(90)

$${\mathit{v}}_k={\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k/k-1}-{\widehat{\mathit{r}}}_k,$$

(91)

$${\mathit{K}}_k={\mathit{P}}_{k,k-1}{\mathit{H}}_k^{\mathrm{T}}{[{\mathit{H}}_k{\mathit{P}}_{k/k-1}{\mathit{H}}_k^{\mathrm{T}}+{\widehat{\mathit{R}}}_k]}^{-1},$$

(92)

$${\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k/k-1}+{\mathit{K}}_k{\mathit{v}}_k,$$

(93)

$${\mathit{P}}_k=(I-{\mathit{K}}_k{\mathit{H}}_k){\mathit{P}}_{k/k-1},$$

(94)

where ${\widehat{q}}_k$ and ${\widehat{\mathit{Q}}}_k$ are the mean and the covariance of the system noise $\mathit{W}$, respectively. ${\widehat{r}}_k$ and ${\widehat{\mathit{R}}}_k$ are the mean and the covariance of the measurement noise $\mathit{V}$, respectively. The recursive estimate formulas can be written as:

$${\widehat{q}}_{k+1}=(1-d_k){\widehat{q}}_k+d_k({\mathit{X}}_{k+1}-{\mathit{X}}_{k+1,k}),$$

(95)

$${\widehat{\mathit{Q}}}_{k+1}=(1-d_k){\widehat{\mathit{Q}}}_k+d_k[{\mathit{K}}_{k+1}{v}_{k+1}{({\mathit{K}}_{k+1}{v}_{k+1})}^{\mathrm{T}}+{\mathit{P}}_{k+1}-{\mathsf{\Phi }}_{k+1,k}{\mathit{P}}_k{\widehat{\mathit{X}}}_k{\mathsf{\Phi }}_{k+1,k}^{\mathrm{T}}],$$

(96)

$${\widehat{r}}_{k+1}=(1-d_k){\widehat{r}}_k+d_k({\mathit{Z}}_{k+1}-{\mathit{H}}_{k+1,k}{\mathit{X}}_{k+1,k}),$$

(97)

$${\widehat{\mathit{R}}}_{k+1}=(1-d_k){\widehat{\mathit{R}}}_k+d_k[v_{k+1}{v}_{k+1}^{\mathrm{T}}-{\mathit{H}}_{k+1}{\mathit{P}}_{k+1,k}{\mathit{H}}_{k+1}^{\mathrm{T}}],$$

(98)

where $d_k=(1-b)/(1-b^k)$, $0<b<1$ is the forgetting factor.

In order to simplify the system and keep positive definiteness of ${\widehat{\mathit{Q}}}_k$ and ${\widehat{\mathit{R}}}_k$, Equations (96) and (98) are modified as:

$${\widehat{\mathit{Q}}}_{k+1}=(1-d_k){\widehat{\mathit{Q}}}_k+d_k[{\mathit{K}}_{k+1}{v}_{k+1}{({\mathit{K}}_{k+1}{v}_{k+1})}^{\mathrm{T}}+{\mathit{P}}_{k+1}],$$

(99)

$${\widehat{\mathit{R}}}_{k+1}=(1-d_k){\widehat{\mathit{R}}}_k+d_k[v_{k+1}{v}_{k+1}^{\mathrm{T}}],$$

(100)

where, $\mathit{Q}$ and $\mathit{R}$ are the covariance of the system noise and the measurement noise, respectively.

4.2. Fuzzy Inference System (FIS)

The system noise and the measurement noise are assumed to be zero-mean white noise in the conventional AKF, and their covariances are $\mathit{Q}$ and $\mathit{R}$, respectively. In general, there is no prior information of $\mathit{R}$ changing in different situations [26]. The covariance changes with varied underwater environment and influence the characters of the filter. The smaller $\mathit{R}$ value is the higher weight that the recent measurement is given, and the faster the filter response to the observed values [26,27].

The covariance can be modified by a fuzzy inference system (FIS). The adaptive Kalman filter will achieve the optimal state though the covariance modified by FIS. Therefore, in order to improve the performance of the filter, ${\widehat{\mathit{R}}}_k$ should be adjusted by FIS:

$$\widehat{\mathit{R}}=T{\widehat{\mathit{R}}}_k,$$

(101)

where $T$ is the output of the FIS and:

$${\mathit{V}}_k={\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k/k-1},$$

(102)

where, ${\mathit{V}}_k$ is defined as residual error which reflects the dependence degree of the measurement value to the system model. The residual errors are the differences between the measurements and the predictions of the filter, which can be seen as zero-mean white noise, in general [26]. If the residual errors are not zero-mean white noise, the filter will converge to a large bound, or even diverge. Therefore, the residual errors are used to adjust the filter. The theoretical value of the residual error covariance matrix ${\mathit{M}}_v$, which has association with $\mathit{Q}$ and $\mathit{R}$, can be described as [28]:

$${\mathit{M}}_v={\mathit{H}}_k\left(\mathsf{\Phi }{\mathit{P}}_{k-1}{\mathsf{\Phi }}^T+{\mathit{Q}}_{k-1}\right){\mathit{H}}_k^{\mathrm{T}}+{\mathit{R}}_{k-1},$$

(103)

The actual mean value and covariance matrix of the residual error with $j$ statistical numbers in a period of time can be defined as:

$$\overline{v}=\frac{1}{n}{\displaystyle \sum _{j=t-n}^t{v}_j},$$

(104)

$${\widehat{\mathit{M}}}_{\mathit{v}}=\frac{1}{n}{\displaystyle \sum _{j=t-n+1}^t{v}_j{v}_j^T},$$

(105)

where $v$ is the first element in ${\mathit{V}}_k$ to simplify the analysis.

If ${\widehat{\mathit{M}}}_v$ is much larger than ${\mathit{M}}_v$ and $\overline{v}$ is detached from zero, the filter would be unstable. Thus, $\overline{v}$ and ${\widehat{\mathit{M}}}_v$ are chosen as the inputs of FIS.

The FIS used in this paper is a double-input-single-output FIS, which is based on T-S fuzzy logic. The first elements in the mean $\overline{v}$ and covariance ${\widehat{\mathit{M}}}_v$ of the residual error are the inputs of the FIS. The output of FIS is $T$ which is used to adjust ${\widehat{\mathit{R}}}_k$. The triangle membership function is used to blur the inputs. A membership function of inputs can be expressed as Figure 2.

Figure 2. Membership function: (a) membership function of input variable “mean”; (b) membership function of input variable “covariance”.

A total of nine fuzzy rules are used to describe the relationship between the inputs and the output. These fuzzy rules can be shown as Figure 3.

Figure 3. Fuzzy rules.

4.3. Modified FUZZY ADAPTIVE KALMAN Filter

Based on the analysis above, the whole modified fuzzy adaptive KF is shown as follows:

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

(106)

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

(107)

$${\mathit{v}}_k={\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k/k-1},$$

(108)

$${\mathit{R}}_{k+1}=(1-d_k){\mathit{R}}_k+d_k[{\mathit{v}}_{k+1}{\mathit{v}}_{k+1}^{\mathrm{T}}],$$

(109)

$${\mathit{K}}_k={\mathit{P}}_{k,k-1}{\mathit{H}}_k^{\mathrm{T}}{[{\mathit{H}}_k{\mathit{P}}_{k/k-1}{\mathit{H}}_k^{\mathrm{T}}+{\mathrm{T}}_k{\widehat{\mathit{R}}}_k]}^{-1},$$

(110)

$${\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k/k-1}+{\mathit{K}}_k{\mathit{v}}_k,$$

(111)

$${\mathit{P}}_k=(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k){\mathit{P}}_{k/k-1},$$

(112)

$${\widehat{\mathit{Q}}}_{k+1}=(1-d_k){\widehat{\mathit{Q}}}_k+d_k[{\mathit{K}}_{k+1}{\mathit{v}}_{k+1}{({\mathit{K}}_{k+1}{\mathit{v}}_{k+1})}^{\mathrm{T}}+{\mathit{P}}_{k+1}],$$

(113)

In order to describe MFAKF clearly, a flowchart shown in Figure 4 is used to express the process of MFAKF, as shown below.

Figure 4. Flow chart of the modified fuzzy adaptive Kalman filter.

5. Simulations and Experimental Results

Simulations and experiments are conducted not only to verify the effectiveness of the polar grid navigation for the UUV, but also to verify the accuracy improvement of MFAKF compared with AKF. For a simplification of illustration, the SINS/OCTANS/DVL integrated navigation system is labelled as SODINS.

5.1. Simulation Results and Analysis

To verify the effectiveness of the proposed algorithm, simulations are realized in the following conditions: Simulation time is 12 h and the filtering period is 0.1 s. The initial position of UUV, including latitude L and longitude λ, are set as 80° and 126°, respectively.

Sine functions are used to describe the attitudes of the UUV, which include pitch angle, roll angle, and yaw angle. The amplitude of pitch angle, roll angle, and yaw angle are 4°, 5° and 3°, respectively. The period of these angles is 3 s 5 s, and 7 s, respectively. The initial phase of these angles is 0°, 0° and 0°, respectively. The actual misalignment angles are 0°, 0° and 0°, respectively.

The gyro drifts and the accelerometer bias are the two main error sources of SINS [29]. The gyro bias is composed of the gyro constant drifts and the gyro random drifts, which are set as ${0.03}^{°}/\mathrm{h}$ and ${\left({0.001}^{°}/\mathrm{h}\right)}^2$, respectively. The accelerometer bias is composed of the accelerometer constant bias and the accelerometer random bias, which are set as $1\times {10}^{-6}{g}_0$ and ${\left(1\times {10}^{-7}{g}_0\right)}^2$, respectively.

The initial conditions of the OCTANS and DVL are set as follows: The gyro constant drifts of OCTANS are set as 0.01°/h and the random drifts are set as ${\left({0.0005}^{°}/\mathrm{h}\right)}^2$. The velocity random drifts of DVL are set as $\delta V_{dx}^m=\delta V_{dy}^m=\delta V_{dz}^m=0.005\mathrm{m}/\mathrm{s}$, the correlation time of $\delta {\mathit{V}}_d^m$ is set as ${\tau }_v=5\min$ and the scale factor error is set as $\delta K_d={10}^{-4}$. To simplify the simulation and for the purpose of this paper, the constant errors of the OCTANS and DVL are assumed to be well compensated, the random errors are assumed well modeled, and white noises are used to describe the measurement noises of the OCTANS and DVL.

The initial state estimation covariance $P_0$, system noise covariance $Q$, and measurement noise covariance $R$ are set shown below.

$$\begin{gathered} {\mathit{P}}_0=diag\left\{\begin{array}{l}{\left(0.01\mathsf{\pi }/180\mathrm{rad}\right)}^2,{\left(0.01\mathsf{\pi }/180\mathrm{rad}\right)}^2,{\left(0.01\mathsf{\pi }/180\mathrm{rad}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,\\ {\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(1\mathrm{m}\right)}^2,{\left(1\mathrm{m}\right)}^2,{\left(1\mathrm{m}\right)}^2,{\left(0.03\mathsf{\pi }/180/3600\mathrm{rad}/\mathrm{s}\right)}^2,{\left(0.03\mathsf{\pi }/180/3600\mathrm{rad}/\mathrm{s}\right)}^2,\\ {\left(0.03\mathsf{\pi }/180/3600\mathrm{rad}/\mathrm{s}\right)}^2,{\left(1\times {10}^{-6}g{\mathrm{m}/\mathrm{s}}^2\right)}^2,{\left(1\times {10}^{-6}g{\mathrm{m}/\mathrm{s}}^2\right)}^2,{\left(1\times {10}^{-6}g{\mathrm{m}/\mathrm{s}}^2\right)}^2,\\ {\left(0.01\mathsf{\pi }/180/3600\mathrm{rad}/\mathrm{s}\right)}^2,{\left(0.01\mathsf{\pi }/180/3600\mathrm{rad}/\mathrm{s}\right)}^2,{\left(0.01\mathsf{\pi }/180/3600\mathrm{rad}/\mathrm{s}\right)}^2,\\ {\left({10}^{-4}\right)}^2,{\left({10}^{-4}\right)}^2,{\left({10}^{-4}\right)}^2,{\left(0.005\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.005\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.005\mathrm{m}/\mathrm{s}\right)}^2\end{array}\right\} \\ \mathit{Q}=diag\left\{\begin{array}{l}{\left(5\times {10}^{-4}\pi /180/3600\mathrm{rad}/\mathrm{s}\right)}^2,{\left(5\times {10}^{-4}\pi /180/3600\mathrm{rad}/\mathrm{s}\right)}^2,\\ {\left(5\times {10}^{-4}\pi /180/3600\mathrm{rad}/\mathrm{s}\right)}^2,{\left(5\times {10}^{-6}g{\mathrm{m}/\mathrm{s}}^2\right)}^2,{\left(5\times {10}^{-6}g{\mathrm{m}/\mathrm{s}}^2\right)}^2,\\ {\left(5\times {10}^{-6}\mathrm{g}{\mathrm{m}/\mathrm{s}}^2\right)}^2,{\left(0.002{\mathrm{m}/\mathrm{s}}^2\right)}^2,{\left(0.002{\mathrm{m}/\mathrm{s}}^2\right)}^2,{\left(0.002{\mathrm{m}/\mathrm{s}}^2\right)}^2\end{array}\right\} \\ \mathit{R}=diag\left\{\begin{array}{l}{\left(0.01\mathsf{\pi }/180\mathrm{rad}\right)}^2,{\left(0.01\mathsf{\pi }/180\mathrm{rad}\right)}^2,{\left(0.01\mathsf{\pi }/180\mathrm{rad}\right)}^2,\\ {\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2\end{array}\right\} \end{gathered}$$

To verify the effective of the polar grid navigation, Model 2 and Model 3 are chosen as the comparative models. They all based on traditional SINS. However, different frames are chosen as the navigation frames in these models. Different from Model 2, the traditional $g$ frame is chosen as the navigation frame, the $G$ frame is chosen as the navigation frame in Model 1, and transversal $g$ frame is chosen as the navigation frame in Model 3. The computational time and the memory consumption of the proposed polar grid navigation algorithm are 12327.269780 s and 2164 MB (54%), respectively. The simulation results can be expressed as shown below.

The coordinate in $e$ frame ${\mathit{R}}^e\left(x,y,z\right)$ describes the position in Model 1. Latitude and longitude describe the position in Model 2. Considering the different expressions of position in Model 1 and Model 2, Figure 5c,d are used to express the position errors, respectively. As shown in Figure 5, compared with Model 2, Model 1 and Model 3 exhibit more accurate performances, and Model 1 is superior to Model 3 in accuracy. Therefore, Model 1 can be effectively used in the polar region.

Figure 5. Simulation results: (a) attitude errors of the unmanned underwater vehicles (UUV); (b) velocity errors of the UUV; (c) position errors of Model 1; (d) position errors of Model 2.

In order to facilitate comparative analysis, the filter model and the simulation condition are the same for MFAKF and AKF. The estimation errors of SODINS based on MFAKF and KF can be expressed as shown below.

As shown in Figure 6, the estimation errors of MFAKF is smaller than that of the KF. MFAKF shows better performance than AKF. The RMS estimation errors of SODINS based on MFAKF and KF are shown in Table 1, respectively.

Figure 6. Simulation results of SODINS based on MFAKF and AKF: (a) estimation errors of attitude; (b) estimation errors of velocity; and (c) estimation errors of position.

Table 1. RMS errors of SODINS in the simulation.

Parameters	MFAKF	AKF
${\varphi }_x$/(′)	0.0872	0.6117
${\varphi }_y$/(′)	0.1429	0.2655
${\varphi }_z$/(′)	9.0768	14.3786
$v_x$/(m/s)	0.0256	0.0912
$v_y$/(m/s)	0.0182	0.0464
$r_x$/(m)	28.2271	37.9480
$r_y$/(m)	22.7024	100.8459
$r_z$/(m)	556.3639	573.4550

From Table 1, the RMS estimation errors of attitude, velocity, and position based on MFAKF are less than 9.0768’, 0.0256 m/s, and 556.3639 m, respectively. The RMS estimation errors of attitude, velocity, and position based on AKF are less than 14.3786’, 0.0912 m/s, and 573.4550 m, respectively. The results indicate that MFAKF is superior to AKF in the estimation of navigation errors.

5.2. Experimental Results and Analysis

With the restriction of the geography, experimental results can be obtained from the semi-physical simulation. The experiment was conducted in non-polar areas, and the measured data is supplied by the IMU. The experimental data in the polar region is composed of the practical measured data and the simulated data. The experimental data includes the angular velocity ${\widehat{\mathbf{\omega }}}_{ib}^b$ and special force ${\widehat{\mathit{f}}}^b$ in the polar region. The angular velocity in the polar region ${\widehat{\mathbf{\omega }}}_{ib}^b$ is composed of the true angular velocity ${\mathbf{\omega }}_{ib}^b$ and the accelerometer bias $\delta {\mathbf{\omega }}_{ib}^b$. Similarly, the special force ${\widehat{\mathit{f}}}^b$ is composed of the true special force ${\mathit{f}}^b$ and the gyro drifts $\delta {\mathit{f}}^b$.

$$\{\begin{array}{c}{\widehat{\mathbf{\omega }}}_{ib}^b={\mathbf{\omega }}_{ib}^b+\delta {\mathbf{\omega }}_{ib}^b\\ {\widehat{\mathit{f}}}^b={\mathit{f}}^b+\delta {\mathit{f}}^b\end{array},$$

(114)

The true values of the angular velocity and the special force can be supplied by the simulation. Meanwhile, gyro drifts and accelerometer bias can be extracted from the practical measured data. Therefore, the practical measured data and the simulated data constitute the experimental IMU data in the polar region. The practical measured data is provided by IMU in the UUV as shown in Figure 7.

Figure 7. Unmanned underwater vehicle (UUV).

This UUV is built by our laboratory. One main rear propeller and three thrusters (including one lateral thruster and two vertical thrusters), one rudder, and one elevator realize the motion control cooperatively. The mission control is realized by the mission control computer. The sensors that we are most concerned with in this paper include the inertial measurement unit (IMU), doppler velocity log (DVL), OCTANS, depth sensor, Global Positioning System (GPS), underwater camera, and sonar. In these sensors, IMU, OCTANS, and DVL play a major role in this paper, while the other sensors are not covered in this paper. The experiment was carried out in a rectangular pool located at the non-polar region (N45°46′ E126°40′). The UUV is ordered to finish the uniform liner motion with a velocity of 1 kn. According to the data collected from the experiment, the gyro drifts and the accelerometer bias can be calculated.

The gyro drifts and the accelerometer bias can be extracted from the practical measured data. The gyro constant drifts are ${0.03}^{°}/\mathrm{h}$; the gyro random drifts are ${\left(4.094\times {10}^{-6}\mathrm{rad}/\mathrm{s}\right)}^2$, ${\left(4.308\times {10}^{-6}\mathrm{rad}/\mathrm{s}\right)}^2$ and ${\left(2.386\times {10}^{-6}\mathrm{rad}/\mathrm{s}\right)}^2$, respectively; the accelerometer constant bias is $1\times {10}^{-6}{g}_0$; and the accelerometer random biases are ${\left(0.00156{\mathrm{m}/\mathrm{s}}^2\right)}^2$, ${\left(0{.001747\mathrm{m}/\mathrm{s}}^2\right)}^2$ and ${\left(0.0004063{\mathrm{m}/\mathrm{s}}^2\right)}^2$, respectively.

The semi-physical simulation was conducted in non-polar areas to overcome the restriction of the geography. Similar with the true values of the angular velocity and the special force of UUV which are supplied by simulation, the signal of the OCTANS and DVL are also provided by the simulation. Based on the characters of the OCTANS and DVL, the relevant parameters of them are set as follows: The OCTANS gyro constant drifts and the gyro random drifts are set as ${0.01}^{°}/\mathrm{h}$ and ${\left({0.0005}^{°}/\mathrm{h}\right)}^2$, respectively. The DVL random velocity drifts, the correlation time of $\delta {\mathit{V}}_d^m$, and the scale factor error are set as $\delta V_{dx}^m=\delta V_{dy}^m=\delta V_{dz}^m=0.005\mathrm{m}/\mathrm{s},$ ${\tau }_v=5\min$ and $\delta K_d={10}^{-4}$, respectively. To simplify the experiment and for the purpose of this paper, the constant errors of the OCTANS and DVL are assumed to be well compensated, the random errors are assumed to be well modeled, and white noise is used to describe the measurement noises of the OCTANS and DVL. Relevant parameters in experiment are the same with those in simulation. In experiment, errors and estimation errors of UUV are also expressed to verify the effectively of the polar grid navigation algorithm.

As shown in Figure 8, Model 1 is superior to Model 2 and Model 3 in accuracy. The experimental results demonstrate that Model 1 and Model 3 can overcome the problems in Model 2 and Model 1 has better performance than Model 3.

Figure 8. Experimental results: (a) attitude errors of the UUV; (b) velocity errors of the UUV; (c) position errors of Model 1; and (d) position errors of Model 2.

The comparative analysis between MFAKF and AKF in experiment can be expressed as follows.

As shown in Figure 9, MFAKF exhibits better navigation accuracy than AKF. The RMS estimation errors of SODINS based on MFAKF and KF are shown in Table 2, respectively.

Figure 9. Experimental results of SODINS based on MFAKF and AKF: (a) estimation errors of attitude; (b) estimation errors of velocity; and (c) estimation errors of position.

Table 2. RMS errors of SODINS in the experiment.

Parameters	MFAKF	AKF
${\varphi }_x$/(′)	0.0571	0.0896
${\varphi }_y$/(′)	0.1257	0.1601
${\varphi }_z$/(′)	11.0893	15.1294
$v_x$/(m/s)	0.0376	0.0909
$v_y$/(m/s)	0.0241	0.0462
$r_x$/(m)	21.3962	36.3067
$r_y$/(m)	33.5115	103.3216
$r_z$/(m)	551.6560	566.5706

From Table 2, RMS estimation errors of the attitude, velocity and position based on MFAKF are less than 11.0893’, $0.0376\mathrm{m}/\mathrm{s}$ and 551.6560 m, respectively. The RMS estimation errors of attitude, velocity, and position based on AKF are less than 15.1294’, $0.0909\mathrm{m}/\mathrm{s}$ and 556.5706 m, respectively. Therefore, the navigation accuracy of SODINS based on MFAKF is better than that based on AKF.

6. Discussion

Simulation and experiment results demonstrate that the proposed polar grid navigation can be effectively used on a UUV in the polar region. The comparison between MFAKF and AKF show that the proposed filter algorithm is superior to AKF. To facilitate comparative discussions, the dynamic model of a UUV and the simulation conditions are the same for both MFAKF and AKF. The comparisons among Model 1, Model 2, and Model 3 demonstrate the effectiveness of the polar grid navigation, which can be expressed as shown below. Additionally, compared with the traditional AKF, the advantages of MFAKF are also concluded as follows:

• SINS is widely used in UUV navigation because of its high autonomy. Model 2 is the typical SINS model for UUVs. It is a general model of SINS and can achieve a navigation accuracy that meets the requirements of UUVs in non-polar regions. While in the polar region, due to the rapid convergence of Earth meridians, there exist calculation overflows and sharply increasing errors in Model 2, in which the geographic frame is chosen as the navigation frame. This is because the error of the upside component of the command angular velocity is related to the tangent value of latitude in traditional SINS. In the polar region, the latitude tends to 90° and the error tends to infinity. Therefore, the geographic frame is not suitable to be chosen as the navigation frame in the polar region. To overcome the problems in the traditional SINS (Model 2) in the polar region, the transversal SINS (Model 3) and polar grid SINS (Model 1) are proposed. The transversal geographic frame and grid frame are chosen as the navigation frame to modify the unsuitability of the traditional SINS, respectively. Although the transversal SINS (Model 3) can realize navigation in the polar region, there are principle errors in the transversal SINS algorithm. The transversal SINS ignores the ellipse of the Earth. Model 1, based on the grid frame, is proposed to overcome the influence caused by the meridians’ convergence in high latitude areas. In the grid frame, the grid planes are parallel with the Greenwich plane. The polar region is the normal region in the polar grid navigation algorithm. There is no impact on the polar grid navigation. Simulation and experiment results also demonstrate that Model 1 is superior to Model 2 and Model 3 in accuracy. The polar grid navigation of UUVs proposed in this paper (Model 1) is suitable for UUV navigating in the polar region.
• Ignoring the negative terms can not only simplify the filter but can also keep the positive definiteness of the filter. T-S fuzzy logic regulates the residual error close to zero. The covariance is also regulated by FIS to adjust the changing of the environment. The adaptive Kalman filter will achieve the optimal state though the covariance modified by FIS. MFAKF can adjust the changing of the environment. Therefore, MFAKF is superior to AKF in estimating the states of filter.

Based on the analysis above, Model 1 and MFAKF have the better performance than Model 2, 3, and AKF in the polar region. Therefore, the proposed polar grid navigation for UUV can be used in the polar region effectively.

7. Conclusions

A polar grid navigation algorithm for UUV is proposed to overcome the unavailability of traditional UUV navigation in polar regions. Considering the complex polar underwater environment and the motion characteristics of UUVs, SINS based on the grid frame with the assistance of OCTANS and DVL is chosen for UUV polar navigation. A modified fuzzy adaptive Kalman filter is used to improve the navigation accuracy. Simulation and experiment results have proven that the proposed polar grid navigation of a UUV can be effectively used in the polar region, and the proposed modified fuzzy adaptive Kalman filter can achieve better accuracy than AKF.