Adaptive Federated IMM Filter for AUV Integrated Navigation Systems

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.


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.

Federated Kalman Filter
The state equation of a linear discrete-time system i can be presented as follows [22,32]: (1) where Φ i k,k−1 is the (n × n) state transition matrix; Γ i k−1 is the (n × r) system noise matrix; W i k−1 is the (r × r) system noise matrix; X i k is the (n × 1) state estimate; H i k is the (m × n) measurement matrix; V i k is the measurement noise matrix; Z i k is the (m × 1) measurement value. In Equation (1), both W i k−1 and V i k are assumed as the zero-mean Gaussian white noise, and their covariances are: In Equation (3), 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 g k , P g k andX g k are allocated according to the following information allocation principles [21,22]: where β i represents the information sharing coefficient, β i > 0. Meanwhile, it satisfies the following information conservation principle: (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: (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 Sensors 2020, 20, 6806 4 of 25 filters conduct the measurement updating of the information separately according to the following equations [22,23]: (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: Sensors 2020, 20, x FOR PEER REVIEW 4 of 25 filters conduct the measurement updating of the information separately according to the following equations [22,23]: (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:

Time Update
Optimal Fusion Figure 1. The schematic diagram of the federated Kalman filter.

Adaptive Federated Kalman Filter
When given a matrix m n i 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 i P 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 ( ) i k P at filtering step k, and it can be expressed as follows:

Adaptive Federated Kalman Filter
When given a matrix P i ∈ C m×n , there are unitary matrix Y i = [λ i,1 λ i,2 · · · λ i,m ] and unitary matrix Λ i = [µ i,1 µ i,2 · · · µ i,n ] that satisfy the following relationship: where (10) is the singular value decomposition of the matrix P i , and ξ i, 1 , ξ i, 2 , · · · , ξ i, r are the singular values of the matrix P i . Define P i ((p λµ ) n×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: 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: 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: Consequently, the process of system information distribution can be expressed as:

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: In addition, the measurement equation of model q can be presented as follows: 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, Sensors 2020, 20, 6806 6 of 25 and the element e pq represents the probability of the model p transferring to the model q. The probability transfer matrix is represented as follows: e 11 · · · e 1s · · · · · · · · · e s1 · · · e ss 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 stateX p (k − 1) and the model probability µ p (k − 1) of each filter at the filtering step k − 1, the mixed estimation stateX 0q (k − 1), and the covariance matrix P 0q (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 The mixed probability of the model p transfer to the model q is The mixed estimation state of the model q is represented aŝ Then the mixed covariance matrix estimation of the model q is represented as (2) Kalman filtering (model q): TakeX 0q (k − 1), P 0q (k − 1) and Z(k) as the input of the Kalman filter, then the estimation statê X q (k) and the estimation covariance matrix P q (k) can be updated.
The prediction state isX The prediction covariance matrix is The gain of the Kalman filter is The estimation stateX q (k) iŝ Then the estimation covariance matrix P q (k) is (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 where Then, the probability of the model q is where c is a normalization constant, and c = s q=1 ϑ q (k)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 stateX(k) and the mixed covariance matrix estimation P(k) can be calculated.
Consequently, the mixed estimation state can be presented aŝ In addition, the mixed estimation covariance matrix can be presented as 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.
Sensors 2020, 20, 6806 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.

Reference System
Local System 1 . .

Time Update
Optimal

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: is the zero-mean Gaussian

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: .
where X(t) is the (n × 1) state estimation, . 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: 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: . . .
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.
Sensors 2020, 20, 6806 9 of 25 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: The position error equations in ENU axes are as follows:

System Measurement Model
The measurement equation of the AUV integrated navigation system can be expressed as follows: 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 SE , north velocity V SN and up velocity V SU exported by the SINS and the east velocity V DE , north velocity V DN and up velocity V DU exported by the DVL.
where δV SE , δV SN , δV SU are the east velocity error, north velocity error and up velocity error of the SINS, respectively; δV DE , δV DN , δV SU 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 SINS , longitude λ SINS and height h SINS exported by the SINS and the latitude L TAN , longitude λ TAN and height h TAN exported by the TAN.
where δL SINS , δλ SINS , δh SINS are the latitude error, longitude error and height error of the SINS, respectively; δL TAN , δλ TAN , δh TAN 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.

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

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    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.
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     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 Figures 4 and 5, respectively.     In the adaptive federated IMM filtering method, the specific parameters of the probability transfer matrix E are set as follows: 0.9 0.05 0.05 0.05 0.9 0.05 0.05 0.05 0.9 The variance matrix of measurement noise in the SINS/DVL integrated navigation system is R 1 = diag [0.1 m/s, 0.1 m/s] 2 . 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 1 , 3R 1 , 8R 1 , respectively.
The variance matrix of measurement noise in the SINS/TAN integrated navigation system is R 2 = diag [10 m, 10 m] 2 . 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 2 , 5R 2 , 10R 2 , 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.

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 Figures 7 and 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.
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 Figures 7 and 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.   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 Figures 7 and 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 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 Figures 9 and 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 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 Figures 9 and 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 Figures  11 and 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 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 Figures 11  and 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 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 Figures  11 and 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 Figures 13 and 14, respectively. As shown in Figure 13  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 Figures 13 and 14, respectively. As shown in Figure 13 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 Figures 13 and 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.  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 Figures 15 and 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. 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 Figures 15 and 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. describe the current motion state.
The estimation curves of north velocity and north velocity error are shown in Figures 15 and 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 Figures 17 and 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. 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 Figures 17 and 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.
The estimation curves of latitude and latitude error are shown in Figures 17 and 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 Figures  19 and 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. Correspondingly, the estimation curves of longitude and longitude error are shown in Figures 19  and 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. 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 Figures  19 and 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.   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.
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 Figures 23 and 24, respectively. It can be seen that during the process of integrated navigation, the model probability of 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 Figures 23 and 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 −3 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   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 −3 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 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 −3 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.  Table 4.

Adaptive Federated IMM Filter (m)
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.

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.