Cooperative Navigation Algorithm of Extended Kalman Filter Based on Combined Observation for AUVs

Abstract

The navigation and positioning of multi-autonomous underwater vehicles (AUVs) in the complex and variable marine environment is a significant and much-needed area of attention, especially considering the fact that cooperative navigation technology is the essential method for multiple AUVs to solve positioning problems. When the extended Kalman filter (EKF) is applied for underwater cooperative localization, the outliers in the sensor observations cause unknown errors in the measurement system due to deep-sea environmental factors, which are difficult to calibrate and cause a significant reduction in the co-location accuracy of AUVs, and can even cause problems with a divergence of estimation error. In this paper, we proposed a cooperative navigation method of the EKF algorithm based on the combined observation of multiple AUVs. Firstly, the corresponding cooperative navigation model is established, and the corresponding measurement model is designed. Then, the EKF model based on combined observation is designed and constructed, and the unknown error is eliminated by introducing a previously measured value. Finally, simulation tests and lake experiments are designed to verify the effectiveness of the algorithm. The results indicate that the EKF algorithm based on combined observation can approximately eliminate errors and improve the accuracy of cooperative localization when the unknown measurement error cannot be calibrated by common EKF methods. The effect of state estimation is improved, and the accuracy of co-location can be effectively improved to avoid serious declines in—and divergence of—estimation accuracy.

1. Introduction

Autonomous underwater vehicles (AUVs) are a class of important autonomous navigation carriers for human exploration of the marine environment. AUVs have a high degree of autonomy, flexibility, and remote navigation capabilities in marine resources exploration, underwater exploration, data acquisition, and other aspects of a wide range of applications [1,2,3,4]. With the rapid development of air- and land-based navigation technology, underwater navigation technology is also progressing rapidly [5,6,7,8,9,10]. However, due to the complexity of the underwater environment, there is still a gap between the navigation and positioning accuracy of underwater vehicles compared to those of air and land, and for the time being, underwater navigation has become an important issue in the field of AUV research [11,12,13,14].

Underwater high-precision navigation is key to AUVs’ mission, and the specific navigation methods applied by AUVs vary depending on the area in which they navigate underwater [15,16,17]. When an AUV sails in an offshore area (water depth <300 m), navigation technologies such as radio navigation (RADNAV) and global navigation satellite systems (GNSS) can be used from time to time; in the near-sea area (from the seabed <100 m), terrain matching navigation can be considered; and when sailing in deep-water environments, it is difficult to obtain the above-mentioned navigation information [18,19]. To sum up, even if there are more sensors, there are still many limitations and deficiencies in the independent operation of AUVs underwater. However, multiple AUVs can use their navigation sensors to obtain corresponding measurement information and then combine underwater acoustic communication technology with underwater acoustic detection technology to share this information and measure the relative positions of adjacent AUVs and obtain navigation parameters by fusing information. Cooperative navigation enables highly accurate underwater positioning and navigation, easily accomplishing complex tasks that would be difficult for a single AUV to perform, and the simplicity of the system and the flexibility of the range of use have led to a wide focus of researchers on cooperative underwater positioning of multiple AUVs [20,21,22].

The key object of research in multi-AUV cooperative navigation is the information fusion algorithm shared among the system’s AUVs, and most of the existing multi-AUV cooperative positioning methods comprise improved Kalman filter (KF) algorithms combined with measurement data from underwater navigation sensors. The model of a multi-AUV cooperative localization system is often nonlinear. Therefore, an extended Kalman filter (EKF) or unscented Kalman filter (UKF) is usually used for state estimation [23,24,25].

The problem of cooperative navigation and positioning is a state estimation problem, and the accurate acquisition of observation information is key to realizing the optimal state estimation of cooperative navigation [26]. At present, the use of hydroacoustic communication devices to obtain relative observations, such as relative distance, between individual AUVs is the most effective and reliable measurement method [27,28,29]. However, AUVs are used in complex marine environments where the cooperative navigation of multiple sensors can be affected by harsh conditions and instability of measurement sensors, causing unknown system noise and resulting in unknown measurement system errors. This inevitably leads to the destruction of effective observation information, and the extended KF filtering algorithm cannot accurately estimate and correct this unknown measurement system error, thus directly leading to a significant decrease in filtering accuracy and stability or even divergence. Ref. [30] describes how, in acoustic ranging-based cooperative navigation, sensors are subjected to temperature, salinity, depth, current, and other factors in the underwater environment, which makes the statistical characteristics of the measured sensor noise inaccurate and generates unknown measurement system errors. In the framework of Kalman filters, these methods require that the statistical characteristics of process noise and measurement noise are known and fixed during cooperative localization [31,32]. However, this unknown measurement system error is difficult to calibrate with the common EKF algorithm and, therefore, leads directly to a reduction in filter accuracy or even divergence [33]. Ref. [34] presented the use of attenuated adaptive filtering to achieve adjustment of the weights of new measurements, but the process of calculating the scalar asymptotic factor is too cumbersome. Adaptive filters based on great likelihood can correct the second-order moments of statistical properties of noise but rely on accurate estimates of the new interest covariance [35]. The above-mentioned works all emphasize the influence of measurement error information on the state estimation effect. To improve the robustness of state estimation regarding measurement errors, it is still necessary to further seek a more reliable and effective co-location algorithm. In Ref. [36], Sheng has proposed a novel transfer alignment method based on combined double-time observations with velocity and attitude to address the problem of static flexural deformation causing measurement system errors, successfully eliminating this unknown system error, effectively improving filter performance, and increasing the accuracy of transfer alignment. However, the linear KF cannot be applied to multi-AUV cooperative navigation [37].

To solve the above problems, we need to redesign the cooperative navigation algorithm, and realize the optimal state estimation of cooperative navigation by filtering and optimizing the measurement information with errors. In this paper, we proposed a cooperative navigation method of the EKF algorithm based on the combined observation of multiple AUVs. The improved EKF algorithm model was designed and constructed to eliminate errors in the measurement system by combining previously measured information with information from the current moment; current observations subtracted from the last observation are used as new measurements for the improved algorithm. The final results of the simulation and experiments verify the feasibility and effectiveness of the algorithm in the field of cooperative localization for AUVs.

The rest of this paper is organized as follows: In Section 2, the formulation of a discrete-time nonlinear model of the cooperative positioning system and the corresponding standard EKF algorithm are described. In Section 3, by analysis and comparison with the common algorithm, the improved EKF based on combined observation is proposed and derived and is specifically applied to cooperative navigation for AUVs. In Section 4, a simulation test and lake tests are designed, and the results verify the feasibility and effectiveness of the algorithm. Finally, conclusions are presented in Section 5.

2. Theoretical Basis of Multi-AUV Cooperative Navigation

2.1. Principles of AUV Cooperative Navigation

It is assumed that multiple AUVs are currently working together in the middle-water environment, AUVs can communicate with each other, and AUVs have independent navigation. The navigation sensor used in the AUVs’ underwater navigation system is utilized to obtain attitude, speed, position, relative distance, and orientation information. The system includes the following: underwater acoustic communication equipment composed of SINS, Doppler velocity log (DVL), terrain-assisted navigation (TAN), magnetic compass pilot (MCP), and other related auxiliary equipment.

Each AUV uses its navigation sensors to obtain the required information, and then uses a nonlinear filter to continuously estimate and update its position, and uses communication equipment to communicate with neighboring AUVs to exchange information for information contribution and to fuse the relative position relationships of each AUV in the whole cooperative navigation system. In this paper, we choose a multi-AUV parallel cooperative navigation system solution to navigate and position each AUV with equal status and comparable navigation accuracy. A schematic representation of the cooperative navigation system in this paper is shown in Figure 1.

Figure 1. The network structure of multiple AUVs.

The system uses each AUV as the processing node, and each AUV can communicate with other AUVs and has independent navigation. Neighboring nodes can communicate with each other and they rely on sharing sensor measurements with neighboring AUVs to fuse the relative position relationships of each AUV in the overall cooperative navigation system, resulting in an equal improvement in the navigation positioning accuracy of all AUVs.

2.2. Multi-AUV Motion Model

As described in the previous subsection, there is a wide range of underwater navigation sensors available in the AUV underwater navigation system. This paper focuses on medium- to high-accuracy SINS, which provides high-update-frequency navigation information such as angular velocity, acceleration, attitude, velocity, and position. The navigation center, centered on the inertial device, is an indispensable device for the AUV and is an important guarantee to support the safe navigation of the AUV and the smooth execution of its work tasks [38,39,40].

In the SINS-based AUV underwater navigation system, we use the error values of the SINS output navigation parameters as the state variables; the SINS state has physical properties, and its system model is based on Newton’s laws of motion, with a high degree of coupling between the state quantities. In this paper, the attitude error ${\mathit{\varphi }}^n$, velocity error $v^n$, position error $\delta p^n$, gyroscope constant zero bias ${\mathit{\epsilon }}^b$, and accelerometer constant zero bias ${\mathbf{\nabla }}^b$ are selected as the state variables of the EKF.

$$X={\left[\begin{array}{ccccc}{\left({\mathit{\varphi }}^n\right)}^{\mathrm{T}}& {\left(\delta {\mathit{V}}^n\right)}^{\mathrm{T}}& {\left(\delta {\mathit{p}}^n\right)}^{\mathrm{T}}& {\left({\mathit{\epsilon }}^b\right)}^{\mathrm{T}}& {\left({\mathbf{\nabla }}^b\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(1)

For navigation systems, the equation of state is the dynamic equation of the system, also known as the equation of motion, which describes the state of the system over time; the equation of observation describes the measurement of the state of the system by the sensors [41]. The equation of state for transfer alignment is obtained from the SINS error equation and state variables:

$$\dot{\mathit{X}}=FX+GW$$

(2)

where $F$ is the transfer matrix, $X$ is the state vector, $G$ is the disturbance matrix, and $W$ is the random disturbance vector.

Suppose there are N AUVs in the multi-AUV collaborative system, and the state quantity ${\mathit{X}}_i(k)$ of the i-th AUV at time $t_k$:

$$X_i(k)={\left[\begin{array}{ccccc}{\left({\mathit{\varphi }}^n\right)}_i{}^{\mathrm{T}}(k)& {\left(\delta {\mathit{V}}^n\right)}_i{}^{\mathrm{T}}(k)& {\left(\delta {\mathit{p}}^n\right)}_i{}^{\mathrm{T}}(k)& {\left({\mathit{\epsilon }}^b\right)}_i{}^{\mathrm{T}}(k)& {\left({\mathbf{\nabla }}^b\right)}_i{}^{\mathrm{T}}(k)\end{array}\right]}^{\mathrm{T}}$$

(3)

Then, in the multi-AUV collaborative navigation system, the state quantity $\mathit{X}(k)$ of the system is:

$$\mathit{X}(k)={\left[\begin{array}{ccc}{\mathit{X}}_1(k)& \dots & {\mathit{X}}_N(k)\end{array}\right]}^{\mathrm{T}}$$

(4)

2.3. Measurement Update Model

When AUV_j is observed by AUV_i at a certain time, the relative observations between AUV_ij are measured by the hydroacoustic communication equipment carried by the AUVs; the relative observations include mutual distance, bearing, etc. The mutual observations of underwater vehicles are shown in Figure 2.

Figure 2. Mutual observation of multiple AUVs.

$d_{ij}$ is the relative horizontal distance between AUV_i and AUV_j, ${\phi }_i$ and ${\phi }_j$ are the heading angles of AUV_i and AUV_j, respectively, and ${\varphi }_{ij}$ is the relative azimuth of AUV_j to AUV_i. Time of arrival (TOA) is the most commonly used distance measurement method for cooperative positioning in the underwater environment. The three-dimensional slope distance between AUV_i and AUV_j can be calculated from the propagation time $\Delta t$ of the hydroacoustic pulse signal between AUV_i and AUV_j and the propagation speed $c$ of the acoustic signal in seawater, and the three-dimensional slope information is converted into a two-dimensional relative horizontal distance measurement based on the precise depth information $h_k^i$ and $h_k^j$ of AUV_i and AUV_j:

$$d_{ij}=\sqrt{{(c\cdot \Delta t)}^2-{(h_i-h_j)}^2}$$

(5)

The relationship between the measured relative distance information and the navigation state quantity can be expressed specifically as follows:

$$d_{ij}=\sqrt{{[(L_i-L_j)(R_M^i+h_i)]}^2+{[({\lambda }_k^i-{\lambda }_k^j)(R_N^i+h_i)\cos L_i]}^2}$$

(6)

Based on the time delay difference between the hydroacoustic pulse signals indirectly received by the different array elements of the sonar to determine the orientation of the measured target, the relationship between the measured relative orientation information and the navigation state quantity, considering the position relationship shown in Figure 2, can be specifically expressed as:

$${\varphi }_{ij}=\arctan \left[\frac{({\lambda }_j-{\lambda }_i)\cos L_l^j}{L_j-L_i}\right]-{\phi }_i$$

(7)

The observation equation is written in general form:

$$\mathit{Z}=h({\mathit{x}}_i,{\mathit{x}}_j)+\mathit{v}$$

(8)

Due to the complexity and variability of the underwater environment, multiple AUVs can observe each other at certain time steps; this allows each underwater robot to acquire one or more relative observations until all relative observations of the other are acquired. According to the ideal position information $x_k^j=(L_k^j,{\lambda }_k^j,h_k^j)$ of AUV_j and $x_k^i=(L_k^i,{\lambda }_k^i,h_k^i)$ of AUV_i at the time $t_k$, the relative distance information is obtained. As can be seen from the above figure, the observation equation of the relative distance is:

$$\begin{gathered} {\mathit{Z}}_k^d=h_d(x_k^i,x_k^j)+v_k^d=\sqrt{{[(L_k^i-L_k^j)(R_{M,k}^i+h_k^i)]}^2+{[({\lambda }_k^i-{\lambda }_k^j)(R_{N,k}^i+h_k^i)\cos L_k^i]}^2}+v_k^d \\ {v}_k^d\sim N(0,{\sigma }_{d,k}^2) \end{gathered}$$

(9)

where ${\mathit{Z}}_k^d$ represents relative distance measurements, $h_d(x_k^i,x_k^j)$ is the relative distance measurement function, $v_k^d$ represents the hydroacoustic distance and the hydroacoustic communication measurement noise, usually assumed to have a mean of zero and a variance of ${\sigma }_{d,k}^2$, and the relative distances observed are independent of each other and uncorrelated. Then, the Jacobian matrix $h_d(x_k^i,x_k^j)$ is:

$$H_d^i={\left.\frac{\partial h_d}{\partial x_k^i}\right|}_{X_k={\widehat{X}}_{k|k-1}}={\left.\left[\begin{array}{cccc}{\mathbf{0}}_{1\times 6}& \frac{\partial h_d}{\partial L_k^i}& \frac{\partial h_d}{\partial {\lambda }_k^i}& 0\end{array}\right]\right|}_{X_k={\widehat{X}}_{k|k-1}}$$

(10)

$$H_d^j={\left.\frac{\partial h_1}{\partial x_k^j}\right|}_{X_k={\widehat{X}}_{k|k-1}}={\left.\left[\begin{array}{cccc}{\mathbf{0}}_{1\times 6}& \frac{\partial h_d}{\partial L_k^j}& \frac{\partial h_d}{\partial {\lambda }_k^j}& 0\end{array}\right]\right|}_{X_k={\widehat{X}}_{k|k-1}}$$

(11)

Take AUV_i for example:

$$\frac{\partial h_d}{\partial L_k^i}=\frac{(L_k^i-L_k^j){(R_{M,k}^i+h_k^i)}^2-({\lambda }_k^i-{\lambda }_k^j)(R_{N,k}^i+h_k^i)\sin L_k^i\cos L_k^i}{\sqrt{{[(L_k^i-L_k^j)(R_{M,k}^i+h_k^i)]}^2+{[({\lambda }_k^i-{\lambda }_k^j)(R_{N,k}^i+h_k^i)\cos L_k^i]}^2}}$$

(12)

$$\frac{\partial h_d}{\partial {\lambda }_k^i}=\frac{({\lambda }_k^i-{\lambda }_k^j){\left[(R_{N,k}^i+h_k^i)\cos L_k^i\right]}^2}{\sqrt{{[(L_k^i-L_k^j)(R_{M,k}^i+h_k^i)]}^2+{[({\lambda }_k^i-{\lambda }_k^j)(R_{N,k}^i+h_k^i)\cos L_k^i]}^2}}$$

(13)

Similarly, it is known that, at the moment $t_l$, AUV_i observes AUV_j and obtains the relative orientation information [42]. Based on the reference attitude information ${\mathit{x}}_l^i=(L_l^i,{\lambda }_l^i,{\phi }_l^i)$ received at moment $t_l$ for AUV_i and the reference attitude information $x_l^j=(L_l^j,{\lambda }_l^j,{\phi }_l^j)$ for AUV_j, the observation equation for the relative bearing is obtained as:

$${\mathit{Z}}_l^{\beta }=h_{\beta }(x_l^i,x_l^j)+v_l^{\beta }=\arctan \left[\frac{({\lambda }_l^j-{\lambda }_l^i)\cos L_l^j}{L_l^j-L_l^i}\right]-{\phi }_l^i+v_l^{\beta }, v_l^{\beta }\sim N(0,{\sigma }_{\beta ,l}^2)$$

(14)

where $h_{\beta }(\cdot )$ is the relative bearing measurement function, $v_l^{\beta }$ is the hydroacoustic bearing measurement noise, usually assumed to be a Gaussian white noise with zero mean and variance ${\sigma }_{\beta ,l}^2$, and each relative bearing observation is independent of each other and uncorrelated. Then, the Jacobian matrix of $h_{\beta }(\cdot )$ with respect to $x_l^i$ and $x_l^j$ is:

$$H_{\beta }^i={\left.\frac{\partial h_{\beta }}{\partial x_l^i}\right|}_{X_l={\widehat{X}}_{l|l-1}}={\left[\begin{array}{ccccccc}0& 0& 1& 0_{1\times 3}& \frac{\partial h_{\beta }}{\partial L_k^i}& \frac{\partial h_{\beta }}{\partial {\lambda }_k^i}& 0\end{array}\right]}_{X_l={\widehat{X}}_{l|l-1}}$$

(15)

$$H_d^j={\left.\frac{\partial h_1}{\partial x_k^j}\right|}_{X_k={\widehat{X}}_{k|k-1}}={\left.\left[\begin{array}{cccc}{\mathbf{0}}_{1\times 6}& \frac{\partial h_d}{\partial L_k^j}& \frac{\partial h_d}{\partial {\lambda }_k^j}& 0\end{array}\right]\right|}_{X_k={\widehat{X}}_{k|k-1}}$$

(16)

Take AUV_i for example:

$$\frac{\partial h_{\beta }}{\partial L_k^i}=\left[\frac{\cos L_j\times ({\lambda }_j-{\lambda }_i)}{{(L_j-L_i)}^2}\right]/\left\{1+{\left[\frac{({\lambda }_j-{\lambda }_i)\cos L_j}{L_j-L_i}\right]}^2\right\}$$

(17)

$$\frac{\partial h_{\beta }}{\partial {\lambda }_k^i}=\left(\frac{\cos L_j}{L_i-L_j}\right)/\left\{1+{\left[\frac{({\lambda }_j-{\lambda }_i)\cos L_j}{L_j-L_i}\right]}^2\right\}$$

(18)

The research in this study is carried out based on obtaining these observations, even if these observations are subjected to preprocessing methods such as removing outliers, including the observation target and the matching of the observed target and the observed target. Due to the complexity of the underwater environment, or the sensor installation error angle and other reasons, often, unknown system errors are brought to the measurement system [43,44]. Thus, it is necessary to consider how the filter can be used to calibrate for this error.

3. Derivation of EKF Based on Combined Observation of Cooperative Navigation

3.1. Scheme of Common EKF

With a model of a multi-AUV cooperative navigation system in place, the algorithm used for information fusion is central to solving the cooperative navigation problem [45,46,47]. It is known from Section 1 that for the filter problem of nonlinear cooperative navigation systems under ideal communication conditions, most systems choose to use the EKF algorithm, and since EKF is an extension of the KF, the form of the KF algorithm needs to be introduced first.

For the common KF, one-step predictive state estimation is adopted:

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

(19)

The updated state estimate is:

$${\widehat{X}}_k={\widehat{X}}_{k,k-1}+K_k\left[Z_k-H_k{\widehat{X}}_{k,k-1}\right]$$

(20)

The Kalman gain is:

$$K_k=P_{k,k-1}{H}_k^{\mathrm{T}}{\left[H_k{P}_{k,k-1}{H}_k^{\mathrm{T}}+R_k\right]}^{-1}$$

(21)

The one-step predicted estimate covariance is:

$$P_{k,k-1}={\mathit{\varphi }}_{k,k-1}{P}_{k-1}{\mathit{\varphi }}_{k,k-1}^{\mathrm{T}}+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^{\mathrm{T}}$$

(22)

The updated state estimate covariance is:

$$P_k=\left[I-K_k{H}_k\right]P_{k,k-1}$$

(23)

From the above KF calculation steps, we can learn that given the filter parameters ${\mathit{X}}_0$ and $P_0$ at the initial moment, the optimal state estimate ${\mathit{X}}_{k+1}$ of the linear Gaussian system at that moment can be obtained based on the observations at moment $k$. In the case of cooperative navigation systems, the state and observation models are nonlinear, as described in the previous subsection, and so require the use of nonlinear filter algorithms for cooperative navigation information fusion.

The general nonlinear discrete system can be described as:

$$X_k=f_{k-1}\left(X_{k-1}\right)+{\Gamma }_{k-1}{W}_{k-1}$$

(24)

$$Z_k=h_k\left(X_k\right)+V_k$$

(25)

In the above equation, $X_k$ is the state vector; ${\mathit{f}}_k(\cdot )$ is the nonlinear vector dispersion function; ${\Gamma }_{k-1}$ and $W_{k-1}$ are the system noise driver matrix and the system noise vector, respectively, ${\mathit{Z}}_k$ is the measurement vector; ${\mathit{h}}_k(\cdot )$ is the nonlinear vector dispersion function, and $V_k$ is the noise vector. The EKF builds on the KF by expanding the system’s nonlinear equation of state around the state values $X_k$ into a Taylor series and omitting the second-order-and-higher terms, myopically transforming it into a linearized model and using the KF framework for state estimation; the transformed equation of state is:

$$X_k={\mathit{\varphi }}_{k/(k-1)}{X}_{k-1}+{\mathit{U}}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}$$

(26)

where:

$${\mathit{\varphi }}_{k/k-1}={\left.\frac{\partial f_{k-1}\left(X_{k-1}\right)}{\partial X_{k-1}}\right|}_{x_{k-1}={\widehat{x}}_{k-1}}$$

(27)

$$U_{k-1}=f_{k-1}\left({\widehat{X}}_{k-1}\right)-{\left.\frac{\partial f_{k-1}\left(X_{k-1}\right)}{\partial X_{k-1}}\right|}_{X_{k-1}={\widehat{x}}_{k-1}}\times {\widehat{X}}_{k-1}$$

(28)

In the above equation, ${\mathit{X}}_k$ is the state vector, and ${\Gamma }_{k-1}$ and ${\mathit{W}}_{k-1}$ are the noise driver matrix and the system noise vector, respectively.

The algorithm structure of the extended KF is essentially the same as that of the linear KF, in that the filter results are obtained by performing temporal and quantitative updates to the state estimates and covariances. In this paper, the EKF-based cooperative navigation algorithm for underwater AUVs requires high accuracy in the measurement equations, while the marine environmental noise, as a disturbing background field in the hydroacoustic channel, is characterized by complexity and variability. Wenz [48] made a summary of deep-sea environmental noise through a submarine deep-water hydrophone. He proposed that deep-sea environmental noise is relatively certain, resulting in unknown errors in the corresponding underwater acoustic ranging measurement system, which affects the accuracy of the measurement data. The traditional EKF algorithm cannot calibrate the error of the measurement system, resulting in a decrease in the filtering accuracy and a great impact on the accuracy of cooperative navigation. The EKF measurement equation including the unknown systematic error is as follows:

$$Z_k=h_k\left(X_k\right)+V_k+ek$$

(29)

The measurement system error $e_k$ is an unknown quantity. As can be seen from the above equation, it is difficult to eliminate the unknown measurement system error $e_k$ with a common EKF, so multi-AUV cooperative navigation based on EKF is susceptible to divergence caused by the measurement system error, which directly affects the results of the multi-AUV cooperative navigation in a bad way.

3.2. Scheme of Improved EKF Based on Combined Observation

As described in the previous section, deep-sea environmental noise makes the noise of the measurement sensors change and, thus, the sensor data produces outliers, resulting in an unknown measurement system error $e_k$ for the cooperative system, which is difficult to model and, therefore, difficult to compensate for with a common EKF, which may make the filter accuracy degrade or even diverge. In this paper, a new algorithm needs to be devised to solve this problem. In engineering practice, the variation of two adjacent measurements ${\mathit{Z}}_k$ and ${\mathit{Z}}_{k-1}$ is small, and if the period we choose is the SINS period, typically 1s or even 0.1s, we can well assume that the unknown errors of the adjacent measurement systems are approximately equal. Therefore, this paper is based on the common EKF, where the measurement information from the previous and subsequent moments is combined and the unknown system error $e_k$ is effectively eliminated by re-establishing the quantity measurement and using the difference between ${\mathit{Z}}_k$ and ${\mathit{Z}}_{k-1}$.

The nonlinear measurement equation with system errors is shown in Equation (29). Through analysis, the measurement equation is re-established in this paper to eliminate the unknown error $e_k$ in the measurement system. It follows from the derivation that at moments before and after a sufficiently short update period $e_k\approx e_{k-1}$, the following equation uses the implementation of the difference process to eliminate the system error $e_k$ within it and to transform the reconstructed measurement equation regarding the linearization process of the nonlinear equation of state:

$$Z_k=H_k{\mathit{X}}_k-{\mathit{B}}_k{\mathit{X}}_{k-1}+{\mathit{Y}}_k+{\mathit{N}}_k$$

(30)

where:

$$H_k={\left.\frac{\partial h_k\left(X_k\right)}{\partial X_k}\right|}_{X_k={\widehat{X}}_{k/(k-1)}}$$

(31)

$${\mathit{B}}_k={\left.\frac{\partial h_{k-1}\left(X_{k-1}\right)}{\partial X_{k-1}}\right|}_{X_{k-1}={\widehat{\mathit{X}}}_{k-1}}$$

(32)

$$Y_k=h_k\left({\widehat{\mathit{X}}}_{k/(k-1)}\right)-h_{k-1}\left({\widehat{\mathit{X}}}_{k-1}\right)-\left(H_k{\widehat{\mathit{X}}}_{k/k-1}-{\mathit{B}}_k{\widehat{\mathit{X}}}_{k-1}\right)$$

(33)

where ${\mathit{Z}}_k$ is the newly constructed measurement vector, which is the measurement difference at moment $k$ and moment $k-1$, ${\mathit{N}}_k={\mathit{V}}_k-{\mathit{V}}_{k-1}$ is the new measurement noise vector, and ${\mathit{B}}_k$ is white noise, which is the measurement matrix at moment $k-1$. From Equation (30), it is noted that the measurement equation of the EKF changes from the current moment of observation to a combined observation. The next step is to derive an improved EKF model based on combined observation from this.

Firstly, the equation of state is not improved by combining observations, so no changes have to be made compared to the common EKF. The set state vector ${\mathit{X}}_k$ satisfies Equation (26), the measurement vector satisfies Equation (30), and the system process noise ${\mathit{W}}_k$ and measurement noise ${\mathit{V}}_k$ satisfy:

$$\left\{\begin{array}{l}E[{\mathit{W}}_k]=0,E[{\mathit{W}}_k{\mathit{W}}_j^{\mathrm{T}}]={\mathit{Q}}_k{\delta }_{kj}\\ E[{\mathit{V}}_k]=0,E[{\mathit{V}}_k{\mathit{V}}_j^{\mathrm{T}}]={\mathit{R}}_k{\delta }_{kj}\\ E[{\mathit{W}}_k{\mathit{V}}_j^{\mathrm{T}}]=0\end{array}\right.$$

(34)

where the system noise variance ${\mathit{Q}}_k$ is non-negative and the measurement noise variance array ${\mathit{R}}_k$ is positive; ${\delta }_{kj}$ is a function of $Kronec \ker -\delta$. ${\mathit{X}}_k$ is uncorrelated with $\left.\left\{{\mathit{W}}_k\right.\right\}$ as well as $\left.\left\{{\mathit{V}}_k\right.\right\}$. The time update process of the combined-observation EKF can be obtained as follows:

For the improved EKF based on combined observation, one-step predictive state estimation is adopted:

$${\widehat{X}}_{k,k-1}=f_{k-1}({\widehat{X}}_{k-1})$$

(35)

The one-step predicted estimate covariance is:

$$P_{k,k-1}={\mathit{\varphi }}_{k,k-1}{P}_{k-1}{\mathit{\varphi }}_{k,k-1}^{\mathrm{T}}+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^{\mathrm{T}}$$

(36)

Then, based on the similar derivation method ideas in Ref. [49], the measurement update process of the EKF based on combined observation is recursively derived, including updating the state estimate ${\widehat{\mathit{X}}}_k$, the Kalman gain matrix ${\mathit{K}}_k$, and the covariance estimate ${\mathit{P}}_k$:

$${\widehat{X}}_k={\widehat{X}}_{k,k-1}+K_k\left[Z_k-{\widehat{Z}}_{k/k-1}\right]$$

(37)

$$\begin{array}{l}{K}_k=\left(P_{k/k-1}{H}_k^{\mathrm{T}}-{\mathit{\varphi }}_{k/k-1}{P}_{k-1}{\mathit{B}}_k^{\mathrm{T}}\right)\times \left(H_k{P}_{k/k-1}{H}_k^{\mathrm{T}}-H_k{\mathit{\varphi }}_{k/k-1}{P}_{k-1}{\mathit{B}}_k^{\mathrm{T}}\right.\\ {\left.-{\mathit{B}}_k{P}_{k-1}{\mathit{\varphi }}_{k/k-1}^{\mathrm{T}}{H}_k^{\mathrm{T}}+{\mathit{B}}_k{P}_{k-1}{\mathit{B}}_k^{\mathrm{T}}+R_k\right)}^{-1}\end{array}$$

(38)

$$P_k=P_{k/k-1}-K_k\left(H_k{P}_{k/k-1}{H}_k^{\mathrm{T}}-H_k{\mathit{\varphi }}_{k/k-1}{P}_{k-1}{C}_k^{\mathrm{T}}\right.\left.-{\mathit{B}}_k{P}_{k-1}{\mathit{\varphi }}_{k/k-1}^{\mathrm{T}}{H}_k^{\mathrm{T}}+{\mathit{B}}_k{P}_{k-1}{\mathit{B}}_k^{\mathrm{T}}+R_k\right)K_k^{\mathrm{T}}$$

(39)

To sum up, Equations (37)–(39) describe the derived measurement update process, and Equations (35) and (36) describe the time update process of the combined double-time observation algorithm.

3.3. The Combined-Observation EKF of Multi-AUV Cooperative Navigation

In the previous section, the common EKF model has been redesigned and the improved EKF algorithm based on combined observation has been obtained. In Ref. [36], Sheng proposed a KF algorithm based on combined double-time observation, but only for a linear model of transfer alignment and based on a single sensor. This study extends the application of the newly designed algorithm to nonlinear systems for cooperative navigation, and the extension from a single sensor can make full use of information from multiple AUV data sources to improve the reliability and performance of the filter.

In a multi-AUV cooperative navigation system, assuming that there are N sensors for measurement, the corresponding measurement equation can be expressed as:

$$Z_k^i=h_k^i\left(X_k\right)+V_k^i+e_k^i,i=1,2,\dots ,N$$

(40)

where $Z_k^i$ denotes the measurement vector of the i-th AUV at moment $k$; $h_k^i\left(X_k\right)$ denotes the nonlinear vector dispersion function of the i-th AUV at moment $k-1$; $V_k^i$ denotes the noise vector of the i-th AUV at moment $k$; and $e_k^i$ denotes the unknown measurement systematic error of the i-th AUV at moment $k$. $V_k^i$ is assumed to follow a Gaussian distribution with mean 0 and variance ${\mathit{R}}_k^i$, and the measured noise at the same time for each AUV is uncorrelated, as is the measured noise at different times for each AUV.

Then, based on the derivation process in Section 3.2, the measurement equation of the combined-observation EKF for multiple AUVs is constructed as:

$$Z_k^i=H_k^i{\mathit{X}}_k-{\mathit{B}}_k^i{\mathit{X}}_{k-1}+{\mathit{Y}}_k^i+{\mathit{N}}_k^i,i=1,2,\dots ,N$$

(41)

where $Z_k^i$ is the newly constructed measurement vector; ${\mathit{B}}_k$ is the measurement matrix of the i-th AUV at the time $k-1$, and ${\mathit{N}}_k^i={\mathit{V}}_k^i-{\mathit{V}}_{k-1}^i$ follows a Gaussian distribution with variance $\mathbf{2}{\mathit{R}}_k^i$. As can be seen from the above equation, in the process of multi-AUV cooperative positioning and navigation, the error of the measurement system cannot be calibrated by a common EKF but can be effectively eliminated by constructing the combined-observation EKF model.

In this study, the parallel filter algorithm is used for the combined-observation EKF model with multiple AUVs such that:

$$Z_k^{}={\left[{\left(Z_k^1\right)}^{\mathrm{T}},{\left(Z_k^2\right)}^{\mathrm{T}},\cdots ,{\left(Z_k^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(42)

$${\mathit{H}}_k^{}={\left[{\left({\mathit{H}}_k^1\right)}^{\mathrm{T}},{\left({\mathit{H}}_k^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathit{H}}_k^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(43)

$${\mathit{B}}_k^{}={\left[{\left({\mathit{B}}_k^1\right)}^{\mathrm{T}},{\left({\mathit{B}}_k^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathit{B}}_k^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(44)

$${\mathit{N}}_k^{}={\left[{\left({\mathit{N}}_k^1\right)}^{\mathrm{T}},{\left({\mathit{N}}_k^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathit{N}}_k^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(45)

$${\mathit{Y}}_k^{}={\left[{\left({\mathit{Y}}_k^1\right)}^{\mathrm{T}},{\left({\mathit{Y}}_k^2\right)}^{\mathrm{T}},\cdots ,{\left({\mathit{Y}}_k^N\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(46)

Then, at the fusion center, the combined-observation measurement equation for all AUVs can be expressed as:

$$Z_k^{}=H_k^{}{\mathit{X}}_k-{\mathit{B}}_k^{}{\mathit{X}}_{k-1}+{\mathit{Y}}_k^{}+{\mathit{N}}_k^{},i=1,2,\dots ,N$$

(47)

Similarly, the equation of state is shown in Equation (26), and the model for a two-moment combined-observation EKF consisting of multiple AUVs is shown in Equations (36)–(39).

4. Experimental Validation and Discussion

4.1. Simulation Test

4.1.1. Test Setting

According to the previous analysis, this section uses the MATLAB platform to simulate the new EKF method and verifies the performance and effectiveness of the improved cooperative navigation algorithm [50].

First, a multi-AUV cooperative navigation system is established, which consists of three AUVs, starting from three different positions and attitudes, and moving uniformly in a certain area. The starting attitude of the three AUVs is the same, but the starting position is different, and the three AUVs operate at a depth of −100 m underwater. The true linear velocity of the three AUVs is 10 m/s. The true angular velocities are 2 °/s.

In practice, the multi-AUV cooperative navigation system does not need to be equipped with different precision navigation equipment. Therefore, in the parallel cooperative navigation system in this section, all AUVs have the same configuration, and each AUV has the same or equal-precision navigation system and underwater acoustic communication equipment. The cooperative navigation system is a network system composed of multiple AUVs, as shown in Figure 1. While each AUV relies on SINS for autonomous navigation in the deep-sea environment, it cannot obtain other external auxiliary information such as GNSS. However, each AUV can continuously integrate the navigation information of adjacent AUVs, set the system update frequency of mutual observation $f=10 \mathrm{Hz}$, and observe and transmit the mutual distance and relative azimuth angle to each other. We adopt different information fusion algorithms to achieve cooperative navigation for comparison.

When there are unknown errors $ek$ in the measurement system, the improved EKF based on combined observation and the common EKF are compared, and the error of $ek$ in the relative distance measurement equation is set to a value of 20 m. The common EKF and the combined-observation EKF are used to fuse information for cooperative navigation, respectively. A total of 100 statistically independent realizations are composed using Monte Carlo simulation and only one of these simulations is given in this paper. The simulation time is 1000 s.

The estimated trajectories of the three AUVs against the reference used by the common EKF and the combined-observation EKF (COEKF) methods are shown in Figure 3. In this figure, the three blue solid lines indicate the model trajectories of the three AUVs; the three red lines show the trajectory estimated by the common EFK method when there is a measurement error $ek$; and the three green lines show the trajectory estimated by the COEKF when there is a measurement error $ek$.

Figure 3. The estimated trajectories of three AUVs.

4.1.2. Results of the Test

The velocity and position errors of the three AUVs are shown in Figure 4 and Figure 5, respectively. In Figure 4 and Figure 5, the blue, red, and green dotted lines indicate the cooperative positioning error curves of AUV1, AUV2, and AUV3 based on the common EKF, respectively. The red, green, and blue solid lines indicate the cooperative positioning error curves of AUV1, AUV2, and AUV3 based on the combined-observation EKF, respectively.

Figure 4. Velocity error comparison curve.

Figure 5. Position error comparison curve.

The relative horizontal distance error curves for cooperative multi-AUV navigation based on the combined-observation EKF are shown in Figure 6; the red, green, and blue dashed lines show the relative horizontal distance error curves between AUV1 and AUV2, between AUV1 and AUV3, and between AUV2 and AUV3, respectively, when autonomous navigation is based on pure SINS. The red, green, and yellow solid lines show the relative horizontal distance error curves between the individual AUVs based on the combined-observation EKF for cooperative navigation. The horizontal distance error statistics for multi-AUV cooperative navigation based on the combined-observation EKF are shown in Table 1.

Figure 6. Relative horizontal distance error for multi-AUV cooperative navigation.

Table 1. Relative distance error statistics.

Content	AUV12	AUV13	AUV23
RMSE/m	2.1449	1.9012	2.6130
AME/m	1.4524	1.4372	2.2626

From Figure 4 and Figure 5, it can be seen that the velocity and position errors of the common EKF for cooperative navigation have a rapid tendency to disperse over time when there are unknown errors in the measurement system and the cooperative navigation relies only on relative observation information. As mentioned in the introduction, cooperative navigation is closely related to state estimation, and unknown errors in measurement equations lead to a decrease in estimation accuracy or even divergence [36]. Therefore, the combined-observation EKF can successfully eliminate the unknown error $ek$ of the measurement system, effectively using the relative observation information between AUVs to delay the dispersion of cooperative navigation errors, and eliminating the influence of the measurement system errors on the dispersion of cooperative navigation errors. In addition to this, the horizontal positioning distance between the AUVs based on the combined-observation EKF algorithm can maintain high accuracy, as can be seen in Figure 6 and Table 1.

In summary, the simulation test results show that when there are unknown errors in the measurement system, the multi-AUV cooperative navigation and positioning algorithm based on the combined-observation EKF can effectively eliminate the influence of unknown errors, effectively use the relative observation information to delay the dispersion of navigation errors, improve the cooperative navigation and positioning accuracy to a certain extent, and has a high degree of flexibility and reliability.

4.2. Lake Experiments

4.2.1. Experiment Setting

The simulation test results above have proved that the combined-observation EKF algorithm proposed in this paper can effectively eliminate the measurement system error and, thus, can delay the dispersion of navigation errors. To continue to test the performance of the algorithm in practical engineering applications, we carried out lake experiments at Yuandang Lake in Suzhou city, using navigation data from three 9-ton surface boats for algorithm validation.

In this cooperative experiment, the reference value of each boat’s navigation position is provided by real-time kinematic (RTK) differential positioning data. The boats are equipped with a certain type of strapdown inertial navigation system, a RTK-GNSS receiver, and a navigation computer. The equipment diagram is shown in Figure 7. The equipment parameters of the SINS are shown in Table 2; the RTK-GNSS receiver includes a base station and multiple mobile terminals. The differential GNSS plane positioning accuracy is better than 0.5 m, the RTK plane positioning accuracy is 1 cm + 1 ppm, and the signal coverage area after access to the network is represented as a 10 km circle.

Figure 7. Equipment and platforms in the boat.

Table 2. Sensor errors.

Gyro		Accelerometer
Constant	Random	Constant	Random
<0.02 $°/\mathrm{h}$	<0.01 $°/\mathrm{h}$	<500 $\mathsf{\mu }\mathrm{g}$	<500 $\mathsf{\mu }\mathrm{g}$

Before the cooperative navigation of the boats, the SINS of each boat independently completes the initial alignment. In the initial alignment process, the boats use GNSS to complete the analytical rough alignment and then use the GNSS position and velocity measurement to achieve fine alignment [51]. After the initial alignment of the SINS is completed, the SINS is used as the onboard equipment to obtain measurement information stably. Due to the lack of available ranging data between boats, the inter-ship ranging reference value is obtained by the difference in the position reference values at the corresponding time, adding 0.2% of the Gaussian white noise of the ranging value to simulate cooperative information. To obtain comparison results of different cooperative navigation algorithms for the unknown error of the measurement system, following Ref. [22], an error $ek=50$ is added to the measurement system. To simulate multiple AUVs that cannot acquire GNSS signals underwater, each boat broadcasts to the other boats during the algorithm validation process, relying only on the relative measurement information shared between each boat for cooperative navigation, and each boat’s information broadcast frequency is set to 1 Hz. We used GNSS for clock synchronization before sailing, and sailing data comprising three segments of 500 s of circular motion after 2 h of sailing away from the pier were selected for algorithm verification, and the trajectory schematic is shown in Figure 8.

Figure 8. Schematic diagram of the trajectory.

4.2.2. Results of the Lake Experiment

The velocity and position errors of the three boats’ cooperative navigation are shown in Figure 9 and Figure 10, respectively. In Figure 9 and Figure 10, the blue, red, and green dotted lines indicate the cooperative positioning error curves of Boat 1, Boat 2, and Boat 3 based on the conventional EKF, respectively. The red, green, and blue solid lines indicate the cooperative positioning error curves of each boat based on the combined-observation EKF.

Figure 9. Velocity error comparison curve.

Figure 10. Position error comparison curve.

The relative horizontal distance error curves for cooperative multi-boat navigation based on the combined-observation EKF are shown in Figure 11; the red, green, and blue dashed lines show the relative horizontal distance error curves between Boat 1 and Boat 2, between Boat 1 and Boat 3, and between Boat 2 and Boat 3, respectively, when autonomous navigation is based on pure SINS. The red, green, and blue solid lines show the relative horizontal distance error curves between the individual boats based on the combined-observation EKF for cooperative navigation.

Figure 11. Relative horizontal distance error for multi-boat cooperative navigation.

From the results of the lake experiment in Figure 9 and Figure 10, we can find that when there is a measurement error in the actual measurement test of cooperative navigation, the position error based on the common EKF algorithm has a certain amount of error offset, and each shows a divergent trend; this shows that the common EKF cannot eliminate the influence of errors of the measurement system.

However, in these figures, it can be seen that the error value of the combined-observation EKF is smaller, and the respective error values are closer, which is more effective for the error processing of the measurement system. The horizontal distance error statistics for multi-AUV cooperative navigation based on the combined-observation EKF are shown in Table 3. It can be seen that, compared with Table 1, the error index is higher. Because the actual test is different from the ideal environment of the simulation, there are environmental factors and related sensor influences, such as GPS interference, that directly affect the measurement of the ideal position, but the performance of different algorithms under the same conditions can still show the superiority of the improved algorithm.

Table 3. Relative distance error statistics.

Content	AUV12	AUV13	AUV23
RMSE/m	15.6219	15.7213	20.9426
AME/m	13.2191	13.6113	16.1552

To sum up, this section verifies the feasibility and effectiveness of the multi-AUV cooperative navigation algorithm based on the combined-observation EKF proposed in this paper through simulation tests and lake experiments.

5. Conclusions

In this study, a cooperative navigation method based on a combined-observation algorithm is presented to solve the problem that unknown errors of measurement system are difficult to calibrate by common EKF methods, which affects the accuracy of multi-AUV cooperative navigation. The concept of combined observation is introduced into the underwater multi-AUV cooperative nonlinear system and re-establishes the EKF model. In the observation process of the algorithm, the measurement sampling values of the current moment and the previous moment are included. The combined observation can approximately eliminate the measurement system error at the algorithm level, avoid the impact of the measurement system errors on the performance of the filter, and improve the accuracy of cooperative navigation. Therefore, each AUV has the capability of bounded navigation and positioning errors, which can effectively reduce the impact of measurement system errors on the system’s navigation and positioning accuracy. Finally, the effectiveness and reliability of the improved algorithm are proven by simulation tests and lake experiments, respectively.