Open AccessArticle

Adaptive Federated IMM Filter for AUV Integrated Navigation Systems

Weiwei Lyu

^1,2,

Xianghong Cheng

^1,2,*

and

Jinling Wang

School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China

Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China

School of Civil and Environmental Engineering, University of New South Wales, Sydney, NSW 2052, Australia

Author to whom correspondence should be addressed.

Sensors 2020, 20(23), 6806; https://doi.org/10.3390/s20236806

Submission received: 21 September 2020 / Revised: 22 November 2020 / Accepted: 24 November 2020 / Published: 28 November 2020

(This article belongs to the Special Issue Inertial Sensors and Systems in 2020)

Download

Browse Figures

Versions Notes

Abstract

High accuracy and reliable navigation in the underwater environment is very critical for the operations of autonomous underwater vehicles (AUVs). This paper proposes an adaptive federated interacting multiple model (IMM) filter, which combines adaptive federated filter and IMM algorithm for AUV in complex underwater environments. Based on the performance of each local system, the information sharing coefficient of the adaptive federated IMM filter is adaptively determined. Meanwhile, the adaptive federated IMM filter designs different models for each local system. When the external disturbances change, the model of each local system can switch in real-time. Furthermore, an AUV integrated navigation system model is constructed, which includes the dynamic model of the system error and the measurement models of strapdown inertial navigation system/Doppler velocity log (SINS/DVL) and SINS/terrain aided navigation (SINS/TAN). The integrated navigation experiments demonstrate that the proposed filter can dramatically improve the accuracy and reliability of the integrated navigation system. Additionally, it has obvious advantages compared with the federated Kalman filter and the adaptive federated Kalman filter.

Keywords:

AUV; federated Kalman filter; integrated navigation; information sharing coefficient; interacting multiple model (IMM)

1. Introduction

Autonomous underwater vehicle (AUV) is an efficient underwater working platform that has been widely used for various underwater tasks in the areas of oil and gas industry, ocean mapping, archaeological exploration, military reconnaissance missions, search and rescue operations, etc. [1,2,3,4,5]. Over the past few decades, AUV has been developed rapidly, due to its great value of application [4,5]. Accurate navigation and positioning is not only a prerequisite for AUV to perform underwater operations, but also a technical guarantee for its safe return [3,6]. Navigation and positioning is one of the benchmark technologies to evaluate the level of development and the maturity of engineering application of AUV [1,3,4]. However, because of the complexity of ocean environment, how to make AUV reach the operation site accurately and return safely is still a challenging issue [4,5].

Currently, most of AUVs adopt a strapdown inertial navigation system (SINS) as the reference navigation system [3,7]. SINS is an independent navigation system that is able to provide comprehensive navigation information, including the velocity, position, and attitude [8,9]. However, because of the errors of inertial sensors, the navigation solution of SINS diverges over time [10,11]. Hence, SINS is often aided by other navigation systems, such as Doppler velocity log (DVL) [2,7,12], magnetometer [7], depth sensor [11], terrain aided navigation (TAN) [13,14,15,16], acoustic long baseline (LBL) [17], vision navigation [18], and so on [6]. In [11], an underwater navigation system composed of a high-rate SINS and low-rate aided sensors was designed, and the aided sensors consisted of a DVL, an inclinometer, a depth sensor, and a global positioning system (GPS). In [7], the Technion autonomous underwater vehicle (TAUV) was discussed, and its navigation system was composed of a SINS integrated by a DVL, a pressure sensor, and a magnetometer. Due to the lack of underwater maps for terrain based navigation, Nygren et al. [15] proposed a new method that uses a multibeam sonar and a linear Kalman filter to accommodate low sampling frequency. The above studies utilize various navigation sensors to aid SINS, and the navigation performance of AUVs has been effectively improved.

The decentralized filtering methods in a multi-sensor information fusion system have attracted the increasing attention of researchers [19]. Compared with the centralized filtering methods, the decentralized filtering methods dramatically reduce the computational effort and increase the fault-tolerant capability of the integrated navigation system [20,21]. Among the decentralized filtering methods, the decentralized federated filter created by Carlson [20,21,22,23] is most well known.

According to the principle of the decentralized federated filter, the local filters are designed to be suboptimal filters that provide the global optimal estimation [20,24]. In [21], Carlson discussed a federated filter that was applied to integrated and fault-tolerant navigation systems. The superiority of fault tolerance, estimation accuracy, and computation speed was demonstrated by the numerical simulation results and real-time implementations. However, the information distribution principle in the federated filter was designed to be at a fixed ratio, which means that each local system has a fixed information coefficient [19,25,26]. However, in practical applications, the performance and estimated accuracy of the local system is continually changing with the complex navigation environment [3,7,27,28]. In order to increase the performance of the federated filter, Shen et al. [25] proposed a new adaptive federated Kalman filter with time-varying information sharing coefficients on the basis of observability analysis for unmanned ground vehicles (UGV) integrated navigation. Xiong et al. [19] designed a novel dynamic vector-form information sharing method based on the analysis of the error covariance matrix and the observability matrix for the federated filter in a highly dynamic environment. Wang et al. [29] proposed an adaptive information sharing factor federated filter (AISFF) which can adaptively adjust the information sharing factor to improve the reliability of autonomous navigation for Unmanned Surface Vehicles (USVs). The above research has optimized the information distribution principle of federated filter, and the navigation accuracy has been effectively improved. However, when underwater environment in concerned, ocean currents, turbulence, changes in salinity and temperature, and other underwater phenomena can influence the models of AUV integrated navigation systems [1,4,30]. In fact, as the errors and disturbances are usually time-varying and change with the underwater environment, it is difficult to model them accurately [5,7,31], which will lead to greater inaccuracies of AUV integrated navigation systems. Therefore, how to achieve accurate and reliable navigation capability in the underwater environment is a challenging problem to be settled.

In this paper, a new adaptive federated IMM filter is designed for AUV integrated navigation systems. The information sharing coefficient of the adaptive federated IMM filter is dynamically adjusted according to the performance of each local system. Aiming to enhance the accuracy and reliability of AUV operations in a complex underwater environment, this paper studies the IMM algorithm and combines it with the adaptive federated filter. Then, an AUV integrated navigation system model that includes the system error dynamics model, SINS/DVL and SINS/TAN measurement models is constructed. The vehicle integrated navigation experiments demonstrate the high accuracy and reliability of the proposed adaptive federated IMM filtering method.

The structure of this paper is organized as follows: the federated Kalman filter is presented in Section 2. In Section 3, the adaptive federated Kalman filter is designed, then the adaptive federated IMM filter is proposed and analyzed in detail. In Section 4, the AUV integrated navigation system model is constructed in detail. Thirty groups of vehicle integrated navigation experiments are conducted to demonstrate the validity of the proposed method in Section 5. Finally, the conclusions are drawn in Section 6.

2. Federated Kalman Filter

The state equation of a linear discrete-time system i can be presented as follows [22,32]:

X_{k}^{i} = Φ_{k, k - 1}^{i} X_{k - 1}^{i} + Γ_{k - 1}^{i} W_{k - 1}^{i}

(1)

Z_{k}^{i} = H_{k}^{i} X_{k}^{i} + V_{k}^{i}

(2)

where

Φ_{k, k - 1}^{i}

is the (n × n) state transition matrix;

Γ_{k - 1}^{i}

is the (n × r) system noise matrix;

W_{k - 1}^{i}

is the (r × r) system noise matrix;

X_{k}^{i}

is the (n × 1) state estimate;

H_{k}^{i}

is the (m × n) measurement matrix;

V_{k}^{i}

is the measurement noise matrix;

Z_{k}^{i}

is the (m × 1) measurement value.

In Equation (1), both

W_{k - 1}^{i}

and

V_{k}^{i}

are assumed as the zero-mean Gaussian white noise, and their covariances are:

\begin{array}{l} E [W_{j}^{i} W_{k}^{i}^{T}] = σ_{j k} Q_{k - 1}^{i} \\ E [V_{j}^{i} V_{k}^{i}^{T}] = σ_{j k} R_{k}^{i} \\ E [W_{j}^{i} V_{k}^{i}^{T}] = 0 \end{array}

(3)

In Equation (3),

Q_{k - 1}^{i} \geq 0

R_{k}^{i} > 0

σ_{j k}

is the Kronecker function, and

σ_{j k} = {\begin{cases} 0 (k \neq j) \\ 1 (k = j) \end{cases}

The federated Kalman filter contains a composite master filter and N independent local filters. The schematic diagram of the federated Kalman filter is presented in Figure 1. In summary, the implementation steps of the federated Kalman filter can be presented as follows:

(1): Information sharing:

Define the symbols with the superscript “g” as the parameters of the global filter. The system information

Q_{k}^{g}

P_{k}^{g}

and

{\hat{X}}_{k}^{g}

are allocated according to the following information allocation principles [21,22]:

{\begin{cases} Q_{k}^{i} = β_{i}^{- 1} Q_{k}^{g} \\ P_{k}^{i} = β_{i}^{- 1} P_{k}^{g} \\ {\hat{X}}_{k}^{i} = {\hat{X}}_{k}^{g}, i = 1 \dots N, m \end{cases}

(4)

where β_i represents the information sharing coefficient,

β_{i} > 0

. Meanwhile, it satisfies the following information conservation principle:

\sum_{i = 1}^{N} β_{i} + β_{m} = 1

(5)

(2): Time updating:

The N independent local filters and the master filter and conduct the time updating of the information separately according to the following equations:

{\begin{cases} {\hat{X}}_{k / k - 1}^{i} = Φ_{k / k - 1}^{i} {\hat{X}}_{k - 1}^{i} \\ P_{k / k - 1}^{i} = Φ_{k / k - 1}^{i} P_{k - 1}^{i} Φ_{k / k - 1}^{i}^{T} + Γ_{k - 1}^{i} Q_{k - 1}^{i} Γ_{k - 1}^{i}^{T}, i = 1 \dots N, m \end{cases}

(6)

(3): Measurement updating:

Because the master filter only plays the role of information fusion and it has no measurement information, the master filter has no process of measurement updating. The N independent local filters conduct the measurement updating of the information separately according to the following equations [22,23]:

{\begin{cases} {(P_{k}^{i})}^{- 1} = {(P_{k / k - 1}^{i})}^{- 1} + {(H_{k}^{i})}^{T} {(R_{k}^{i})}^{- 1} H_{k}^{i} \\ {(P_{k}^{i})}^{- 1} {\hat{X}}_{k}^{i} = {(P_{k / k - 1}^{i})}^{- 1} {\hat{X}}_{k / k - 1}^{i} + {(H_{k}^{i})}^{T} {(R_{k}^{i})}^{- 1} Z_{k}^{i}, i = 1, 2, \dots, N \end{cases}

(7)

(4): Information fusion:

The local estimation state of each local filter is fused based on the following two equations, and the global optimal estimation can be obtained:

P_{k}^{g} = {[{(P_{k}^{1})}^{- 1} + {(P_{k}^{2})}^{- 1} + \dots + {(P_{k}^{N})}^{- 1} + {(P_{k}^{m})}^{- 1}]}^{- 1}

(8)

\begin{array}{l} {\hat{X}}_{k}^{g} = & P_{k}^{g} [{(P_{k}^{1})}^{- 1} {\hat{X}}_{k}^{1} + {(P_{k}^{2})}^{- 1} {\hat{X}}_{k}^{2} + \dots + {(P_{k}^{N})}^{- 1} {\hat{X}}_{k}^{N} \\ + {(P_{k}^{m})}^{- 1} {\hat{X}}_{k}^{m}] \end{array}

(9)

3. Adaptive Federated IMM Filter

3.1. Adaptive Federated Kalman Filter

When given a matrix

P_{i} \in C^{m \times n}

, there are unitary matrix

Υ_{i} = [λ_{i, 1} λ_{i, 2} \dots λ_{i, m}]

and unitary matrix

Λ_{i} = [μ_{i, 1} μ_{i, 2} \dots μ_{i, n}]

that satisfy the following relationship:

Υ_{i}^{T} P_{i} Λ_{i} = [\begin{matrix} ξ_{i} & 0_{c, n - c} \\ 0_{m - c, c} & 0_{m - c, n - c} \end{matrix}]

(10)

where

ξ_{i} = d i a g (ξ_{i, 1}, ξ_{i, 2}, \dots, ξ_{i, r})

ξ_{i, 1} > ξ_{i, 2} > \dots > ξ_{i, r} > 0

c = r a n k (P_{i})

. The Equation (10) is the singular value decomposition of the matrix

P_{i}

, and

ξ_{i, 1}, ξ_{i, 2}, \dots, ξ_{i, r}

are the singular values of the matrix

P_{i}

Define

P_{i} ({(p_{λ μ})}_{n \times n})

as the covariance matrix of local filter i, and

p_{λ μ}

represents the cross-covariance of local filter i between state λ and state μ. Then the singular value

ξ_{i}

of the matrix

P_{i}

contains both the information of auto-covariance and the information of cross-covariance in each system’s state estimation. Therefore, the covariance matrix of local filter i contains the information of the estimation error, and it reflects the filtering performance of local filter i. Based on the above analysis, this paper determines the information sharing coefficient dynamically from the global filter to the local filter. In local filter i,

ξ_{i} (k)

is defined as the sum of singular values of the covariance matrix

P_{i} (k)

at filtering step k, and it can be expressed as follows:

ξ_{i} (k) = \sum_{l = 1}^{n} ξ_{i, l} (k)

(11)

Firstly, the information sharing coefficient

β_{m}

of the master filter is selected, then the information sharing coefficients of local filters can be obtained according to the following equation:

β_{i} (k) = \frac{1 / ξ_{i} (k)}{1 / ξ_{1} (k) + 1 / ξ_{2} (k) + \dots + 1 / ξ_{N} (k)} (1 - β_{m}), i = 1, 2, \dots, N

(12)

where

β_{i} (k)

represents the information sharing coefficient of local filter i at filtering step k.

The information sharing coefficients of the adaptive federated Kalman filter satisfy the following relationship:

0 \leq β_{i} (k) \leq 1, \sum_{i = 1}^{N} β_{i} (k) + β_{m} = 1

(13)

Consequently, the process of system information distribution can be expressed as:

{\begin{cases} Q_{k}^{i} = β_{i} {(k)}^{- 1} Q_{k}^{g} \\ P_{k}^{i} = β_{i} {(k)}^{- 1} P_{k}^{g} \\ {\hat{X}}_{k}^{i} = {\hat{X}}_{k}^{g}, i = 1 \dots N, m \end{cases}

(14)

3.2. Adaptive Federated IMM Filter

When the system has discrete uncertainties together with continuous uncertainties in the dynamic or measurement model, the IMM algorithm is a very effective method [32,33,34,35,36]. The IMM algorithm has shown superior performance with a low computational burden in a variety of applications, such as target tracking [37,38,39,40], mobile node localization [41], and motion planning [42]. However, there are few studies about the application of the IMM algorithm to the underwater navigation system. There are also few studies about the combination of the IMM algorithm and the federated filter. Focused on the above problems, this paper designs a novel adaptive federated IMM filter that combines the adaptive federated filter and the IMM algorithm to enhance the accuracy and reliability of the AUV operations in complex underwater environments. In the proposed method, each local system includes different models, and when the underwater environment changes the model for each local system can switch in real-time. Therefore, the proposed method can use the most accurate mixed model to describe the current state of the local system.

Assume there are S states of motion in the system, then there should be S motion models accordingly. The state equation of model q can be described as follows:

X_{q} (k) = Φ_{q} (k | k - 1) X_{q} (k - 1) + Γ_{q} (k - 1) W_{q} (k - 1)

(15)

In addition, the measurement equation of model q can be presented as follows:

Z_{q} (k) = H_{q} (k) X_{q} (k) + V_{q} (k)

(16)

where

W_{q} (k - 1)

is the uncorrelated zero-mean Gaussian white noise, and its conversance matrix is

Q_{q} (k - 1)

. The Markov probability transfer matrix determines the transfer between each of the model, and the element

e_{p q}

represents the probability of the model p transferring to the model q. The probability transfer matrix is represented as follows:

E = [\begin{matrix} e_{11} & \dots & e_{1 s} \\ \dots & \dots & \dots \\ e_{s 1} & \dots & e_{s s} \end{matrix}]

(17)

The IMM algorithm runs recursively, and each recursion is mainly divided into the following four steps:

(1): Interactive input (model q):

Based on the estimation state

{\hat{X}}_{p} (k - 1)

and the model probability

μ_{p} (k - 1)

of each filter at the filtering step k − 1, the mixed estimation state

{\hat{X}}_{0 q} (k - 1)

, and the covariance matrix

P_{0 q} (k - 1)

is obtained, and the mixed estimation results are regarded as the initial states of the current recursion. The specific parameters are calculated as follows:

The prediction probability of the model q can be represented as

{\bar{c}}_{q} = \sum_{p = 1}^{s} e_{p q} μ_{p} (k - 1)

(18)

The mixed probability of the model p transfer to the model q is

μ_{p q} (k - 1) = e_{p q} μ_{p} (k - 1) / {\bar{c}}_{q}

(19)

The mixed estimation state of the model q is represented as

{\hat{X}}_{0 q} (k - 1) = \sum_{p = 1}^{s} {\hat{X}}_{p} (k - 1) μ_{p q} (k - 1)

(20)

Then the mixed covariance matrix estimation of the model q is represented as

\begin{array}{l} P_{0 q} (k - 1) = & \sum_{p = 1}^{s} μ_{p q} (k - 1) {P_{p} {(k - 1)}^{^{^{}}} + [{\hat{X}}_{p} (k - 1) - {\hat{X}}_{0 q} (k - 1)] \\ \cdot {[{\hat{X}}_{p} (k - 1) - {\hat{X}}_{0 q} (k - 1)]}^{T}} \end{array}

(21)

(2): Kalman filtering (model q):

Take

{\hat{X}}_{0 q} (k - 1)

P_{0 q} (k - 1)

and

Z (k)

as the input of the Kalman filter, then the estimation state

{\hat{X}}_{q} (k)

and the estimation covariance matrix

P_{q} (k)

can be updated.

The prediction state is

{\hat{X}}_{q} (k | k - 1) = Φ_{q} (k | k - 1) {\hat{X}}_{0 q} (k - 1)

(22)

The prediction covariance matrix is

P_{q} (k | k - 1) = Φ_{q} (k | k - 1) P_{0 q} (k - 1) Φ_{q} {(k | k - 1)}^{T} + Γ_{q} (k) Q_{q} (k) Γ_{q} {(k)}^{T}

(23)

The gain of the Kalman filter is

K_{q} (k) = P_{q} (k | k - 1) H {(k)}^{T} {[H (k) P_{q} (k | k - 1) H {(k)}^{T} + R (k)]}^{- 1}

(24)

The estimation state

{\hat{X}}_{q} (k)

{\hat{X}}_{q} (k) = {\hat{X}}_{q} (k | k - 1) + K_{q} (k) [Z (k) - H (k) X_{q} (k | k - 1)]

(25)

Then the estimation covariance matrix

P_{q} (k)

P_{q} (k) = [I - K_{q} (k) H (k)] P_{q} (k | k - 1)

(26)

(3): Model probability updating (model q):

The likelihood function is applied to update the model probability

μ_{q} (k)

, the likelihood function of the model q can be presented as

ϑ_{q} (k) = \frac{1}{{(2 π)}^{n / 2} {| T_{q} (k) |}^{1 / 2}} \exp {- \frac{1}{2} ρ_{q} {(k)}^{T} T_{q}^{- 1} (k) ρ_{q} (k)}

(27)

where

ρ_{q} (k) = Z (k) - H (k) {\hat{X}}_{q} (k | k - 1)

(28)

T_{q} (k) = H (k) P_{q} (k | k - 1) H {(k)}^{T} + R (k)

(29)

Then, the probability of the model q is

μ_{q} (k) = ϑ_{q} (k) {\bar{c}}_{q} / c

(30)

where c is a normalization constant, and

c = \sum_{q = 1}^{s} ϑ_{q} (k) {\bar{c}}_{q}

(4): Interactive output:

Based on the probability of each model, the estimation result of each filter can be combined, then the mixed estimation state

\hat{X} (k)

and the mixed covariance matrix estimation

P (k)

can be calculated.

Consequently, the mixed estimation state can be presented as

\hat{X} (k) = \sum_{q = 1}^{s} {\hat{X}}_{q} (k) μ_{q} (k)

(31)

In addition, the mixed estimation covariance matrix can be presented as

P (k) = \sum_{q = 1}^{s} μ_{q} (k) {P_{q} (k | k - 1) + [{\hat{X}}_{q} (k) - \hat{X} (k)] {[{\hat{X}}_{q} (k) - \hat{X} (k)]}^{T}}

(32)

In general, the whole output results of the IMM algorithm are the combined values of all filters, and the weight factor of each filter is the model probability that represents the accuracy of the current motion state of the system. The schematic diagram of the proposed adaptive federated IMM filter is presented in Figure 2.

4. AUV Integrated Navigation System Model

4.1. System Error Dynamics Model

This paper defines “east-north-up (ENU)” as the navigation frame, and “right-forward-up” as the body frame. The SINS’ state equation in a continuous-time system can be constructed as follows:

\dot{X} (t) = F (t) X (t) + G (t) W (t)

(33)

where

X (t)

is the (n × 1) state estimation,

\dot{X} (t)

is the (n × 1) one step predicted state,

F (t)

is the (n × n) state transition matrix,

G (t)

is the system noise matrix, and

W (t)

is the zero-mean Gaussian white noise.

In the model of the AUV integrated navigation system, fifteen dimensions’ states are selected to establish the equation of states. The system’s states can be expressed as follows:

X (t) = {[ϕ_{E}, ϕ_{N}, ϕ_{U}, δ V_{E}, δ V_{N}, δ V_{U}, δ L, δ λ, δ h, ε_{x}, ε_{y}, ε_{z}, \nabla_{x}, \nabla_{y}, \nabla_{z}]}^{T}

(34)

where φ_E, φ_N, and φ_U are the misalignment angles; δV_E, δV_N and δV_U are the velocity errors; δL, δλ and δh are the latitude error, longitude error, and height error, respectively; ε_x, ε_y, and ε_z are the gyro drifts; and ∇_x, ∇_y and ∇_z are the accelerometer biases.

The instruction angular velocity and gyro bias influence the attitude angle error of the SINS. The attitude error equations in ENU axes are as follows:

\begin{array}{l} {\dot{ϕ}}_{E} = & ϕ_{N} (ω_{i e} \sin L + \frac{V_{E}}{R_{N} + h} \tan L) - ϕ_{U} (ω_{i e} \cos L + \frac{V_{E}}{R_{N} + h}) - \frac{δ V_{N}}{R_{M} + h} \\ + δ h \frac{V_{N}}{{(R_{M} + h)}^{2}} - ε_{E} \end{array}

(35)

\begin{array}{l} {\dot{ϕ}}_{N} = & - ϕ_{E} (ω_{i e} \sin L + \frac{V_{E}}{R_{N} + h} \tan L) - ϕ_{U} \frac{V_{N}}{R_{M} + h} - δ L ω_{i e} \sin L + \frac{δ V_{E}}{R_{N} + h} \\ - δ h \frac{V_{E}}{{(R_{N} + h)}^{2}} - ε_{N} \end{array}

(36)

\begin{array}{l} {\dot{ϕ}}_{U} & = ϕ_{E} (ω_{i e} \cos L + \frac{V_{E}}{R_{N} + h}) + ϕ_{N} \frac{V_{N}}{R_{M} + h} + δ L (ω_{i e} \cos L + \frac{V_{E}}{R_{N} + h} \sec^{2} L) \\ + \frac{δ V_{E}}{R_{N} + h} \tan L \begin{matrix} - δ h \frac{V_{E} \tan L}{{(R_{N} + h)}^{2}} \end{matrix} - ε_{U} \end{array}

(37)

where L is the latitude, R_M is the radius of curvature in meridian, and R_N is the radius of curvature in prime vertical.

The output of inertial components is processed by the SINS to obtain the navigation data. The analytical relationship between the accelerometer output and the carrier’s velocity can be described by the velocity error equation. The velocity error equations in ENU axes are as follows:

\begin{array}{l} δ {\dot{V}}_{E} = & ϕ_{U} f_{N} - ϕ_{N} f_{U} + δ V_{N} (2 ω_{i e} \sin L + \frac{V_{E} \tan L}{R_{N} + h}) - δ V_{U} (2 ω_{i e} \cos L + \frac{V_{E}}{R_{N} + h}) \\ + δ V_{E} \frac{V_{N} \tan L - V_{U}}{R_{N} + h} + δ L [2 ω_{i e} (V_{U} \sin L + V_{N} \cos L) + \frac{V_{N} V_{E}}{R_{N} + h} \sec^{2} L] \\ + δ h \frac{V_{U} V_{E} - V_{N} V_{E} \tan L}{{(R_{N} + h)}^{2}} + \nabla_{E} \end{array}

(38)

\begin{array}{l} δ {\dot{V}}_{N} = & ϕ_{E} f_{U} - ϕ_{U} f_{E} - δ V_{N} \frac{V_{U}}{R_{M} + h} - δ V_{U} \frac{V_{N}}{R_{M} + h} - δ V_{E} \cdot 2 (ω_{i e} \sin L + \frac{V_{E} \tan L}{R_{N} + h}) \\ - δ L (2 V_{E} ω_{i e} \cos L + \frac{V_{E}^{2}}{R_{N} + h} \sec^{2} L) + δ h [\frac{V_{N} V_{U}}{{(R_{M} + h)}^{2}} + \frac{V_{E}^{2} \tan L}{{(R_{N} + h)}^{2}}] + \nabla_{N} \end{array}

(39)

\begin{array}{l} δ {\dot{V}}_{U} = & - ϕ_{E} f_{N} + ϕ_{N} f_{E} + δ V_{N} \frac{2 V_{N}}{R_{M} + h} + δ V_{E} \cdot 2 (ω_{i e} \cos L + \frac{V_{E}}{R_{N} + h}) \\ - δ L \cdot 2 V_{E} ω_{i e} \sin L - δ h [\frac{V_{N}^{2}}{{(R_{M} + h)}^{2}} + \frac{V_{E}^{2}}{{(R_{N} + h)}^{2}}] + \nabla_{U} \end{array}

(40)

The position error equations in ENU axes are as follows:

δ \dot{L} = \frac{δ V_{N}}{R_{M} + h} - δ h \frac{V_{N}}{{(R_{M} + h)}^{2}}

(41)

δ \dot{λ} = \frac{δ V_{E}}{R_{N} + h} \sec L + δ L \frac{V_{E}}{R_{N} + h} \tan L \sec L - δ h \frac{V_{E} \sec L}{{(R_{N} + h)}^{2}}

(42)

δ \dot{h} = δ V_{U}

(43)

4.2. System Measurement Model

The measurement equation of the AUV integrated navigation system can be expressed as follows:

Z (t) = H (t) X (t) + V (t)

(44)

In the AUV integrated navigation system, SINS is chosen as the basic navigation system, and the aided navigation sensor can be chosen according to the actual situation. In this paper, DVL and TAN are chosen as the aided navigation sensors. Therefore, there will be two integrated system measurement equations that are discussed respectively as follows:

(1): SINS/DVL measurement equation

The DVL provides velocity information for the AUV. As shown in Equation (45), the measurement information of the SINS/DVL measurement equation consists of the difference between the east velocity

V_{S E}

, north velocity

V_{S N}

and up velocity

V_{S U}

exported by the SINS and the east velocity

V_{D E}

, north velocity

V_{D N}

and up velocity

V_{D U}

exported by the DVL.

\begin{array}{l} Z_{S I N S / D V L} (t) & = [\begin{array}{l} V_{S E} - V_{D E} \\ V_{S N} - V_{D N} \\ V_{S U} - V_{D U} \end{array}] = [\begin{array}{l} δ V_{S E} - δ V_{D E} + ζ_{E} \\ δ V_{S N} - δ V_{D N} + ζ_{N} \\ δ V_{S U} - δ V_{S U} + ζ_{U} \end{array}] \\ = [\begin{matrix} 0_{1 \times 3} & 1 & 0 & 0 & 0_{1 \times 9} \\ 0_{1 \times 3} & 0 & 1 & 0 & 0_{1 \times 9} \\ 0_{1 \times 3} & 0 & 0 & 1 & 0_{1 \times 9} \end{matrix}] X (t) + V_{S I N S / D V L} \end{array}

(45)

where

δ V_{S E}

δ V_{S N}

δ V_{S U}

are the east velocity error, north velocity error and up velocity error of the SINS, respectively;

δ V_{D E}

δ V_{D N}

δ V_{S U}

are the east velocity error, north velocity error and up velocity error of the DVL, respectively;

ζ_{E}

ζ_{N}

ζ_{U}

are the measurement noises of the SINS/DVL integrated navigation system, they are the independent zero-mean Gaussian white-noise sequences.

(2): SINS/TAN measurement equation

The TAN provides position information for the AUV. As shown in Equation (46), the measurement information of the SINS/TAN measurement equation consists of the difference between the latitude

L_{S I N S}

, longitude

λ_{S I N S}

and height

h_{S I N S}

exported by the SINS and the latitude

L_{T A N}

, longitude

λ_{T A N}

and height

h_{T A N}

exported by the TAN.

\begin{array}{l} Z_{S I N S / T A N} (t) & = [\begin{array}{l} L_{S I N S} - L_{T A N} \\ λ_{S I N S} - λ_{T A N} \\ h_{S I N S} - h_{T A N} \end{array}] = [\begin{array}{l} δ L_{S I N S} - δ L_{T A N} + η_{L} \\ δ λ_{S I N S} - δ λ_{T A N} + η_{λ} \\ δ h_{S I N S} - δ h_{T A N} + η_{h} \end{array}] \\ = [\begin{matrix} 0_{1 \times 6} & 1 & 0 & 0 & 0_{1 \times 6} \\ 0_{1 \times 6} & 0 & 1 & 0 & 0_{1 \times 6} \\ 0_{1 \times 6} & 0 & 0 & 1 & 0_{1 \times 6} \end{matrix}] X (t) + V_{S I N S / T A N} \end{array}

(46)

where

δ L_{S I N S}

δ λ_{S I N S}

δ h_{S I N S}

are the latitude error, longitude error and height error of the SINS, respectively;

δ L_{T A N}

δ λ_{T A N}

δ h_{T A N}

are the latitude error, longitude error and height error of the TAN, respectively;

η_{L}

η_{λ}

η_{h}

are the measurement noises of the SINS/TAN integrated navigation system, they are the independent zero-mean Gaussian white-noise sequences.

5. Experimental Results and Discussions

5.1. Experimental Settings

In order to verify the proposed adaptive federated IMM filtering method in the AUV integrated navigation system, an integrated navigation experiment in a real system was conducted. The vehicle experiment was carried out outdoors in Beijing, China. The approximate location was east longitude 116° and north latitude 39°. To simulate the output of DVL and the output of TAN in the AUV integrated navigation system, an odometer and a GNSS receiver were adopted. The odometer provided measurement values of velocities and the GNSS receiver provided measurement values of positions, respectively, in the integrated navigation experiment. Figure 3 shows the experimental vehicle platform that includes the SINS, GNSS receiver, odometer, navigation computer, power source, and communication lines. The SINS and the odometer used in the experiments are shown in Figure 4 and Figure 5, respectively.

When the integrated navigation experiment was conducted, the reference baseline information was provided by a high-accuracy GNSS/SINS integrated navigation system consisting of a high-accuracy SINS and a GNSS receiver. The reference information system provides precise information on attitudes, velocities, and positions during the whole experiment. The experimental integrated navigation system that is implemented with the proposed adaptive federated IMM filtering method includes a low-accuracy SINS, a GNSS receiver, and an odometer. The detailed specifications of the instruments in the experimental integrated navigation system are listed in Table 1. The update frequency of the SINS was 200 Hz, and the cycle of attitude solution was 5 ms. The data update cycles of the odometer and the GNSS receiver were all 1 s. To realistically simulate the output of the DVL and the output of the TAN in the underwater environment, noises were added to the outputs of the odometer and the GNSS receiver. To be more specific, during the time intervals of 0–150 s, 150–300 s, and 300–600 s, measurement noises of different levels and different properties were added to the output of the odometer and the output of the GNSS receiver, respectively. Furthermore, the measurement noises added to the output of the odometer and the output of the GNSS receiver were also different from each other. Moreover, the integrated navigation experiment was designed to be close to the experiment in AUV integrated navigation system.

In the adaptive federated IMM filtering method, the specific parameters of the probability transfer matrix E are set as follows:

E = [\begin{matrix} 0.9 & 0.05 & 0.05 \\ 0.05 & 0.9 & 0.05 \\ 0.05 & 0.05 & 0.9 \end{matrix}]

(47)

The variance matrix of measurement noise in the SINS/DVL integrated navigation system is R₁ = diag [0.1 m/s, 0.1 m/s]². In the proposed adaptive federated IMM filtering method, there are three models for the SINS/DVL integrated navigation system, and the variance matrices of the measurement noise for those three models are R₁, 3R₁, 8R₁, respectively.

The variance matrix of measurement noise in the SINS/TAN integrated navigation system is R₂ = diag [10 m, 10 m]². In the proposed adaptive federated IMM filtering method, there are three models for the SINS/TAN integrated navigation system, and the variance matrices of the measurement noise for those three models are R₂, 5R₂, 10R₂, respectively.

In the integrated navigation experiment, the test vehicle ran on the road, and the power source powered the SINS, the GNSS receiver, the odometer, and the high-accuracy GNSS/SINS reference baseline system. Then the real-time output data of the experimental integrated navigation system and the reference baseline system were transmitted to the navigation computer via communication lines. In the whole integrated navigation experiment, the navigation computer recorded the sensor data in real-time for subsequent processing.

5.2. Experimental Results and Discussions

To compare the integrated navigation effect under different filtering methods, the federated Kalman filter, the adaptive federated Kalman filter, and the adaptive federated IMM filter were separately applied in the real experiment. The integrated navigation experiment lasted for 600 s. Figure 6 shows the estimation trajectories of the integrated navigation experiment by using the three filtering methods. In Figure 6, the black line represents the true trajectory, the blue line represents the estimation trajectory of the federated Kalman filter, the green line represents the estimation trajectory of the adaptive federated Kalman filter, and the red line represents the estimation trajectory of the adaptive federated IMM filter. It can be seen from Figure 6 that all three filtering methods can be used for integrated navigation. But from the partially enlarged figure, the estimation trajectory of the adaptive federated IMM filter is the closest to the true trajectory compared with the estimation trajectories of the adaptive federated Kalman filter and the federated Kalman filter. The estimation trajectory of the adaptive federated Kalman filter is closer to the true trajectory compared with that of the federated Kalman filter. It indicates that among the three filtering methods, the proposed adaptive federated IMM filter can achieve the highest accuracy of integrated navigation, and the adaptive federated Kalman filter is second, followed by the federated Kalman filter.

To further analyze the results of integrated navigation by using the three filtering methods in detail, the attitude angles, velocities, and positions of the integrated navigation system are presented, respectively, in this paper. The estimation curves of heading angle and heading angle error are presented in Figure 7 and Figure 8, respectively. From Figure 7, the heading angle of the test vehicle changes between 100° and −50° in the whole integrated navigation experiment, and it has a big range of change. The estimation values of the heading angle based on the three filtering methods can all track the change of the true heading angle, but the estimation accuracies are different.

It can be seen from Figure 8 that the estimation values of heading angle error of the federated Kalman filter are the biggest. The estimation values reach the maximum value of 9.38° around 100 s, and after that they reduce to 2.68° at 600 s. The estimation values of the adaptive federated Kalman filter are smaller than that of the federated Kalman filter. Its estimation values reach the maximum value of 6.31° around 100 s, and after that they reduce to 1.05° at 600 s. Comparatively, the estimation values of the adaptive federated IMM filter is the smallest. Its estimation values reach the negative maximum value of −0.63° around 50 s, then after that the estimation values tend to stabilize around 0.26°, and they are little influenced by the change of external disturbances. This is because the proposed adaptive federated IMM filter uses different models for each local system, when the external disturbances change the model for each local system can switch in time. Therefore, the adaptive federated IMM filter can use the most accurate model to describe the current state of each local system, and the estimation values of heading angle error can be effectively reduced.

The estimation curves of pitch angle and pitch angle error are presented in Figure 9 and Figure 10, respectively. In Figure 9, the pitch angle of the test vehicle changes between 2° and −3° in the integrated navigation experiment. Although the estimation values of pitch angle from the three filtering methods are close to the true pitch angle, there still are some differences.

It can be seen from Figure 10 that when using the federated Kalman filter, the estimation values of pitch angle error reach the maximum value of 0.89° around 60 s, and after that they begin to reduce to smaller values with some fluctuations. When using the adaptive federated Kalman filter, the estimation values reach the maximum value of 0.57° around 60 s, and after that they begin to reduce with fluctuations smaller than that of the federated Kalman filter. In contrast, when using the adaptive federated IMM filter, the estimation values reach the maximum value of 0.50° around 30 s, and after that they quickly form pattern more stable than the other two filtering methods.

Correspondingly, the estimation curves of roll angle and roll angle error are shown in Figure 11 and Figure 12, respectively. As shown in Figure 11, the roll angle of the test vehicle changes between 2.5° and −2.5° in the integrated navigation experiment. The estimation values of the roll angle of all three filtering methods are close to the true roll angle, but there are still some differences.

It can be seen from Figure 12 that when using the federated Kalman filter, the estimation values of roll angle error reach a negative maximum value of −0.65° around 20 s and a positive maximum value of 0.75° around 90 s, and they are greatly affected by the external noises. When using the adaptive federated Kalman filter, the estimation values of roll angle error reach a negative maximum value −0.33° around 20 s and a positive maximum value of 0.57° around 90 s, and they are less affected by the external noises. In contrast, when using the adaptive federated IMM filter, the estimation values only reach a negative maximum value of −0.22° around 20 s, and after that, the estimation values become distinctly more stable than those from the other two filtering methods.

The estimation curves of east velocity and east velocity error are shown in Figure 13 and Figure 14, respectively. As shown in Figure 13, the east velocity changes between −20 m/s and 15 m/s. The estimation values of east velocity of three filtering methods can all track the change of the true east velocity, but their estimation accuracy is different. From the partially enlarged figure, the estimation values of east velocity of the adaptive federated IMM filter is the closest to the true east velocity.

As shown in Figure 14, when using the federated Kalman filter, the estimation values of east velocity error have large fluctuations within 300 s, with a maximum value of 1.68 m/s around 200 s. When using the adaptive federated Kalman filter, the estimation values reach a maximum value of 1.05 m/s around 200 s, and the fluctuations are smaller than those of the federated Kalman filter. Comparatively, when using the adaptive federated IMM filter, the estimation values keep stable in the whole process, and its fluctuations are the smallest among the three filtering methods. This is because the proposed adaptive federated IMM filter can switch the model of each local system in time when the external disturbances change, and the most accurate model can be established to describe the current motion state.

The estimation curves of north velocity and north velocity error are shown in Figure 15 and Figure 16, respectively. From Figure 15, the north velocity changes between 0 and 20 m/s. It can be seen from the partially enlarged figure that the estimation values of north velocity by using the adaptive federated IMM filter is the closest to the true north velocity compared with that by using the other two filtering methods.

As shown in Figure 16, when using the federated Kalman filter, the estimation values of north velocity error have a couple of big fluctuations within 350 s, and the estimation values reach a negative maximum value of −1.99 m/s around 50 s. Therefore, its estimation values are greatly affected by the external noises during the integrated navigation experiment. When using the adaptive federated Kalman filter, the estimation values reach a negative maximum value of −0.99 m/s around 110 s, and the fluctuations are distinctly smaller than those using the federated Kalman filter. Comparatively, when using the adaptive federated IMM filter, its estimation values have the smallest fluctuations among all three filtering methods.

The estimation curves of latitude and latitude error are shown in Figure 17 and Figure 18, respectively. Because the position is the integral of velocity, the estimation curves of position are smoother than that of velocity. From Figure 17, the estimation curves by using all the filtering methods can track the change of latitude, but the estimation values of latitude by using the adaptive federated IMM filter are the closest to the true latitude among the three filtering methods.

From Figure 18, the estimation values of latitude error by using the federated Kalman filter exist big fluctuations and they reach a negative maximum value of −13.10 m around 300 s. The estimation values by using the adaptive federated Kalman filter reach a maximum value of 7.29 m around 100 s, and its estimation values have smaller fluctuations than that using the federated Kalman filter. In contrast, when using the adaptive federated IMM filter, the estimation values of latitude error are distinctly more stable than that using the other two filtering methods.

Correspondingly, the estimation curves of longitude and longitude error are shown in Figure 19 and Figure 20, respectively. As shown in Figure 19, the estimation values of longitude by using the adaptive federated IMM filter are closer to the true longitude compared with that using the federated Kalman filter and the adaptive federated Kalman filter.

Moreover, it can be seen from Figure 20 that when using the federated Kalman filter the estimation values of longitude error reach a maximum value of 37.44 m around 200 s, and they are greatly affected by the external disturbances. When using the adaptive federated Kalman filter, the estimation values reach a maximum value of 27.59 m around 200 s, and they are less affected by the external disturbances than that using the federated Kalman filter. Comparatively, when using the adaptive federated IMM filter, the estimation values of longitude error are the least affected by the external disturbances and they keep stable in the whole process of integrated navigation.

Consequently, the estimation curves of position error are shown in Figure 21. It can be seen that the estimation curves of position error of the federated Kalman filter have some large fluctuations during the 600 s of integrated navigation, and its estimation values reach a maximum value of 37.49 m around 200 s. When using the adaptive federated Kalman filter, the estimation values reach a maximum value of 27.73 m around 200 s, and the fluctuations are smaller than that using the federated Kalman filter. By contrast, when using the adaptive federated IMM filter, the estimation values of position error are the least affected by the external disturbances, and the fluctuations are the smallest among the three filtering methods. The mean absolute errors (MAEs) of position error when using the federated Kalman filter, the adaptive federated Kalman filter, and the adaptive federated IMM filter are 10.87 m, 7.36 m, 3.82 m, respectively. As a result, the position error when using the adaptive federated IMM filter was reduced by 64.86% and 48.10%, respectively, compared with the federated Kalman filter and the adaptive federated Kalman filter. In summary, the MAEs and standard deviations (STDs) of integrated navigation errors when using the three filtering methods are listed in Table 2.

In the AUV integrated navigation experiment, the external environment is complex and time-varying, and the disturbances in the underwater environment are bigger than those on the land. Therefore, the outputs of the DVL and the TAN are easily disturbed by the underwater environment, and the output accuracy of the DVL and the TAN is decreased and is unstable. When the beams of the DVL are unable to reach the seabed, the accuracy of the SINS/DVL measurement model is decreased. When the underwater map information is inaccurate in some places, or the underwater terrain is relatively flat, the SINS/TAN measurement model is unable to provide accurate position information. To solve this problem, the proposed adaptive federated IMM filter is designed to adaptively adjust the information sharing coefficients of the local SINS/DVL system and the local SINS/TAN system during the integrated navigation experiment. Figure 22 shows the values of the information sharing coefficient in the adaptive federated IMM filter. It can be seen that, when the performance of each local system changes, the information sharing coefficients of the local SINS/DVL system and the local SINS/TAN system are adjusting in real time.

Moreover, the model probabilities of the local SINS/DVL system and the local SINS/TAN system in the proposed adaptive federated IMM filter are presented in Figure 23 and Figure 24, respectively. It can be seen that during the process of integrated navigation, the model probability of each local system switches in real-time, and the most accurate model can be established to describe the current state of the local system.

The three filtering methods are coded with C++ and the integrated navigation experiments are run on a computer with Intel Core i7-6500 CPU at 2.50 GHz. The implementation times of the three filtering methods in single step run (a single prediction and update step) are listed in Table 3. It can be seen from Table 3 that the implementation time of the proposed adaptive federated IMM filter in single step run is 2.96 × 10⁻³ s. Therefore, the proposed filtering method can ensure running in real-time during the integrated navigation experiments, and it is fast enough in real-world applications.

In order to further compare the federated Kalman filter, the adaptive federated Kalman filter, and the adaptive federated IMM filter in the process of integrated navigation, this study designed a total of 30 groups of vehicle integrated navigation experiments on a real platform. The test vehicle’s moving trajectories, running speeds, and conditions of the road surface are entirely different in those experiments. In each experiment, the SINS, GNSS receiver, odometer were restarted before working. The line charts of the MAEs of position errors in 30 groups of integrated navigation experiments are shown in Figure 25, and the MAEs of position errors in 30 groups of integrated navigation experiments are listed in Table 4.

From Figure 25 and Table 4, it can be seen that in those vehicle integrated navigation experiments, the position errors determined when using the adaptive federated IMM filter are obviously smaller than those determined when using the federated Kalman filter and the adaptive federated Kalman filter. By calculation, the mean position errors of 30 groups of integrated navigation experiments by using the federated Kalman filter, the adaptive federated Kalman filter, and the adaptive federated IMM filter are 11.86 m, 8.47 m, and 4.03 m, respectively. As a result, the mean position error determined when using the adaptive federated IMM filter was reduced by 66.02% compared with the federated Kalman filter and 52.42% compared with the adaptive federated Kalman filter. The position errors of those vehicle integrated navigation experiments further illustrate that the proposed adaptive federated IMM filter can effectively improve the accuracy of integrated navigation, and it has obvious advantages compared with the federated Kalman filter and the adaptive federated Kalman filter.

6. Conclusions

In this paper, an adaptive federated IMM filter for the AUV integrated navigation system is presented. The adaptive federated IMM filter combines an adaptive federated filter and IMM algorithm to improve the accuracy and reliability of the AUV integrated navigation system in the complex underwater environment. The information sharing coefficient of the adaptive federated IMM filter is adaptively adjusted when the performance of each local system changes. Meanwhile, each local system of the integrated navigation system includes different models, with the change of the underwater environment, and the adaptive federated IMM filter can use the most accurate mixed model to describe the current state of the local system. Furthermore, an AUV integrated navigation system model that includes the system error dynamics model, SINS/DVL and SINS/TAN measurement models was established.

In order to verify the effectiveness of the adaptive federated IMM filter, a total of 30 groups of vehicle integrated navigation experiments on a real platform were performed. The experimental results show that the proposed adaptive federated IMM filter has obvious advantages compared with the federated Kalman filter and the adaptive federated Kalman filter. The research presented in this paper provides a new idea for AUV integrated navigation system in the underwater environment. Further work will focus on the practical application of AUV in the underwater environment to validate the performance of the proposed method.

Author Contributions

W.L., X.C. and J.W. conceived and designed this study; W.L. and X.C. performed the experiments; W.L. wrote the paper. X.C. and J.W. reviewed and edited the manuscript. All authors have read and agreed to the published version of the manuscript.

Funding

The research described above was supported by the National Natural Science Foundation of China (61773116), the Fundamental Research Funds for the Central Universities, the Postgraduate Research and Practice Innovation Program of Jiangsu Province (KYCX17_0103), the Scientific Research Foundation of Graduate School of Southeast University (YBJJ1847), and also supported by the Shanghai Aerospace Science and Technology Innovation Fund (SAST2019079).

Conflicts of Interest

The authors declare no conflict of interest.

References

Costanzi, R.; Fanelli, F.; Meli, E.; Ridolfi, A.; Caiti, A.; Allotta, B. UKF-based navigation system for AUVs: Online experimental validation. IEEE J. Ocean. Eng. 2018, 44, 633–641. [Google Scholar] [CrossRef]
Tang, K.; Wang, J.; Li, W.; Wu, W. A novel INS and Doppler sensors calibration method for long range underwater vehicle navigation. Sensors 2013, 13, 14583–14600. [Google Scholar] [CrossRef] [Green Version]
Hegrenaes, O.; Berglund, E. Doppler water-track aided inertial navigation for autonomous underwater vehicle. In Proceedings of the OCEANS 2009-EUROPE, Bremen, Germany, 11–14 May 2009; pp. 1–10. [Google Scholar]
Stutters, L.; Liu, H.; Tiltman, C.; Brown, D.J. Navigation technologies for autonomous underwater vehicles. IEEE Trans. Syst. Man Cybern. Part C (Appl. Rev.) 2008, 38, 581–589. [Google Scholar] [CrossRef]
Melo, J.; Matos, A. Survey on advances on terrain based navigation for autonomous underwater vehicles. Ocean Eng. 2017, 139, 250–264. [Google Scholar] [CrossRef] [Green Version]
Miller, P.A.; Farrell, J.A.; Zhao, Y.; Djapic, V. Autonomous underwater vehicle navigation. IEEE J. Ocean. Eng. 2010, 35, 663–678. [Google Scholar] [CrossRef]
Tal, A.; Klein, I.; Katz, R. Inertial navigation system/Doppler velocity log (INS/DVL) fusion with partial DVL measurements. Sensors 2017, 17, 415. [Google Scholar] [CrossRef] [PubMed]
Lyu, W.; Cheng, X.; Wang, J. Adaptive UT-H∞ Filter for SINS’Transfer Alignment Under Uncertain Disturbances. IEEE Access 2020, 8, 69774–69787. [Google Scholar] [CrossRef]
Lyu, W.; Cheng, X. A Novel Adaptive H∞ Filtering Method with Delay Compensation for the Transfer Alignment of Strapdown Inertial Navigation Systems. Sensors 2017, 17, 2753. [Google Scholar] [CrossRef] [Green Version]
Karmozdi, A.; Hashemi, M.; Salarieh, H. Design and practical implementation of kinematic constraints in Inertial Navigation System-Doppler Velocity Log (INS-DVL)-based navigation. Navig. J. Inst. Navig. 2018, 65, 629–642. [Google Scholar] [CrossRef]
Davari, N.; Gholami, A. An asynchronous adaptive direct Kalman filter algorithm to improve underwater navigation system performance. IEEE Sens. J. 2016, 17, 1061–1068. [Google Scholar] [CrossRef]
Li, W.; Wu, W.; Wang, J.; Lu, L. A fast SINS initial alignment scheme for underwater vehicle applications. J. Navig. 2013, 66, 181–198. [Google Scholar] [CrossRef] [Green Version]
Donovan, G.T. Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter. IEEE J. Ocean. Eng. 2012, 37, 431–445. [Google Scholar] [CrossRef]
Kalyan, B.; Balasuriya, A. Multisensor data fusion approach for terrain aided navigation of autonomous underwater vehicles. In Proceedings of the Oceans’ 04 MTS/IEEE Techno-Ocean’04 (IEEE Cat. No. 04CH37600), Kobe, Japan, 9–12 November 2004; pp. 2013–2018. [Google Scholar]
Nygren, I.; Jansson, M. Terrain navigation for underwater vehicles using the correlator method. IEEE J. Ocean. Eng. 2004, 29, 906–915. [Google Scholar] [CrossRef]
Wang, K.; Zhu, T.; Gao, Y.; Wang, J. Efficient Terrain Matching With 3-D Zernike Moments. IEEE Trans. Aerosp. Electron. Syst. 2018, 55, 226–235. [Google Scholar] [CrossRef]
Zhang, T.; Chen, L.; Yan, Y. Underwater positioning algorithm based on SINS/LBL integrated system. IEEE Access 2018, 6, 7157–7163. [Google Scholar] [CrossRef]
Fang, Y.; Yang, K.; Cheng, R.; Sun, L.; Wang, K. A Panoramic Localizer Based on Coarse-to-Fine Descriptors for Navigation Assistance. Sensors 2020, 20, 4177. [Google Scholar] [CrossRef]
Xiong, Z.; Chen, J.-H.; Wang, R.; Liu, J.-Y. A new dynamic vector formed information sharing algorithm in federated filter. Aerosp. Sci. Technol. 2013, 29, 37–46. [Google Scholar] [CrossRef]
Carlson, N.A. Federated square root filter for decentralized parallel processors. IEEE Trans. Aerosp. Electron. Syst. 1990, 26, 517–525. [Google Scholar] [CrossRef]
Carlson, N.A.; Berarducci, M.P. Federated Kalman filter simulation results. Navigation 1994, 41, 297–322. [Google Scholar] [CrossRef]
Carlson, N.A. Federated filter for fault-tolerant integrated navigation systems. In Proceedings of the IEEE PLANS’88, Position Location and Navigation Symposium, Record, ‘Navigation into the 21st Century’, Orlando, FL, USA, 29 November–2 December 1988; pp. 110–119. [Google Scholar]
Carlson, N.A. Federated filter for computer-efficient, near-optimal GPS integration. In Proceedings of the Position, Location and Navigation Symposium-PLANS’96, Atlanta, GA, USA, 22–25 April 1996; pp. 306–314. [Google Scholar]
Tupysev, V. Federated Kalman filtering via formation of relation equations in augmented state space. J. Guid. Control Dyn. 2000, 23, 391–398. [Google Scholar] [CrossRef]
Shen, K.; Wang, M.; Fu, M.; Yang, Y.; Yin, Z. Observability Analysis and Adaptive Information Fusion for Integrated Navigation of Unmanned Ground Vehicles. IEEE Trans. Ind. Electron. 2019, 67, 7659–7668. [Google Scholar] [CrossRef]
Qi-tai, G.; Song, W. Optimized Algorithm for Information-sharing Coefficients of Federated Filter. J. Chin. Inert. Technol. 2003, 11, 1–6. [Google Scholar]
Zhang, Y.; Yu, F.; Wang, Y.; Wang, K. A robust SINS/VO integrated navigation algorithm based on RHCKF for unmanned ground vehicles. IEEE Access 2018, 6, 56828–56838. [Google Scholar] [CrossRef]
Zhu, Y.; Cheng, X.; Hu, J.; Zhou, L.; Fu, J. A novel hybrid approach to deal with DVL malfunctions for underwater integrated navigation systems. Appl. Sci. 2017, 7, 759. [Google Scholar] [CrossRef] [Green Version]
Wang, Q.; Cui, X.; Li, Y.; Ye, F. Performance enhancement of a USV INS/CNS/DVL integration navigation system based on an adaptive information sharing factor federated filter. Sensors 2017, 17, 239. [Google Scholar] [CrossRef] [Green Version]
Kimball, P.; Rock, S. Sonar-based iceberg-relative navigation for autonomous underwater vehicles. Deep Sea Res. Part. II Top. Stud. Oceanogr. 2011, 58, 1301–1310. [Google Scholar] [CrossRef]
Huang, H.; Chen, X.; Zhang, J. Weight self-adjustment Adams implicit filtering algorithm for attitude estimation applied to underwater gliders. IEEE Access 2016, 4, 5695–5709. [Google Scholar] [CrossRef]
Daeipour, E.; Bar-Shalom, Y. An interacting multiple model approach for target tracking with glint noise. IEEE Trans. Aerosp. Electron. Syst. 1995, 31, 706–715. [Google Scholar] [CrossRef]
Farrell, W.J. Interacting multiple model filter for tactical ballistic missile tracking. IEEE Trans. Aerosp. Electron. Syst. 2008, 44, 418–426. [Google Scholar] [CrossRef]
Mazor, E.; Averbuch, A.; Bar-Shalom, Y.; Dayan, J. Interacting multiple model methods in target tracking: A survey. IEEE Trans. Aerosp. Electron. Syst. 1998, 34, 103–123. [Google Scholar] [CrossRef]
Kirubarajan, T.; Bar-Shalom, Y.; Pattipati, K.R.; Kadar, I. Ground target tracking with variable structure IMM estimator. IEEE Trans. Aerosp. Electron. Syst. 2000, 36, 26–46. [Google Scholar] [CrossRef]
Wang, L.; Li, S. Enhanced multi-sensor data fusion methodology based on multiple model estimation for integrated navigation system. Int. J. Control. Autom. Syst. 2018, 16, 295–305. [Google Scholar] [CrossRef]
Hong, L. Multirate interacting multiple model filtering for target tracking using multirate models. IEEE Trans. Autom. Control. 1999, 44, 1326–1340. [Google Scholar] [CrossRef]
Evans, J.S.; Evans, R.J. Image-enhanced multiple model tracking. Automatica 1999, 35, 1769–1786. [Google Scholar] [CrossRef]
Dunne, D.; Kirubarajan, T. Multiple model multi-Bernoulli filters for manoeuvering targets. IEEE Trans. Aerosp. Electron. Syst. 2013, 49, 2679–2692. [Google Scholar] [CrossRef]
Kim, Y.S.; Hong, K.S. An IMM algorithm with federated information mode-matched filters for AGV. Int. J. Adapt. Control Signal. Process. 2007, 21, 533–555. [Google Scholar] [CrossRef]
Song, H.; Shin, V.; Jeon, M. Mobile node localization using fusion prediction-based interacting multiple model in cricket sensor network. IEEE Trans. Ind. Electron. 2011, 59, 4349–4359. [Google Scholar] [CrossRef]
Lee, D.; Liu, C.; Liao, Y.-W.; Hedrick, J.K. Parallel interacting multiple model-based human motion prediction for motion planning of companion robots. IEEE Trans. Autom. Sci. Eng. 2016, 14, 52–61. [Google Scholar] [CrossRef]

Figure 1. The schematic diagram of the federated Kalman filter.

Figure 2. The schematic diagram of the adaptive Federated IMM filter.

Figure 3. The test vehicle platform.

Figure 4. The real picture of the strapdown inertial navigation system (SINS).

Figure 5. The real picture of the odometer.

Figure 6. The estimation trajectories of the integrated navigation experiment.

Figure 7. The estimation curves of heading angle.

Figure 8. The estimation curves of heading angle error.

Figure 9. The estimation curves of pitch angle.

Figure 10. The estimation curves of pitch angle error.

Figure 11. The estimation curves of roll angle.

Figure 12. The estimation curves of roll angle error.

Figure 13. The estimation curves of east velocity.

Figure 14. The estimation curves of east velocity error.

Figure 15. The estimation curves of north velocity.

Figure 16. The estimation curves of north velocity error.

Figure 17. The estimation curves of latitude.

Figure 18. The estimation curves of latitude error.

Figure 19. The estimation curves of longitude.

Figure 20. The estimation curves of longitude error.

Figure 21. The estimation curves of position error.

Figure 22. The values of the information sharing coefficient.

Figure 23. The model probability of the local SINS/DVL system.

Figure 24. The model probability of the local SINS/TAN system.

Figure 25. The mean absolute errors (MAEs) of position errors in 30 groups of integrated navigation experiments.

Table 1. Specifications of the instruments in the integrated navigation experiment.

Instruments	Parameters	Accuracy
SINS	three-axis gyro random constant drifts three-axis gyro random noise three-axis accelerometer random constant biases three-axis accelerometer random noise	1.0°/h (1σ) 0.25°/h^1/2 (1σ) 0.1 mg (1σ) 0.04 μg /Hz^1/2 (1σ)
Odometer	Velocity	120 pulse/circle
GNSS receiver	Position	10 m (1σ)

Table 2. The mean absolute errors (MAEs) and standard deviations (STDs) of integrated navigation errors by the three filtering methods.

Parameter Errors	Federated Kalman Filter		Adaptive Federated Kalman Filter		Adaptive Federated IMM Filter
Parameter Errors	MAE	STD	MAE	STD	MAE	STD
Heading Angle (°)	3.99	1.97	1.54	1.35	0.33	0.12
Pitch Angle (°)	0.25	0.20	0.22	0.10	0.21	0.07
Roll Angle (°)	0.14	0.23	0.14	0.13	0.13	0.07
East Velocity (m/s)	0.23	0.59	0.14	0.34	0.02	0.22
North Velocity (m/s)	0.14	0.60	0.05	0.30	−0.02	0.25
Latitude (m)	−0.99	4.20	−0.62	2.79	−0.26	1.64
Longitude (m)	4.58	11.68	2.76	7.79	0.78	3.97
Position (m)	10.87	7.59	7.36	4.72	3.82	2.13

Table 3. The implementation times of the three filtering methods in single step run.

Filtering Methods	Time (s)
Federated Kalman Filter	9.61 × 10⁻⁴
Adaptive Federated Kalman Filter	9.72 × 10⁻⁴
Adaptive Federated IMM Filter	2.96 × 10⁻³

Table 4. The MAEs of position errors (m) in 30 groups of integrated navigation experiments.

Number	Federated Kalman Filter (m)	Adaptive Federated Kalman Filter (m)	Adaptive Federated IMM Filter (m)
1	10.87	7.36	3.82
2	13.06	9.51	4.23
3	11.87	8.43	4.06
4	10.12	7.14	3.35
5	14.42	10.67	4.78
6	10.07	8.11	3.24
7	11.25	7.68	3.89
8	13.43	9.95	5.21
9	11.98	8.51	4.09
10	10.49	7.04	3.26
11	10.86	7.56	3.08
12	13.65	10.04	5.33
13	15.51	11.27	4.56
14	12.90	7.57	3.18
15	11.35	7.84	3.77
16	12.57	9.21	4.39
17	11.87	8.64	3.31
18	10.76	7.45	2.89
19	12.30	8.93	4.41
20	15.83	10.94	5.42
21	11.74	8.87	4.60
22	10.18	7.16	3.14
23	9.59	7.03	3.91
24	10.94	7.81	4.07
25	10.89	8.10	4.35
26	9.75	7.23	3.57
27	13.70	11.25	5.04
28	11.66	7.59	4.11
29	10.52	7.14	3.36
30	11.78	8.10	4.53

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Lyu, W.; Cheng, X.; Wang, J. Adaptive Federated IMM Filter for AUV Integrated Navigation Systems. Sensors 2020, 20, 6806. https://doi.org/10.3390/s20236806

AMA Style

Lyu W, Cheng X, Wang J. Adaptive Federated IMM Filter for AUV Integrated Navigation Systems. Sensors. 2020; 20(23):6806. https://doi.org/10.3390/s20236806

Chicago/Turabian Style

Lyu, Weiwei, Xianghong Cheng, and Jinling Wang. 2020. "Adaptive Federated IMM Filter for AUV Integrated Navigation Systems" Sensors 20, no. 23: 6806. https://doi.org/10.3390/s20236806

APA Style

Lyu, W., Cheng, X., & Wang, J. (2020). Adaptive Federated IMM Filter for AUV Integrated Navigation Systems. Sensors, 20(23), 6806. https://doi.org/10.3390/s20236806

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

Adaptive Federated IMM Filter for AUV Integrated Navigation Systems

Abstract

1. Introduction

2. Federated Kalman Filter

3. Adaptive Federated IMM Filter

3.1. Adaptive Federated Kalman Filter

3.2. Adaptive Federated IMM Filter

4. AUV Integrated Navigation System Model

4.1. System Error Dynamics Model

4.2. System Measurement Model

5. Experimental Results and Discussions

5.1. Experimental Settings

5.2. Experimental Results and Discussions

6. Conclusions

Author Contributions

Funding

Conflicts of Interest

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI