Keywords

1 Introduction

The ocean is rich in resources, and all countries in the world have focused on the ocean. With the implementation of China’ s maritime power strategy, various types of marine resource development work have sprung up [1, 2], and these works need the support of high-precision and high-reliability marine geodetic datum and marine navigation technology [3,4,5]. Due to the serious attenuation of information carriers such as electromagnetic waves under water, they cannot penetrate the water layer, and the attenuation of sound waves under water is much smaller than that of electromagnetic waves, making underwater acoustic navigation technology the core technology of underwater navigation [6]. According to the baseline length, underwater acoustic navigation technology can be divided into long baseline navigation technology, short baseline navigation technology and ultra-short baseline navigation technology. The long baseline navigation technology has a wide range of applications and high navigation accuracy [7, 8], which makes it the most widely used and most mature underwater acoustic navigation technology [8].

Underwater vehicle navigation is often solved by Kalman filter (KF) [9, 10]. Kalman filter not only has high computational efficiency, but also can be used for real-time estimation. However, when the underwater measurement environment is complex, the acoustic observation often contains gross errors. In addition, the motion state of the underwater vehicle is changeable, and the single model filtering has the problem of model matching misalignment or failure, which reduces the navigation accuracy of traditional filtering methods such as Kalman filtering and cannot meet the needs of high-precision underwater navigation [11, 12].

To this end, scholars in related fields have conducted a lot of research. For the problem of gross error in observations, Huber proposed a generalized maximum likelihood estimation (M-estimation), which eliminates or weakens the influence of gross error on the estimated value by constructing an equivalent weight function [13, 14]. Because M-estimation is easy to implement, it becomes the most widely used robust estimation method. On the basis of M-estimation, many scholars have proposed the construction methods of equivalent weight functions with different characteristics, including Hampel method, IGG method, IGG-III method, etc., which have good robustness [14, 15]. Aiming at the problem of variable motion state, Blom and Bar-Shalom proposed the Interacting Multiple Model (IMM) algorithm [16]. The IMM uses multiple motion models to describe the motion state of the underwater vehicle, and estimates the model probability of each model in real time to realize the free switching of the motion model, thereby reducing the influence of variable motion state on navigation accuracy. On the basis of the above methods, in order to further improve the navigation accuracy, ZHU proposed a robust IMM-KF (IMM-RKF) algorithm based on Mahalanobis distance [17]. When gross error occurs, the measurement noise variance of the algorithm will increase, and the likelihood function is recalculated by using the expanded measurement noise variance. LIU proposes an IMM navigation algorithm based on Robust Cubature Kalman Filter (RCKF) [18]. A robust factor is used to weaken the influence of measurement outliers on the median filtering solution, and then IMM algorithm is used to fuse the estimation results of each model. The essence of the above two robust interacting multiple model (RIMM) algorithms is to combine robust Kalman filter (RKF) with IMM algorithm, which can not only adaptively adjust the model, but also reduce the influence of gross error on navigation results by equivalent variance replacement. However, in the robust interacting multiple model algorithm, the mismatched model has the problem of abnormal corrections of normal observations when estimating the model, resulting in the system misjudged the observation of abnormal corrections as gross errors, resulting in inaccurate state estimation of the model and affecting the probability update of the model. In addition, the robust interacting multiple model method also has the problem of unreasonable model probability, so as to reduce the navigation accuracy of long underwater acoustic baseline.

In view of the above problems, this paper presents an improved robust interacting multiple model filtering algorithm. In this algorithm, the mixed equivalent variance is constructed through the interaction of multiple models, and the mixed equivalent variance is introduced into each model to realize the interacting robust estimation. Then, the matching model probability is replaced by a power function with a faster growth rate to realize the model probability correction. Finally, according to the corrected model probability, the weighted sum of the results of the interacting robust estimation of each model is obtained to obtain the final state estimation value and its covariance matrix, so as to improve the navigation accuracy of underwater acoustic long baseline.

2 Underwater Acoustic Long Baseline Navigation Algorithm Based on Robust Interacting Multiple Model

2.1 Underwater Acoustic Long Baseline Navigation Technology

The underwater acoustic long baseline navigation system consists of two parts, including a transducer mounted on the underwater carrier and a transponder deployed on the seabed, as shown in Fig. 1. The basic principle of the long baseline navigation system is as follows: the transducer transmits the inquiry acoustic signal, and the seabed transponder receives the inquiry signal and then transmits the response signal. The response signal is received by the transducer installed on the underwater vehicle, so as to obtain the acoustic wave propagation time from the underwater vehicle to the seabed transponder. Then, the distance between the underwater vehicle and each transponder is calculated by combining the sound velocity profile. Finally, the position of the underwater vehicle is calculated according to the geometric relationship between the underwater vehicle and the transponder.

The observation model of underwater acoustic long baseline navigation technology is:

$$\begin{array}{*{20}c} {\left\{ {\begin{array}{*{20}l} {\rho_{i} = f\left( {\varvec{X},{\varvec{X}}_{i} } \right)} \hfill \\ {\rho_{i} = ct_{i} } \hfill \\ {f_{i} \left( {\varvec{X},{\varvec{X}}_{i} } \right) = \sqrt {(x - x_{i} )^{2} + (y - y_{i} )^{2} + (z - z_{i} )^{2} } } \hfill \\ \end{array} } \right.} & {i = 1,2, \cdots ,N} \\ \end{array}$$
(1)

where \({\varvec{X}} = \left( {x,y,z} \right)\) represents the coordinate of the underwater vehicle, \(N\) represents the number of transponders laid on the seabed, \({\varvec{X}}_{i} = \left( {x_{i} ,y_{i} ,z_{i} } \right)\) represents the coordinate of the transponder \(i\), \(c\) is the sound velocity, and \(t_{i}\) is the propagation time of the sound signal between the underwater vehicle and the transponder \(i\).

Fig. 1.
figure 1

Diagram of underwater acoustic long baseline navigation system

Full size image

All observation equations are expressed in the form of matrix:

$${\varvec{Z}} = F\left( {\varvec{X}} \right)$$
(2)
$${\varvec{Z}} = \left[ {\begin{array}{*{20}c} {\rho_{1} } \\ {\rho_{2} } \\ \vdots \\ {\rho_{i} } \\ \end{array} } \right]$$
(3)
$$F\left( {\varvec{X}} \right) = \left[ {\begin{array}{*{20}c} {f_{1} \left( {\varvec{X},{\varvec{X}}_{1} } \right)} \\ {f_{2} \left( {\varvec{X},{\varvec{X}}_{2} } \right)} \\ \vdots \\ {f_{i} \left( {\varvec{X},{\varvec{X}}_{i} } \right)} \\ \end{array} } \right]$$
(4)

Linearize formula (2) to \({\varvec{Z}} = {\varvec{HX}}\), Where \({\varvec{H}}\) is the Jacobi matrix of \(F\left( {\varvec{X}} \right)\) at \({\varvec{X}}\).

2.2 Robust Kalman Filter

Robust Kalman filter is based on the standard Kalman filter using equivalent variance to replace the measurement noise variance. In underwater acoustic long baseline navigation system, the equivalent variance can be expressed as:

$$\left\{ {\begin{array}{*{20}l} {\overline{R}_{ii} = \lambda_{i} \cdot R_{ii} } \hfill \\ {\lambda_{i} = \frac{1}{{\eta_{i} }}} \hfill \\ \end{array} } \right.$$
(5)

In the formula, \(R_{ii}\) and \(\overline{R}_{ii}\) represent the variance and equivalent variance of the \(i\)-th observation, \(\lambda_{i}\) represents the reciprocal of the equivalent weight function, and \(\eta_{i}\) is the equivalent weight factor.

Because the calculation of \(\lambda_{i}\) involves the reciprocal operation, this paper adopts Huber equivalent weight function. At this time, \(\eta_{i}\) can be expressed as:

$$\eta_{i} = \left\{ {\begin{array}{*{20}l} 1 & {\tilde{v}_{i} \le c} \\ {\frac{c}{{\left| {\tilde{v}_{i} } \right|}}} & {\tilde{v}_{i} > c} \\ \end{array} } \right.$$
(6)

In the formula, \(c\) is a constant, generally \(c = 2.5{-}3.0\). \(\tilde{v}_{i}\) is the standardized residual, the calculation method is [19]:

$$\left\{ {\begin{array}{*{20}l} {\tilde{v}_{i} = \frac{{v_{i} }}{{\sigma_{i} }}} \hfill \\ {\sigma_{i} = \frac{{\sigma_{0} }}{{\sqrt {p_{i} } }}} \hfill \\ {\sigma_{0} = med\left\{ {\left| {\sqrt {p_{i} } \cdot v_{i} } \right|} \right\}/0.6745} \hfill \\ \end{array} } \right.$$
(7)

In the formula, \(v_{i}\) is the residual and \(\sigma_{i}\) is the mean square error of the residual; \(p_{i}\) is the original weight of the observation value, which can be obtained by taking the reciprocal of \(R_{ii}\); \(\sigma_{0}\) is the mean square error factor, and \(med\) represents the median.

2.3 Robust Interacting Multiple Model

The state equation and observation equation of underwater acoustic long baseline navigation can be expressed as:

$$\left\{ {\begin{array}{*{20}l} {\varvec{X}\left( k \right) = \varvec{A}\left( k \right)\varvec{X}\left( {k - 1} \right) + \varvec{W}\left( k \right)} \hfill \\ {\varvec{Z}\left( k \right) = \varvec{H}\left( k \right)\varvec{X}\left( k \right) + \user2{\varepsilon } \left( k \right)} \hfill \\ \end{array} } \right.$$
(8)

In the formula, \({\varvec{X}}\left( k \right)\) represents the state of the underwater vehicle at \(k\) time, \({\varvec{A}}\left( k \right)\) is the state transition matrix from time \(k - 1\) to time \(k\), \({\varvec{W}}\left( k \right)\) is the Gaussian white noise process error vector, and its covariance matrix is \({\varvec{Q}}\left( k \right)\); \({\varvec{Z}}\left( k \right)\) represents the observation vector, the covariance matrix is \({\varvec{R}}\left( k \right)\), \({\varvec{H}}\left( k \right)\) is the observation matrix, \({\user2{\varepsilon}}\left( k \right)\) is the observation noise vector.

The essence of RIMM algorithm is to replace Kalman filter in IMM algorithm with robust Kalman filter. The specific steps are as follows:

  1. 1.

    Input interaction

Suppose the state estimation value of the \(i\)-th \(\left( {i = 1,2, \cdots ,r} \right)\) model at time \(k - 1\) is \(\widehat{\user2{X}}_{i} (k - 1|k - 1)\), the covariance matrix is \({\varvec{P}}_{i} (k - 1|k - 1)\), and the model probability is \(\mu_{i} \left( {k - 1} \right)\). The mixing probability from model \(i\) to model \(j\) is:

$$\left\{ {\begin{array}{*{20}l} {\mu_{ij} (k - 1|k - 1) = \mathop \sum \nolimits_{i = 1}^{r} p_{ij} \mu_{i} \left( {k - 1} \right)/\overline{c}_{j} } \hfill \\ {\overline{c}_{j} = \mathop \sum \nolimits_{i = 1}^{r} p_{ij} \mu_{i} \left( {k - 1} \right)} \hfill \\ \end{array} } \right.$$
(9)

where \(p_{ij}\) is the transition probability from model \(i\) to model \(j\).

The mixed estimation of model \(j\) and the calculation method of its covariance are:

$$\widehat{\user2{X}}_{Oj} (k - 1|k - 1) = \mathop \sum \nolimits_{i = 1}^{r} \widehat{\user2{X}}_{i} (k - 1|k - 1) \cdot \mu_{ij} \left( {k - 1|k - 1} \right)$$
(10)
$$\begin{aligned} & \user2{P}_{{Oj}} \left( {k - 1{\text{|}}k - 1} \right) = \mathop \sum \nolimits_{{i = 1}}^{r} \mu _{{ij}} \left( {k - 1{\text{|}}k - 1} \right)\{ \user2{P}_{i} (k - 1|k - 1) + [\user2{\widehat{X}}_{i} (k - 1|k - 1) \\ & - \user2{\widehat{X}}_{{Oj}} (k - 1|k - 1)\left] \cdot \right[\user2{\widehat{X}}_{i} (k - 1|k - 1) - \user2{\widehat{X}}_{{Oj}} (k - 1|k - 1)]^{T} \} \\ \end{aligned}$$
(11)
  1. 2.

    Filtering estimation

The state variables and covariance matrix after input interaction are substituted into the robust Kalman filter for state estimation. The main steps are as follows:

State prediction:

$$\widehat{\user2{X}}_{j} (k|k - 1) = {\varvec{A}}_{j} \left( {k - 1} \right)\widehat{\user2{X}}_{Oj} (k - 1|k - 1)$$
(12)

Prediction state covariance matrix:

$${\varvec{P}}_{j} (k|k - 1) = {\varvec{A}}_{j} {\varvec{P}}_{Oj} (k - 1|k - 1){\varvec{A}}_{j}^{T} + {\varvec{Q}}_{j} \left( k \right)$$
(13)

Innovation vector:

$${\varvec{v}}_{j} \left( k \right) = {\varvec{Z}}\left( k \right) - {\varvec{H}}\left( k \right)\widehat{\user2{X}}_{j} (k|k - 1)$$
(14)

The covariance of innovation vector:

$${\varvec{S}}_{j} \left( k \right) = {\varvec{H}}\left( k \right){\varvec{P}}_{j} (k|k - 1){\varvec{H}}(k)^{T} + \overline{\user2{R}}_{j} \left( k \right)$$
(15)

Gain matrix of Kalman filter:

$${\varvec{K}}_{j} \left( k \right) = {\varvec{P}}_{j} (k|k - 1){\varvec{H}}(k)^{T} {\varvec{S}}_{j} (k)^{ - 1}$$
(16)

Status update:

$$\widehat{\user2{X}}_{j} (k|k) = \widehat{\user2{X}}_{j} (k|k - 1) + {\varvec{K}}_{j} \left( k \right){\varvec{v}}_{j} \left( k \right)$$
(17)
$${\varvec{P}}_{j} (k|k) = {\varvec{P}}_{j} (k|k - 1) - {\varvec{K}}_{j} \left( k \right){\varvec{S}}_{j} \left( k \right){\varvec{K}}_{j} (k)^{T}$$
(18)
  1. 3.

    Update of model probability

The likelihood function of model \(j\) is:

$$\Lambda_{j} \left( k \right) = \frac{1}{{(2\pi )^{n/2} |{\varvec{S}}_{j} \left( k \right)|^{1/2} }} \cdot exp \left\{ { - \frac{1}{2}{\varvec{v}}_{j} (k)^{T} {\varvec{S}}_{j} (k)^{ - 1} {\varvec{v}}_{j} \left( k \right)} \right\}$$
(19)

The model probability of model \(j\) is updated to:

$$\left\{ {\begin{array}{*{20}l} {\mu_{j} \left( k \right) = \varLambda_{j} \left( k \right)\overline{c}_{j} /c} \hfill \\ {c = \mathop \sum \nolimits_{j = 1}^{r} \varLambda_{j} \left( k \right)\overline{c}_{j} } \hfill \\ \end{array} } \right.$$
(20)
  1. 4.

    Output interaction

According to the weighted sum of the estimated results of each model based on the model probability, the state estimation value of IMM and its covariance matrix can be obtained:

$$\widehat{\user2{X}}(k|k) = \mathop \sum \nolimits_{j = 1}^{r} \widehat{\user2{X}}_{j} (k|k)\mu_{j} \left( k \right)$$
(21)
$${\varvec{P}}\left( {k{|}k} \right) = \mathop \sum \nolimits_{j = 1}^{r} \mu_{j} \left( k \right)\{ {\varvec{P}}_{j} (k|k) + [\widehat{\user2{X}}_{j} (k|k) - \widehat{\user2{X}}(k|k)\left] \cdot \right[\widehat{\user2{X}}_{j} (k|k) - \widehat{\user2{X}}(k|k)]^{T} \}$$
(22)

3 An Improved Robust Interacting Multiple Model Algorithm

In order to solve the problems of misjudgment of gross error and unreasonable model probability caused by model mismatch in RIMM, this paper proposes an improved robust interacting multiple model algorithm. This algorithm improves the navigation accuracy of long underwater acoustic baseline through interacting robust estimation and model probability correction. The flow chart of improved robust interacting multiple model algorithm is shown in Fig. 2. The detailed steps and instructions are as follows:

  1. 1.

    Input interaction

The mixed estimation \(\widehat{\user2{X}}_{Oj} (k - 1|k - 1)\) and its covariance \({\varvec{P}}_{Oj} (k - 1|k - 1)\) of model \(j\left( {j = 1,2, \cdots ,r} \right)\) are calculated from the state estimation \(\widehat{\user2{X}}_{i} (k - 1|k - 1)\) of model \(i\left( {i = 1,2, \cdots ,r} \right)\) at time \(k - 1\) and its model probability \(\mu_{i} \left( {k - 1} \right)\).

  1. 2.

    Interacting robust estimation

Using \(\widehat{\user2{X}}_{Oj} (k - 1|k - 1)\) and \({\varvec{P}}_{Oj} (k - 1|k - 1)\) as the input of the model, the standard robust estimation is carried out for each model, and the equivalent variance \(\overline{\user2{R}}\left( k \right)\) is obtained. Then the model probability is updated to obtain the model probability \(\mu^{\prime}\left( k \right)\) after standard robust estimation, and the mixed equivalent variance \(\widehat{\user2{R}}\left( k \right)\) is calculated:

$$\widehat{\user2{R}}\left( k \right) = \mathop \sum \nolimits_{j = 1}^{r} \mu_{j}^{^{\prime}} \left( k \right)\overline{\user2{R}}_{j} \left( k \right)$$
(23)

In the formula, \(\overline{\user2{R}}_{j} \left( k \right)\) represents the equivalent variance of the model \(j\), and \(\mu_{j}^{^{\prime}} \left( k \right)\) represents the model probability of the model \(j\) after standard robust estimation.

The mixed equivalent variance \(\widehat{\user2{R}}\left( k \right)\) is used to replace the variance of measurement noise to estimate the model again, and the interacting robust estimation is realized. where, the innovation covariance of model \(j\) at time \(k\) is:

$$\widehat{\user2{S}}_{j} \left( k \right) = {\varvec{H}}\left( k \right){\varvec{P}}_{j} (k|k - 1){\varvec{H}}(k)^{T} + \widehat{\user2{R}}\left( k \right)$$
(24)

Gain matrix of Kalman filter:

$$\widehat{\user2{K}}_{j} \left( k \right) = {\varvec{P}}_{j} (k|k - 1){\varvec{H}}(k)^{T} \widehat{\user2{S}}_{j} (k)^{ - 1}$$
(25)

The estimated state and its covariance matrix are:

$$\widehat{\user2{X}}_{j} (k|k) = \widehat{\user2{X}}_{j} (k|k - 1) + \widehat{\user2{K}}_{j} \left( k \right){\varvec{v}}_{j} \left( k \right)$$
(26)
$${\varvec{P}}_{j} (k|k) = {\varvec{P}}_{j} (k|k - 1) - \widehat{\user2{K}}_{j} \left( k \right)\widehat{\user2{S}}_{j} \left( k \right)\widehat{\user2{K}}_{j} (k)^{T}$$
(27)
  1. 3.

    Update of model probability

After interacting robust estimation, the model probability of model \(j\) is updated to:

$$\left\{ {\begin{array}{*{20}l} {\mu_{j}^{\prime \prime } \left( k \right) = \widehat{\varLambda }_{j} \left( k \right)\overline{c}_{j} /\widehat{c}} \hfill \\ {\widehat{\varLambda }_{j} \left( k \right) = \frac{1}{{(2\pi )^{n/2} |\widehat{\user2{S}}_{j} \left( k \right)|^{1/2} }} \cdot exp \left\{ { - \frac{1}{2}{\varvec{v}}_{j} (k)^{T} \widehat{\user2{S}}_{j} (k)^{ - 1} {\varvec{v}}_{j} \left( k \right)} \right\}} \hfill \\ {\widehat{c} = \mathop \sum \nolimits_{j = 1}^{r} \widehat{\varLambda }_{j} \left( k \right)\overline{c}_{j} } \hfill \\ \end{array} } \right.$$
(28)
  1. 4.

    Correction of model probability

The model with the largest model probability is called the main model, and other models are called the sub-model. Let model \(M\) be the main model, the main model probability can be expressed as \(\mu_{M}^{\prime \prime }\), and the threshold of model probability correction is expressed as \(\lambda\).

When \(\mu_{M}^{\prime \prime } \le \lambda\), the model probability is not corrected. When \(\mu_{M}^{\prime \prime } > \lambda\), The master model probability \(\mu_{M}^{\prime \prime }\) can be expressed as:

$$\mu_{M}^{\prime \prime } = \lambda + \varDelta \mu$$
(29)

where \(\varDelta \mu\) is the part of the main model probability \(\mu_{M}^{\prime \prime }\) exceeding the threshold \(\lambda\). In order to increase the probability of matching model, power function \(f\left( {\varDelta \mu } \right)\) is used instead of \(\varDelta \mu\), as shown in formula (31).

$$\begin{array}{*{20}c} {f\left( {\varDelta \mu } \right) = \frac{{\left( {100 \cdot \varDelta \mu } \right)^{\theta } }}{100}} & {\theta > 1} \\ \end{array}$$
(30)

where \(\theta\) is the exponent of power function, generally \(\theta > 1\). The probability of the modified main model is:

$$\mu^{\prime\prime\prime}_{M} \left( k \right) = \lambda + f\left( {\varDelta \mu } \right)$$
(31)

The probability of the sub-model decreases proportionally. The calculation method is:

$$\mu^{\prime\prime\prime}_{j} \left( k \right) = \mu_{j}^{\prime \prime } \left( k \right) - \left[ {f\left( {\varDelta \mu } \right) - \varDelta \mu } \right] \cdot \frac{{\mu_{j}^{\prime \prime } \left( k \right)}}{{1 - \mu_{M}^{\prime \prime } \left( k \right)}}$$
(32)

In the formula, \(j \ne M\).

  1. 5.

    Output interaction

The revised model probability \(\mu^{\prime\prime\prime}_{j} \left( k \right)\) is taken as the weight of output interaction, and the estimated results of each model are weighted and summed to obtain the final state estimation value and its covariance matrix:

$$\widehat{\user2{X}}(k|k) = \mathop \sum \nolimits_{j = 1}^{r} \widehat{\user2{X}}_{j} (k|k)\mu^{\prime\prime\prime}_{j} \left( k \right)$$
(33)
$${\varvec{P}}(k|k) = \mathop \sum \nolimits_{j = 1}^{r} \mu^{\prime\prime\prime}_{j} \left( k \right)\{ {\varvec{P}}_{j} (k|k) + [\widehat{\user2{X}}_{j} (k|k) - \widehat{\user2{X}}(k|k)\left] \cdot \right[\widehat{\user2{X}}_{j} (k|k) - \widehat{\user2{X}}(k|k)]^{T} \}$$
(34)

Among them, the modified model probability \(\mu^{\prime\prime\prime}_{j} \left( k \right)\) is only used for the output interaction at \(k\) time, and the input interaction at \(k + 1\) time still uses the modified model probability \(\mu_{j}^{\prime \prime } \left( k \right)\).

Fig. 2.
figure 2

Improved robust interacting multiple model method

Full size image

4 Simulation and Real Experimental Analysis

In order to verify the effectiveness and feasibility of the improved robust interacting multiple filtering algorithm, two simulation experiments and a measured experiment are carried out in this paper. The experimental results of standard KF, IMM, RIMM and improved RIMM are compared and analyzed from the aspects of navigation accuracy.

4.1 Simulation Analysis

Experimental Parameters

Simulation Experiment 1: The underwater vehicle is simulated to navigate along the S-shaped trajectory at a speed of 5 m/s with a sampling interval of 2 s, and 1312 epochs are simulated. Four transponders are arranged on the seabed, and the track and position of the underwater vehicle are shown in Fig. 3(a) and (b). The sound velocity profile uses the Munk classical sound velocity profile, and the thickness is 0.5 m, as shown in Fig. 3(c). the horizontal positioning error of the transponder is 0.1 m, and the vertical positioning error is 0.2 m; the random error of acoustic delay observation is 0.1 ms. The motion model of standard KF is a constant velocity model. The model sets of IMM, RIMM and improved RIMM include a constant velocity model, a collaborative turning model with a turning rate of 0.02 rad/s and a collaborative turning model with a turning rate of −0.02 rad/s. The threshold of model probability correction is \(\lambda = 40\%\), and the exponent of power function is \(\theta = 1.5\).

Simulation Experiment 2: On the basis of Simulation Experiment 1, the random gross error is added to the acoustic delay observation, which is 10 times of the random error and the probability is 2%.

Fig. 3.
figure 3

Diagram of simulated track and transponder position and Munk sound velocity profile

Full size image

Comparison and Analysis of Navigation Accuracy

In order to compare the navigation accuracy between the proposed method and the traditional method, two sets of simulation experiments are simulated. The experimental results are shown in Fig. 4, 5.

It can be seen from Fig. 4, 5 that the navigation error of standard KF is large at the turning point and when there is gross error; IMM effectively reduces the navigation error caused by the changeable motion state, but there is still a large error at the gross error. RIMM reduces the influence of gross error on navigation results on the basis of IMM, but the navigation accuracy is reduced without gross error. The improved RIMM not only has no obvious navigation error at the turning point and when there is gross error, but also reduces the influence of gross error judgment and unreasonable model probability on navigation accuracy, with the highest navigation accuracy.

In order to further evaluate the performance of the proposed method, 100 simulations were conducted for two groups of simulation experiments, and the experimental results are shown in Table 1.

It can be seen from Table 1 that the navigation error of KF is large, which is greater than 1 m in both experiments. Compared with KF, the navigation accuracy of IMM in the two groups of experiments increased by 65.06% and 51.37%, respectively. The position error of RIMM with gross error is 0.4459 m, which is 17.61% lower than that of IMM, but the position error without gross error is 0.3745 m, which is greater than 0.3652 m of IMM. Compared with the standard KF, IMM and RIMM, the navigation accuracy of the improved RIMM is improved by 68.70%, 10.41% and 12.63% respectively without gross error. With gross errors, the navigation accuracy of the improved RIMM is increased by 64.18%, 26.35% and 10.61% respectively. The improved RIMM navigation accuracy is significantly better than the other three algorithms.

Fig. 4.
figure 4

Navigation error of different algorithms without gross error

Full size image
Fig. 5.
figure 5

Navigation error of different algorithms with gross error

Full size image
Table 1. Average root mean square error of different algorithms in 100 simulation experiments
Full size table

4.2 Real Experiment Analysis

The observation data of the 10th line in the observation task of the KAMN station published by the Ministry of Marine Information of Japan in June 2019 were verified. In this experiment, six transponders are installed on the seabed. The average sampling interval of the transducer is 12 s, and the number of sampling epochs is 470. The trajectory and the position of the seabed transponder are shown in Fig. 6(a) and (b), and the sound velocity profile is shown in Fig. 6(c). The experimental results are shown in Fig. 7 and Table 2.

It can be seen from Fig. 7 and Table 2 that the navigation error of the standard KF is 5.5255 m, and there is a large navigation error at the turning point. The navigation errors of IMM and RIMM are 4.4214 m and 4.4299 m, respectively. Compared with the standard KF, IMM and RIMM reduce the influence of the change of motion state on the navigation results. The navigation error of the improved RIMM is 3.7687 m. Compared with the standard KF, IMM and RIMM, the navigation accuracy of the improved RIMM is increased by 31.79%, 14.76% and 14.93%, respectively, indicating that the proposed method significantly improves the navigation accuracy.

Fig. 6.
figure 6

Track and transponder position and sound velocity profile in field test

Full size image
Fig. 7.
figure 7

Errors of different algorithms in field test

Full size image
Table 2. Root mean square error statistics of different algorithms in field test
Full size table

5 Conclusion

Aiming at the problem that the motion state of underwater vehicle is changeable and there are abnormal observations, this paper proposes an improved robust interacting multiple model filtering algorithm. Through two simulation experiments and one measurement experiment, the traditional KF, IMM, RIMM method and the method in this paper are compared and analyzed, and the following conclusions are drawn:

  1. 1.

    When the motion state of underwater vehicle changes, KF has the problem of model mismatch; IMM reduces the navigation error caused by changing motion state, but does not have the ability to resist error. RIMM filtering algorithm can effectively reduce the influence of model mismatch and gross error on navigation accuracy, but there are problems of gross error judgment and unreasonable model probability, which leads to the decrease of navigation accuracy without gross error.

  2. 2.

    The improved RIMM can not only reduce the influence of model mismatch and gross error on navigation accuracy, but also solve the gross error by interacting robust estimation, and improve the model probability of matching model by model probability correction, so as to improve the navigation accuracy and stability of underwater vehicles.