1 Introduction

With the development of the national marine strategy and the marine resource exploration, accurate ocean navigation and positioning technology are needed to obtain the high-precision, large-scale marine environmental information [1,2,3]. Sound waves, rather than electromagnetic waves or light waves, are mainly used to estimate the position of the underwater target. The reason is that sound waves can spread hundreds of kilometers in the water while electromagnetic waves and light waves decay quickly [4]. The classical acoustic-based approaches for underwater target positioning include long baseline (LBL), short baseline (SBL), ultrashort baseline (USBL) and underwater global positioning system (GPS) according to the acoustic baseline range [5, 6]. The shipborne acoustic positioning generally adopts the voyage positioning mode, which is affected by the geometric structure of trajectory and the measurement error related to the time delay as well as the sound speed [7, 8].

For underwater acoustic positioning, there inevitably exist the gross error, the random error and the systematic error caused by the marine environment and the observation instrument. Many studies have been dedicated to improve the underwater positioning model and the error correction method. Xu et al. [9] first proposed the underwater difference positioning algorithm including the single difference algorithm between the observation epochs and the double difference algorithm which can greatly improve the accuracy of seafloor deformation measurement. Zhao et al. [10] proposed a ship-board difference positioning method based on selecting weight iteration. Although the difference positioning algorithm can weaken the effects of the systematic errors, it enlarges the influence of random errors, which decreases the accuracy of the underwater acoustic positioning. Aiming at the time delay error, a positioning model considering the apparent time delay error as unknown parameter is proposed [11]. Yan et al. [12] proposed a long baseline positioning algorithm for moving buoy by estimating the uncertain sound speed as an unknown parameter. However, even though the time delay error or the unknown sound speed is estimated as a fixed systematic parameter, it is hard to be accurately estimated since it changes with the change of the marine environment. In the global navigation satellite system (GNSS) positioning, Paziewski and Wielgosz [13] used the random walk model to estimate the inter-system biases as the unknown parameters, which inspires us to apply this method to estimate the systematic error in underwater acoustic positioning. As for gross errors, Zhou [14] proposed the IGG robust estimation method based on the equivalence weight. On this basis, Yang et al. [15] developed the bi-factor equivalence weight based on the robust estimation for correlation observation. Xu et al. [16] proposed a robust estimation method based on the symbol constraint. Wang et al. [17] proposed a robust extended Kalman filter using W-test statistics based on filtering residuals to eliminate the effect of gross errors on GNSS navigation solutions. Yang et al. [18] proposed the robust M–M unscented Kalman filtering for GPS/IMU navigation. The applications of the robust estimation in GPS navigation and positioning have been widely adopted and tested [19]. Wang et al. [20] proposed an adaptive robust unscented Kalman filter for autonomous underwater vehicle (AUV) acoustic navigation, which constructs the judgment factor and adaptive factor by the prediction residual to balance the contribution between the observation information and AUV motion state information.

The zero-difference (ZD) least squares (LS) is rarely adopted for the high precision underwater positioning due to the effects of systematic error as well as gross error. At the same time, the single-difference (SD) LS of adjacent epoch enlarges the influence of random errors and even the gross errors while it suppresses the effects of systematic errors. To solve the aforementioned problems, this paper proposes a robust zero-difference Kalman filter based on the random walk model and the equivalent gain matrix to resist the effects of systematic errors and gross errors in underwater acoustic positioning. The proposed method involves a robust estimation method based on the prediction residual as well as the observation variance, and an improved KF with systematic error compensation, which has obvious differences compared to the method of reference [20].

The paper is organized as follows. We firstly present an improved zero-difference positioning function model as well as the zero-difference Kalman filter by estimating the systematic error as the random walk process in Sect. 2. Then Sect. 3 introduces the theoretical derivation and algorithm implementation of the robust zero-difference Kalman filter. The robust zero-difference Kalman filter is verified and analyzed by the simulation experiment and a real one for underwater acoustic positioning in Sect. 4. Finally, we summarize the significant conclusions in Sect. 5.

2 Method

2.1 Zero-difference positioning function model

The transducer under the survey ship can continuously send sound waves to the transponder to get the signal propagation time [21]; therefore, the range between the transducer and the transponder at the different time and position can be obtained by the travel time and the sound speed structure [22].

As shown in Fig. 1, assuming that the transducer at position \({\mathbf{X}}_{k}\) and time \(t_{k}\) transmits an acoustic signal to the transponder to get the slant range \(\rho_{k}\), the transponder coordinates can be obtained through the intersection positioning method, which can be regarded as a prototype of the underwater zero-difference positioning, since there are no differential operations on observations between epochs and transponder stations. The observation model of underwater zero-difference positioning can be expressed as

$$\rho_{k} = f\left( {{\mathbf{X}}_{k} ,{\mathbf{X}}_{o} } \right) + \delta \rho_{{d_{k} }} + \delta \rho_{{v_{k} }} + \varepsilon_{k} ,$$
(1)
$$f\left( {{\mathbf{X}}_{k} ,{\mathbf{X}}_{o} } \right) = \sqrt {(x_{k} - x_{o} )^{2} + (y_{k} - y_{o} )^{2} + (z_{k} - z_{o} )^{2} } ,$$
(2)
$$\rho_{k} = ct_{k} ,{\kern 1pt} {\kern 1pt}$$
(3)
Fig. 1
figure 1

The geometric diagram of underwater zero-difference positioning

Full size image

where \({\mathbf{X}}_{o} = (x_{o} ,y_{o} ,z_{o} )\) is the unknown position vector of the transponder, and \({\mathbf{X}}_{k} = (x{}_{k},y_{k} ,z_{k} )\) is the position vector of transducer under the ship, which can be directly calculated from the kinematic GNSS. \(f\left( {{\mathbf{X}}_{k} ,{\mathbf{X}}_{o} } \right)\) is the theoretical range between the transponder and the transducer. \(\delta \rho_{{d_{k} }}\) is the systematic error due to the time delay in re-transmitting the received signal from the transponder back to the transducer, \(\delta \rho_{{v_{k} }}\) is the systematic error due to the spatial and temporal variation in the sound speed structure, \(\varepsilon_{k}\) is the random ranging error. \(t_{k}\) is the travel time between the transducer and the transponder, and \(c\) is the sound speed.

In the actual underwater acoustic positioning, the sound speed error is affected by the ocean internal wave and has a periodic variation. In addition, the systematic error related to time delay for the same transponder is approximately equal [23, 24]. Therefore, the systematic error related to the time delay and the sound speed can be estimated as an unknown parameter and the observation Eq. 1 can be rewritten as

$$\rho_{k} = f\left( {{\mathbf{X}}_{k} ,{\mathbf{X}}_{o} } \right) + \delta \rho_{k} + \varepsilon_{k} ,$$
(4)

where \(\delta \rho_{k}\) is the estimated parameter of the systematic error.

Equation 4 is linearized as

$$\rho_{k} - f\left( {{\mathbf{X}}_{k} ,{\mathbf{X}}_{o}^{0} } \right) - \delta \rho_{k}^{0} = a_{k} d{\mathbf{X}}_{o} + d\delta \rho_{k} + \varepsilon_{k} + b_{k} \varepsilon_{{{\mathbf{X}}_{k} }} ,$$
(5)

where \({\mathbf{X}}_{o}^{0}\), \(\delta \rho_{k}^{0}\) are approximate values for \({\mathbf{X}}_{o}\) and \(\delta \rho_{k}\). \(d{\mathbf{X}}_{o}\) and \(d\delta \rho_{k}\) are the unknown coordinate correction vector and the systematic error correction vector to be estimated with \({\mathbf{X}}_{o} = {\mathbf{X}}_{o}^{0} + d{\mathbf{X}}_{o}\) and \(\delta \rho_{k} = \delta \rho_{k}^{0} + d\delta \rho_{k}\), respectively. \(a_{k}\) and \(b_{k}\) are the first-order partial derivatives with respect to \({\mathbf{X}}_{o}\) and \({\mathbf{X}}_{k}\), respectively, and \(\varepsilon_{{{\mathbf{X}}_{k} }}\) is the random errors of the survey ship positions.

When combining all the measurements, the linear observation equation of underwater zero-difference positioning can be expressed as:

$${\mathbf{Z}} = {\mathbf{H}}d{\mathbf{X^{\prime}}}_{o} + {\mathbf{V}},$$
(6)
$${\mathbf{H}} = \left[ {\begin{array}{*{20}c} {\frac{{\partial f_{1} }}{\partial x}} & {\frac{{\partial f_{1} }}{\partial y}} & {\frac{{\partial f_{1} }}{\partial z}} & 1 \\ {\frac{{\partial f_{2} }}{\partial x}} & {\frac{{\partial f_{2} }}{\partial y}} & {\frac{{\partial f_{2} }}{\partial z}} & 1 \\ {\frac{{\partial f_{3} }}{\partial x}} & {\frac{{\partial f_{3} }}{\partial y}} & {\frac{{\partial f_{3} }}{\partial z}} & 1 \\ \vdots & \vdots & \vdots & \vdots \\ {\frac{{\partial f_{n} }}{\partial x}} & {\frac{{\partial f_{n} }}{\partial y}} & {\frac{{\partial f_{n} }}{\partial z}} & 1 \\ \end{array} } \right],$$
(7)
$${\mathbf{Z}} = \left[ {\begin{array}{*{20}c} {\rho_{1} - f\left( {{\mathbf{X}}_{1} ,{\mathbf{X}}_{o}^{0} } \right) - \delta \rho_{1}^{0} } \\ {\rho_{2} - f\left( {{\mathbf{X}}_{2} ,{\mathbf{X}}_{o}^{0} } \right) - \delta \rho_{2}^{0} } \\ {\rho_{3} - f\left( {{\mathbf{X}}_{3} ,{\mathbf{X}}_{o}^{0} } \right) - \delta \rho_{3}^{0} } \\ \vdots \\ {\rho_{n} - f\left( {{\mathbf{X}}_{n} ,{\mathbf{X}}_{o}^{0} } \right) - \delta \rho_{n}^{0} } \\ \end{array} } \right],$$
(8)

where \({\mathbf{X^{\prime}}}_{o} = (x_{o} ,y_{o} ,z_{o} ,\delta \rho_{k}^{{}} )\), \({\mathbf{Z}}\) is the constant term, \({\mathbf{H}}\) is the coefficient matrix of observation equation, and \({\mathbf{V}}\) is the observation residual vector.

2.2 Zero-difference Kalman filter

The observation and state equations using zero-difference Kalman filter for underwater acoustic positioning can be expressed as [25]:

$${\mathbf{Z}}_{{\text{k}}} = {\mathbf{H}}_{k} d{\mathbf{X^{\prime}}}_{o,k} + {\mathbf{V}}_{k} ,$$
(9)
$${\mathbf{X^{\prime}}}_{o,k} = {\mathbf{\varphi }}_{k,k - 1} {\mathbf{X^{\prime}}}{}_{o,k - 1} + {{\varvec{\upomega}}}_{k - 1} ,$$
(10)

where \({\mathbf{X^{\prime}}}_{o,k} = (x_{o,k} ,y_{o,k} ,z_{o,k} ,\delta \rho_{k} )\) denotes the estimated parameter vector of the transponder position and the systematic error at time \(t_{k}\), \({\mathbf{Z}}_{{\text{k}}}\) is the observation vector with covariance matrix \({\mathbf{R}}_{k}\), assumed to be white, \({\mathbf{H}}_{k}\) is the coefficient matrix of observation equation and \({\mathbf{V}}_{k}\) is the observation residual vector. \({\mathbf{\varphi }}_{k,k - 1}\) is the state transition matrix from epoch \(t_{k - 1}\) to \(t_{k}\). \({{\varvec{\upomega}}}_{k - 1}\) is the process noise vector with covariance matrix \({\mathbf{Q}}_{k}\), assumed to be white.

The discrete first-order Gauss–Markov process [26] describes the epoch state changes of related parameters, and the mathematical expression is as follows:

$$\left\{ {\begin{array}{*{20}c} {{\mathbf{X^{\prime}}}_{o,k} = e^{{ - \frac{\Lambda t}{\tau }}} {\mathbf{X^{\prime}}}_{o,k - 1} + {{\varvec{\upomega}}}_{k - 1} ,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {{\varvec{\upomega}}}_{k - 1} = \int\limits_{{t_{k - 1} }}^{{t_{k} }} {e^{{ - \frac{\Lambda t}{\tau }}} } {{\varvec{\upomega}}}dt{\kern 1pt} {\kern 1pt} } \\ {E({{\varvec{\upomega}}}_{k} {{\varvec{\upomega}}}_{k - 1} ) = \frac{1}{2}\tau (1 - e^{{ - \frac{2\Lambda t}{\tau }}} )\delta } \\ \end{array} } \right.,$$
(11)

where \(\Lambda t = t_{k} - t_{k - 1}\), \(\tau\) is the time constant, and \(\delta\) is Dirac function.

When \(\tau \to \infty\), the state of Eq. 11 is the random walk process

$$\left\{ {\begin{array}{*{20}c} {{\mathbf{X^{\prime}}}_{o,k} = {\mathbf{X^{\prime}}}_{o,k - 1} + {{\varvec{\upomega}}}_{k - 1} {\kern 1pt} {\kern 1pt} } \\ {E({{\varvec{\upomega}}}_{k} {{\varvec{\upomega}}}_{k - 1} ) = \sigma_{{\mathbf{w}}}^{2} \delta } \\ \end{array} } \right..$$
(12)

When \(\tau \to 0\), the state of Eq. 11 is the white noise

$$\left\{ {\begin{array}{*{20}c} {{\mathbf{X^{\prime}}}_{o,k} = {{\varvec{\upomega}}}_{k - 1} {\kern 1pt} {\kern 1pt} } \\ {E({{\varvec{\upomega}}}_{k} {{\varvec{\upomega}}}_{k - 1} ) = \sigma_{{\mathbf{w}}}^{2} \delta } \\ \end{array} } \right.,$$
(13)

where \(\sigma_{{\mathbf{w}}}^{2}\) is the variance of the state parameters.

The unknown parameters of the zero-difference Kalman filter are the transponder position and the systematic error. Since the transponder position parameters are constants and the systematic error parameter changes regularly with time, the state transition matrix and the state noise matrix of Eq. 10 are given by

$${\mathbf{\varphi }}_{k,k - 1} = {\mathbf{I}}_{4 \times 4} ,$$
(14)
$${\mathbf{Q}}_{k} = \left[ {\begin{array}{*{20}c} 0 & {} & {} & {} \\ {} & 0 & {} & {} \\ {} & {} & 0 & {} \\ {} & {} & {} & {\sigma_{{\omega_{\delta \rho } }}^{2} } \\ \end{array} } \right],$$
(15)

where \({\mathbf{I}}_{4 \times 4}\) is a unit array of four rows and four columns, and \(\sigma_{{\omega_{\delta \rho } }}^{2}\) is the variance of the systematic error parameter.

The covariance propagation equation is given by

$${\mathbf{P}}_{k,k - 1} = {\mathbf{\varphi }}_{k,k - 1} {\mathbf{P}}_{k - 1} {\mathbf{\varphi }}_{k,k - 1}^{T} + {\mathbf{Q}}_{k - 1} .$$
(16)

The solutions for the estimated state vector, the Kalman filter gain matrix and the covariance matrix of the estimated state can be obtained as [25]

$${\mathbf{X^{\prime}}}_{o,k} = {\mathbf{X^{\prime}}}_{o,k - 1} + {\mathbf{K}}_{k} [{\mathbf{Z}}_{k} - {\mathbf{H}}_{k} d{\mathbf{X^{\prime}}}_{o,k - 1} ],$$
(17)
$${\mathbf{K}}_{k} = {\mathbf{P}}_{k,k - 1} {\mathbf{H}}_{k}^{T} [{\mathbf{H}}_{k} {\mathbf{P}}_{k,k - 1} {\mathbf{H}}_{k}^{T} + R_{k} ]^{ - 1} ,$$
(18)
$${\mathbf{P}}_{k} = [{\mathbf{I}} - {\mathbf{K}}_{k} {\mathbf{H}}_{k} ]{\mathbf{P}}_{k,k - 1} .$$
(19)

3 Robust zero-difference Kalman filter

In underwater acoustic positioning, when the observation contains the gross error, the measurement equation should be

$$\rho_{k} = f\left( {{\mathbf{X}}_{k} ,{\mathbf{X}}_{o} } \right) + \delta \rho_{k} + {\mathbf{B}}_{k} {{\varvec{\Delta}}}_{k} + \varepsilon_{k} ,$$
(20)

where \({\mathbf{B}}_{k}\) is the interference matrix of gross error, and \({{\varvec{\Delta}}}_{k}\) is the gross error.

If the standard Kalman filter is still used for calculation, the prediction residual with the gross error is as follows:

$$\tilde{V}_{k} = \rho_{k} - f\left( {{\mathbf{X}}_{k} ,{\mathbf{X}}_{o} } \right) - \delta \rho_{k} = V_{k} + {\mathbf{B}}_{k} {{\varvec{\Delta}}}_{k} ,$$
(21)

where \(V_{k}\) and \(\tilde{V}_{k}\) are the prediction residual vector without the gross error and with the gross error respectively.

The gross error is fully reflected in the prediction residual, then the state vector is

$${\mathbf{X^{\prime}}}_{k} = {\mathbf{X^{\prime}}}_{k,k - 1} + {\mathbf{K}}_{k} \tilde{V}_{k} .$$
(22)

According to Eq. 22, the gross error in the observation affects the state vector \({\mathbf{X^{\prime}}}_{k}\) through the gain matrix \({\mathbf{K}}_{{\text{k}}}\). To resist the influence of gross error, a robust zero-difference Kalman filter is adopted based on the equivalent gain matrix. By using the prediction residual and the observation variance to construct the judgment factor \(S_{k}\), the gross error in the observation equation can be efficiently detected. Based on the idea of equivalent gain matrix [20] and equivalent weight function of Huber [27, 28], the equivalent gain matrix related to the constant \(k_{0}\) is constructed to reduce the influence of gross error. The equivalent gain matrix is expressed as

$${\overline{\mathbf{K}}}_{k} = \left\{ {\begin{array}{*{20}c} {{\mathbf{K}}_{k} } & {\left| {S_{k} } \right| < k_{0} } \\ {{\mathbf{K}}_{k} \frac{{k_{0} }}{{\left| {S_{k} } \right|}}} & {\left| {S_{k} } \right| \ge k_{0} } \\ \end{array} } \right.,$$
(23)

where \(k_{0}\) is a constant, generally, \(k_{0} = 1\sim 2\).

$$S_{k} = V_{k} /\sqrt {{\mathbf{D}}_{V(k)} } ,$$
(24)
$${\mathbf{D}}_{V(k)} = [{\mathbf{I}} - {\mathbf{H}}_{k} {\mathbf{K}}_{k} ]R_{k} ,$$
(25)

where \(V_{k}\) is the prediction residual vector, and \({\mathbf{D}}{}_{V(k)}\) is the covariance matrix of observation vector.

When there exists gross error in the observations, the corresponding \({\overline{\mathbf{K}}}_{k}\) will be decreased and the influence of gross error on KF will be reduced.

To resist the effects of both systematic error and gross error, a robust zero-difference Kalman filter based on the random walk model and the equivalent gain matrix is proposed. The flowchart of the proposed algorithm is shown in Fig. 2. The detailed steps of the algorithm are explained as follows:

  1. 1.

    The initial state vector \({\mathbf{X^{\prime}}}_{o,1} = (x_{o,1} ,y_{o,1} ,z_{o,1} ,\delta \rho_{1} )\) including the transducer position and the systematic error is given.

  2. 2.

    The state vector \({\mathbf{X^{\prime}}}_{o,k}\) and the corresponding covariance matrix \({\mathbf{P}}_{k,k - 1}\) are calculated by Eqs. 10 and 16.

  3. 3.

    The gain matrix of the Kalman filter \({\mathbf{K}}_{{\text{k}}}\) is computed by Eq. 18. The equivalent gain matrix \({\overline{\mathbf{K}}}_{k}\) based on robust estimation is calculated by Eq. 23.

  4. 4.

    The state vector and the error covariance matrix are estimated by Eqs. 26 and 27.

    $${\mathbf{X^{\prime}}}_{o,k} = {\mathbf{X^{\prime}}}_{o,k - 1} + {\overline{\mathbf{K}}}_{k} [Z_{k} - {\mathbf{H}}_{k} d{\mathbf{X^{\prime}}}_{o,k - 1} ],$$
    (26)
    $${\mathbf{P}}_{k} = [{\mathbf{I}} - {\overline{\mathbf{K}}}_{k} {\mathbf{H}}_{k} ]{\mathbf{P}}_{k,k - 1} .$$
    (27)
  5. 5.

    The steps (2)–(4) are repeated until filter convergence.

Fig. 2
figure 2

The flowchart of the robust zero-difference Kalman filter

Full size image

4 Simulation and real experimental analysis

4.1 Simulation analysis

Simulation analysis on the acoustic positioning based on the proposed method is conducted in this paper. As shown in Fig. 3, the four transponders are located at the positions of the asterisk symbol with the different underwater depth of 30 m, 100 m, 500 m and 3000 m. The trajectories of the ship are circles with the radius of 100 m, 200 m, 800 m and 3000 m as well as the linear track with the grid shape. The sampling interval is 2 s and the speed of the survey ship is about 1 m/s. The measured sound speed profile of 3000 m is adopted for the calculation of sound speed. The layered ray acoustic tracking algorithm is used to simulate the travel time, and the systematic error [29] is simulated based on Eq. 28 proposed in Xu et al. [9]. The slant range error caused by the random error is 0.1 m; therefore, the initial measurement noise variance is \(R = 0.01\) m2. The initial system noise matrix is \({\mathbf{Q}} = {\text{diag}}[0{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} 0{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} 0{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} 0.001]\) m2.

$$\delta \rho_{{\text{v}}} = c_{1} + c_{2} \sin \left( {\frac{{2\left( {t - t_{0} } \right)}}{{T_{S} }}\pi } \right) + c_{3} \sin \left( {\frac{{\left( {t - t_{0} } \right)}}{{T_{L} }}\pi } \right){ + }c_{4} \left[ {1 - \exp \left\{ { - \frac{1}{2}\left\| {{\mathbf{X}}_{o} - {\mathbf{X}}} \right\|/\left( {2{\text{km}}} \right)^{2} } \right\}} \right],$$
(28)
Fig. 3
figure 3

The diagram of simulated ship and transponder

Full size image

where the constant term is \(c_{1} = 0.1{\kern 1pt}\) m, the short-period internal wave error term is \(c_{2} = 0.12{\kern 1pt}\) m, the long-period error term is \(c_{3} = 0.3{\kern 1pt}\) m, the term related to the measurement range is \(c_{4} = 0.02{\kern 1pt}\) m, the short period of internal wave is \(T_{S} = 15*60{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\text{s}}{\kern 1pt} {\kern 1pt}\) (equal to 15 min) and the long period of internal wave is \(T_{L} = 12*3600{\kern 1pt} {\kern 1pt} {\kern 1pt} {\text{s}}{\kern 1pt} {\kern 1pt}\) (equal to 12 h). \(\left\| {{\mathbf{X}}_{o} - {\mathbf{X}}} \right\|\) is the distance between the transducer and the transponder.

The zero-difference least squares (LS), the single-difference least squares (SD), the zero-difference Kalman filter (KF), the robust zero-difference least squares(R-LS), the robust single difference least squares (R-SD) and the robust zero-difference Kalman filter (R-KF) are conducted and compared for underwater acoustic positioning. Firstly, the proposed algorithm is validated in the case without gross error. The observation residuals of LS, SD and KF as well as the estimated systematic errors by KF are shown in Figs. 4 and 5. Monte Carlo experimental simulation with 100 times is conducted, the root mean squares (RMS) of the transponder position calculated at the different depth and the different algorithm is shown in the Fig. 6. In calculating the formula of RMS, \(X_{o,k}\) and \(\hat{X}_{o,k}\) are the calculation value and the real value of the transponder respectively. \(N\) is the number of the transponder.

Fig. 4
figure 4

The observation residuals of LS, SD and KF at different depths

Full size image
Fig. 5
figure 5

The results of the estimated systematic errors by KF at different depths

Full size image
Fig. 6
figure 6

The underwater positioning results of different algorithms at different depths

Full size image

As shown in Figs. 4 and 5, the SD can effectively reduce the effects of systematic errors compared with the LS. However, the SD produces larger random errors compared with the KF. The KF can effectively estimate systematic error parameters, together with position parameter without enlarging the influence of random errors. Therefore, the KF can significantly improve the underwater positioning accuracy.

As shown in Fig. 6 and Table 1, when there exist random errors, systematic errors and no gross errors in the acoustic observations, the LS cannot resist the effects of the systematic errors on the positioning result, especially in the Z direction of coordinates. The SD and KF can both reduce the influence of the systematic errors and greatly improve the positioning accuracy. For the case of underwater 30 m depth, the three-dimension (3D) RMSs of SD and KF are 0.065 m and 0.048 m, respectively, compared to LSs 0.455 m, with the improvement of 85.7% and 89.4%. For the case of 100 m depth, they are 0.087 m and 0.044 m, respectively, compared to LSs 0.380 m, with the improvement of 77.1% and 88.4%. For the case of 500 m depth, they are 0.090 m and 0.040 m, respectively, compared to LSs 0.406 m, with the improvement of 77.8% and 90.1%. For the case of 3000 m depth, they are 0.110 m and 0.065 m, respectively, compared to LSs 0.504 m, with the improvement of 78.2% and 87.1%. In addition, the KF can further enhance the positioning accuracy with about 5–12% improvement compared to the SD.

Table 1 The positioning result statistics of different algorithms
Full size table

Secondly, to further verify the performance of LS, SD and KF in the case of the ship trajectories with irregular curve, the transponders with the different underwater depth of 30 m, 100 m, 500 m and 3000 m are positioned by the simulated trajectories as shown in Fig. 7. Table 2 presents the RMSs of different algorithms and depths. For the case of underwater 30 m depth, the 3D RMSs of SD and KF are 0.079 m and 0.057 m compared to the 0.362 m of LS, with the improvement of 78.2% and 84.3%. For the case of 100 m depth, they are 0.175 m and 0.082 m compared to the 0.304 m of LS, with the improvement of 42.4% and 73.0%. For the case of 500 m depth, they are 0.127 m and 0.061 m compared to the 0.373 m of LS, with the improvement of 66.0% and 83.6%. For the case of 3000 m depth, they are 0.223 m and 0.165 m compared to the 0.442 m of LS, with the improvement of 47.3% and 62.7%. Therefore, the performance of KF is also better than that of LS and SD in the case of the ship trajectories with irregular curve.

Fig. 7
figure 7

The diagram of the simulated ship and transponder in different depths

Full size image
Table 2 The positioning result statistics of different algorithms
Full size table

Finally, the time delay observation is added to the gross errors based on normal distribution with zero mean and standard deviation of 0.05 s, and the gross errors are added in the acoustic observations every 50 s. Figure 8 shows the result of the estimated systematic error by the R-KF. At the same time, the positioning results of the non-robust estimation and the robust estimation are shown in Figs. 9, 10, 11 and 12.

Fig. 8
figure 8

The results of the estimated systematic errors by R-KF at different depths

Full size image
Fig. 9
figure 9

The positioning results at the depth of underwater 30 m

Full size image
Fig. 10
figure 10

The positioning results at the depth of underwater 100 m

Full size image
Fig. 11
figure 11

The positioning results at the depth of underwater 500 m

Full size image
Fig. 12
figure 12

The positioning results at the depth of underwater 3000 m

Full size image

Figure 8 shows that the R-KF can resist the influence of gross errors on the position and systematic error parameter based on the equivalent gain matrix. As shown in the Figs. 9, 10, 11 and 12, the accuracy of the SD is significantly reduced compared with the LS and the KF due to the effects of the gross errors. The R-SD can also resist the influences of the gross errors by the robust estimation and reduce the influence of the systematic errors to improve the accuracy of the underwater positioning compared with the R-LS. However, the R-SD has also the disadvantage of enlarging the effects of random errors, which leads to the positioning accuracy lower than that of the R-KF. The R-KF can estimate the systematic errors by the random walk process without enlarging the influence of the random errors, and provide robust solutions by using the equivalent gain matrix, which has higher precision and stability than those of the other two algorithms.

The means of RMS for 100 times of each algorithm are shown in Table 3. From Table 3, it can be seen that the positioning accuracy of the LS, SD and KF is greatly decreased by gross errors, especially for the SD due to the enlarged gross errors. R-LS, R-SD and R-KF can obviously improve the positioning by using robust estimation to resist the influence of the gross errors. For the case of underwater 30 m depth, the three-dimension (3D) RMSs of R-SD and R-KF are 0.076 m and 0.045 m, respectively, compared to R-LSs 0.463 m, with the improvement of 83.5% and 90.3%. For the case of 100 m depth, they are 0.103 m and 0.051 m, respectively, compared to LSs 0.378 m, with the improvement of 72.7% and 86.5%. For the case of 500 m depth, they are 0.099 m and 0.040 m, respectively, compared to LSs 0.406 m, with the improvement of 75.6% and 90.1%. For the case of 3000 m depth, they are 0.137 m and 0.062 m, respectively, compared to LSs 0.504 m, with the improvement of 72.8% and 87.7%. In addition, the R-KF can further enhance the positioning accuracy with about 7–15% improvement compared to R-SD.

Table 3 The positioning result statistics of different algorithms
Full size table

4.2 Real experiment analysis

The in situ data were collected from an experiment conducted at Lingshan Island in Dec. 2017. Lingshan Island is located in Qingdao City, Shandong Province in China with longitude and latitude about 120° 13′ 02″ E, and 35° 46′ 53″ N, respectively. The single transponder is located at the ocean bottom, and the trajectory of the voyage move is centered around the transponder with a radius of about 50 m. The ultrashort baseline response mode is used for underwater positioning, and GPS receiver, attitude sensor and sound velocity profiler are auxiliary installed.

After preprocessing the measured data, LS, SD, KF, R-LS, R-SD and R-KF are used for the positioning calculation and then compared. The observation noise and the system noise of Kalman filter are set as \(R = 1\) m2 and \(Q = {\text{diag}}[0{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} 0{\kern 1pt} {\kern 1pt} {\kern 1pt} 0{\kern 1pt} {\kern 1pt} {\kern 1pt} 0.001]\) m2, respectively. Since the position of the underwater transponder in the experimental area is unknown, the positioning accuracy cannot be directly evaluated. To verify the accuracy of the algorithm, as shown in Fig. 13, the observations that are not involved in positioning calculation on the trajectory are selected to calculate the residuals, namely the observation ranges minus computation/theory ranges (O–C).

Fig. 13
figure 13

The trajectory of ship and checkpoint

Full size image

As shown in Table 4, the RMS of the validated residuals of the SD is lower than that of the LS and the KF. The reason may be that: (1) since the experiment is conducted in shallow sea, the influence of the systematic errors is relatively small; (2) the systematic error between the adjacent epochs are not exactly equal in the actual observations, and the SD cannot totally eliminate the systematic errors; (3) SD enlarges the influence of random errors. When using robust estimation, all the RMSs of the three methods decrease, which indicates that robust estimation can efficiently control the influence of gross errors. Compared with the R-LS and the R-SD, the RMS of the validated residuals of the R-KF is greatly reduced from 1.63 m and 1.81 m, respectively, to 0.85 m, which proves the higher precision of R-KF. From Fig. 14, it can be seen that both the KF and the R-KF need some certain epochs to make the filtering solution convergence. There is a bias between the solutions of R-KF and KF, since the former uses robust estimation to reduce the influence of the gross errors on the systematic error parameter, while the latter has no action on gross errors and inevitably brings the deviation of solution for underwater positioning.

Table 4 The residuals statistics of the 20 epochs
Full size table
Fig. 14
figure 14

The positioning result of epoch by epoch (blue line represents KF and red line is R-KF)

Full size image

5 Conclusion

To reduce the effects of the systematic error and the gross error on the underwater positioning, this paper proposes a robust zero-difference Kalman filter based on the random walk model and the equivalent gain matrix. After the validation from the simulation experiment and a real example, the following conclusions can be drawn.

  1. 1.

    Compared with the zero-difference LS, the single-difference LS between the observation epochs can reduce the influence of the systematic error. However, it also enlarges the influence of the random errors and the gross errors. Although the robust single-difference LS can eliminate the influence of the gross errors by the robust estimation, its accuracy of underwater positioning is greatly reduced by the enlarged random errors and the remained systematic errors. The accuracy of zero-difference Kalman filter can be significantly improved compared to the zero-difference LS and the single-difference LS. At the same time, the zero-difference Kalman filter has the better performance in the case of the ship trajectories with irregular curve.

  2. 2.

    The proposed robust zero-difference Kalman filter can estimate the systematic error by the random walk model without enlarging the influence of the random errors, and resist the influence of the gross error by the equivalent gain matrix. In the simulating experiment, the positioning accuracy of the proposed algorithm is obviously superior to that of robust zero-difference LS and robust single-difference LS with the improvement of about 86–90% and about 5–15%, respectively. In the real data experiment, the RMS of the validated residuals of the robust zero-difference Kalman filter is about 0.8 m, and obviously less than that of robust zero-difference LS and the robust single-difference LS, which proves that the proposed algorithm has higher accuracy and stability.