A New Variational Bayesian Adaptive Extended Kalman Filter for Cooperative Navigation.

To solve the problem of unknown state noises and uncertain measurement noises inherent in underwater cooperative navigation, a new Variational Bayesian (VB)-based Adaptive Extended Kalman Filter (VBAEKF) for master⁻slave Autonomous Underwater Vehicles (AUV) is proposed in this paper. The Inverse Wishart (IW) distribution is used to model the predicted error covariance and measurement noise covariance matrix. The state, together with the predicted error covariance and measurement noise covariance matrix, can be adaptively estimated based on VB approximation. The performance of the proposed algorithm is demonstrated through a lake trial, which shows the advantage of the proposed algorithm.

1. Introduction

With the continuous expansion of marine exploitations and the increasing complexity of military requirements, it will be difficult for a single Autonomous Underwater Vehicle (AUV) to achieve the desired goals. Therefore, a multi-AUV cooperative system has gradually become more popular [1,2,3]. Similar to a single AUV, the multi-AUV cooperative operation also requires accurate navigation and localization abilities [4,5]. In the master–slave cooperative navigation, it is generally considered that the AUVs with strong positioning ability are the master AUVs, and the master AUVs are the core of the AUV group that each slave AUV needs to communicate with. This cooperative navigation method has many advantages, such as low cost, easy realization, group resolution and flexible combination [6,7]. We aim to improve the navigation of slave AUVs by the filtering method, using the accurate location information of the master AUV and the relative observation between the master and slave AUVs.

The sensor of the multi-AUV cooperative navigation system includes various navigation, communication, and detection sensors, and they are the main sources of information for cooperative navigation. According to the object classification described by the sensor information, the navigation equipment can be divided into internal sensors and external sensors [8,9,10]. The internal sensors refer to the sensors used to measure the AUV’s own motion parameters, including Doppler Velocity Log (DVL), magnetic compass, and depth sensor. The DVL can measure the speed of the vehicle relative to the seabed, the magnetic compass is used to measure the vehicle heading information, while the pressure depth gauge can obtain the depth information. The external sensors include sensors for realizing underwater sound detection and underwater acoustic communication. In practical cooperative navigation, these sensors are affected by many factors such as temperature, salinity, depth, current, interface reflection and refraction in the underwater environment [11,12,13,14]. The underwater acoustic channel becomes a high noise channel with strong reverberation, narrow channel bandwidth, and multipath effects. The underwater acoustic communication of an AUV consisting of multiple sensors is constrained by these factors, making the noise statistical characteristics of the sensor inaccurate and even able to change over time. At this point, if the information fusion filter adopts the wrong noise covariance matrix such as the method described in [3], its accuracy may degrade or even diverge. Therefore, if real-time online estimation of the unknown process and measurement noise statistics can be performed in the filtering process, the filtered noise covariance matrix will act as an adaptive adjustment to meet the actual noise characteristics of the system, which will further improve the accuracy of AUV cooperative navigation [15,16].

In recent years, various adaptive filters have been proposed in order to solve the problems existing in conventional filters when the noise statistics are unknown or time-varying [12,17]. Based on maximum posterior estimation, the Sage-Husa adaptive Kalman filter can estimate the statistical characteristics of the system measurement noise in real time to improve the estimation accuracy of the filtering [18]. However, it requires that the system noise covariance and measurement noise covariance must be positive or positive definite, otherwise it will easily lead to filter divergence [19,20]. Fading adaptive filtering adjusts the weight of the newly measured data by increasing the one step predicted covariance matrix, but the calculation process of the scalar fade-in factor is cumbersome and has the same adjustment ability for each filter channel [21]. Although the maximum likelihood based adaptive filtering method can estimate and correct the second-order moments of the statistical characteristics of noise online, it needs to rely on an accurate innovation covariance estimate [22,23,24]. The existing Variational Baysian (VB) based adaptive Kalman filter can estimate an inaccurate and slowly varying measurement noise covariance matrix for linear Gaussian state-space models offline [17,25]. However, it is not suitable for the cooperative navigation model since it is nonlinear.

In this paper, in order to further improve the estimation accuracy of the filter when the noise statistical information is unknown or time-varying in AUV cooperative navigation, a Gaussian approximate extended Kalman filter based on VB is proposed. In order to solve the inaccurate measurement noise generated by underwater acoustic communication, and uncertain process noise during dead reckoning, this paper models the Inverse Wishart (IW) prior to the prediction error covariance matrix and measurement noise covariance matrix. This VB method is then used to calculate the system state together with the unknown noise parameters. The VB method is used to estimate the predicted error covariance matrix instead of process noise parameters, so that state estimation considers not only the change of noise, but also the variation of the predicted error covariance. The effectiveness of the proposed filtering algorithm was evaluated by using postprocessed data collected in lake trials, which shows that the proposed filter has significantly improved robustness of unknown or time-varying noises than existing filters.

The main structure is listed as follows. In Section 2, we provide a short review of the cooperative navigation model using acoustic range measurement. In Section 3, a new Variational Bayesian-based Adaptive Extended Kalman Filter (VBAEKF) is derived. In Section 4, the proposed algorithm and existing state-of-the-art algorithms are tested in the actual lake field trial to show the excellent performance of the proposed algorithm. Finally, Section 5 discusses our conclusions.

2. System Model

To achieve master–slave cooperative navigation, master AUVs are usually equipped with a high-precision Inertial Navigation System (INS), DVL, and depth sensors to form a high-precision integrated navigation system for underwater navigation. Another option is having the master AUVs periodically emerge to receive high-precision GPS location information to achieve its own high-precision navigation and positioning. The slave AUV only has a low-precision magnetic compass, DVL, and depth sensors to perform dead reckoning. During the operation of the system, when the slave AUV uses an underwater acoustic communication device to obtain high-precision AUV position information and relative range reference information from the master AUV, this reference information can be used to realize the self-correction of its own position error and realize the cooperative navigation between vehicles [3,7].

Since the actual depth information can be directly measured by the pressure sensor in real time with bounded error, the depth does not need to be considered in the process equations and the practical working environment of the slave AUV can be simplified to a two-dimensional (2D) space. When calculating the relative range between the AUVs, the depth information is required. If the initial position of the slave AUV is known, the position at time will be calculated in real time according to the speed and azimuth information measured by its own sensors as follows: where and respectively denote the eastward and northward position of the AUV at time , is the sampling interval, and denote the forward and starboard velocity measurement information of the DVL along the vehicle coordinate system, and is the heading measured by the magnetic compass. is system process noise, including speed measurement noise and azimuth measurement noise.

The slave AUV uses the relative range measurement to correct the navigation error. Therefore, the measurement equation is the range between the master AUV and slave AUV, which can be written as follows: where and represent the position of the master AUV and the slave AUV at time step , respectively. In addition, is the measurement noise.

According to the dynamic system model (1) and measurement model (2), the discrete-time state space equation from the AUV can be written as: where the state transition matrix and represent a two-dimensional identity matrix. The control input is and . and are system process noise and measurement noise, respectively, and are usually assumed to be independent zero-mean Gaussian white noise sequences that satisfy the distributions and . The measurement Equation (4) shows that is non-linear, so it is necessary to apply the partial derivative matrix to the proposed algorithm. The partial derivative matrix of the measurement equation can be written as: where represents moment predicted position of the slave AUV.

3. The Proposed Cooperative Navigation Algorithm Based on Variational Bayesian

The previous section studied the multi-AUV’s cooperative navigation technology under ideal communication; that is, suppose we know the exact mathematical model of the system, and both the system noise and the measurement noise satisfy the Gaussian distribution. However, during the actual navigation of AUVs, the underwater environment is affected by factors such as temperature, season, and current layer, while the underwater acoustic distance observation is often interfered with by abnormal measurement noise. The resulting measurement noises and its covariance matrix are often unknown or inaccurate. At the same time, the process equation of the system is influenced by the dead reckoning sensor, and the process noise often cannot be a constant value. The variational Bayesian method is introduced to solve this problem [17,26].

In the conventional Kalman filtering model, the one-step predicted Probability Density Function (PDF) and likelihood PDF are normal distributions: where is the normal distribution with mean and variance , and the probability density function of the normal distribution is:

According to Equation (3), the predicted state vector and the corresponding one-step predicted covariance matrix can be written as: where and respectively represent the state estimation at time and the corresponding estimation error covariance matrix. Note that the in Equation (10) is not exact, because the true process noise covariance matrix is unknown due to the effects of complex underwater environments.

In Bayesian statistics, the Inverse Wishart (IW) distribution can be viewed as the conjugate prior distribution for the covariance matrix of a normal distribution with known mean [27]. If the inverse matrix of a positive definite matrix follows the Wishart distribution , then the matrix follows the IW distribution: where is the degree of freedom, is a positive definite matrix, is the dimension of , is the multivariate gamma distribution, and is the trace function. If the matrix obeys the IW distribution, its expectation is [27]:

Since and are the covariance matrices of the normal PDFs, their prior distributions and can be written as IW distributions: where and represent the degrees of freedom and scale matrix of , and represent the degrees of freedom and scale matrix of , respectively. Next, we need to get the value of , , , and .

The mean value of can be set as the nominal predicted error covariance matrix . According to Equation (12), we obtain: where represents the dimension of the state and represents the nominal process noise covariance. Let: where is a tuning parameter.

Substituting (16) into (15) gives:

According to Bayes’ theorem, prior distribution can be written as [17]: where is the posterior probability density function of the measurement noise covariance matrix .

According to Equation (14), the prior distribution of the measurement noise covariance matrix is subject to the IW distribution, and its posterior distribution should also be an IW distribution as follows:

In practical applications, the dynamic model of the noise variance of Equation (18) is unknown. So we choose a factor to preserve and propagate the approximate posterior at the previous moment [28], and the prior parameters can be written as: where is a forgetting factor, which represents the degree of fluctuation over time. represents the steady state variance; the smaller the value of , the greater the frequency of fluctuations over time.

In order to estimate the state , the predicted state error covariance and the measurement noise covariance matrix , we need to calculate their joint posterior probability density function . We use the variational Bayesian method and find an approximate PDF of a free form as follows [28,29]: where denotes the approximate posterior PDF of . The VB-approximation can now be formed by minimizing the Kullback-Leibler Divergence (KLD) between the approximation posterior PDF , , and the true joint posterior [29,30]: where KLD represents relative entropy, which is defined as follows:

The optimal solution of Equation (22) satisfies the following equation [29]: where , , represent the constants with respect to variable , predicted error covariance and measurement noise covariance matrix , respectively. represents the logarithm function, and represents the expectation of the approximate posterior PDF of the variable . Because , , are coupled, Equations (25)–(27) cannot be directly solved. These parameters can be solved by the fixed-point iterative method.

Therefore, (26) can be further written as (see the Appendix A for a detail derivation): where represents the approximate PDF of the th iteration of , and is defined as: where represents the expectation of variable at the -th iteration.

According to (28), can be viewed as the IW distribution with the degree of freedom and scale matrix : where the degree of freedom and scale matrix can be expressed as

Equation (27) can be further written as: where is defined as:

According to (33), can be viewed as the IW distribution with the degree of freedom and scale matrix where the degree of freedom and scale matrix can be expressed as

Equation (25) can be further written as: where and can be represented as the following equation [29]:

The one-step predicted PDF and likelihood PDF after updating the th iteration can be written in the following equations:

The corrected predicted error covariance matrix and the measurement noise covariance matrix can be written as:

Employing (41)–(44) in (38), we have: where the normalization constant is given by:

Considering (41)–(46), it can be upgraded to a Gaussian distribution with mean and variance : where mean and variance at th iteration are calculated as:

After fixed-point iteration , we obtain the variational approximation of the posterior PDFs:

The variational Bayesian adaptive EKF proposed in this paper consists of Equations (9), (15)–(17), (20), (21) and the measurement update process in Equations (29)–(32), (34)–(37), (39), (40), (43), (44), (47)–(53). The implementation pseudocode for the proposed adaptive cooperative navigation algorithm is shown in Algorithm 1.

The tuning parameter is a key parameter to implement the proposed VBAEKF. In general, is deemed as a harmonic weight to balance the efficacy of the nominal predicted error covariance matrix and , a smaller tuning parameter means that a large quantity of information about the process model is lost. On the other hand, when becomes larger, the substantial prior uncertainties induced by the inaccurate nominal predicted error covariance matrix will factor into the measurement update. In this paper, the tuning parameter is selected as .

4. Field Trial Results

The effectiveness and superiority of the adaptive cooperative navigation algorithm proposed in this paper is validated in practical systems by using the offline experimental data collected in a lake trial experiment. This experiment was conducted in August 2014 in Taihu Lake, Wuxi. Subject to experimental conditions, the lake test was carried out using the surface boat and acoustic communication equipment shown in Figure 1 and Figure 2. The experimental constitution scheme of the cooperative navigation lake test is shown in Figure 3. Three survey vessels are used in the test. Two vessels acted as the master AUVs and the other is the slave AUV. Each AUV is equipped with underwater acoustic communication equipment S2CR 7/17 (Evologics, Berlin, Germany), which is used to construct the underwater acoustic communication network, obtain the relative range measurements between the master and slave AUVs and transfer the reference information. The real-time GPS position information of two master AUVs is used as a reference during the test. The position deduced from the dead reckoning (DR) of the slave AUV is based on the absolute speed information provided by the DVL and the heading information provided by the magnetic compass with a 1 Hz frequency. At the same time, a GPS/Photonics Inertial Navigation System (PHINS) integrated navigation system is also installed on the slave AUV to provide relatively accurate position, speed and heading references as benchmarks. The relevant sensor indicators used during the test are shown in Table 1. The slave AUV can only receive one acoustic measurement from the master AUV at a time. Due to the complicated underwater environment, the underwater acoustic measurement information may be lost or garbled. For the convenience of data processing, the received acoustic communication data will be stored only if the measurement information received from the master AUVs is correct.

Figure 1

The vessel employed in the experiment.

Figure 2

S2CR 7/17 acoustic equipment.

Figure 3

Experiment scheme of cooperative navigation. DVL: Doppler Velocity Log; PHINS: Photonics Inertial Navigation System.

Table 1

The performance of the sensors used in experiments.

Sensors	Index	Parameters
S2CR 7/17(Master/Slave)	Working range	Up to 8000 m
	Data transfer rate	Up to 6.9 kbit/s
	Error rate	Less than 10^−10
GPS (Master/Slave)	Velocity accuracy	0.1 m/s
	Position accuracy	Less than 2.5 m (Root Mean Square (RMS))
	Data update rate	10 Hz
Compass (Slave)	Heading accuracy	2°
DVL (Slave)	Velocity accuracy	0.1%

The trajectories of the two master AUVs and one slave AUV together with the dead reckoning of slave AUV are drawn in Figure 4. It can be seen that the DR trajectory of the slave vehicle (blue dashed line) gradually diverges from the true trajectory (red dashed line) over time, which will cause a large navigation error.

Figure 4

Paths taken by the two master Autonomous Underwater Vehicles (AUVs) and one slave AUV.

We use the multi-AUVs cooperative navigation scenario through this lake trial experiment to verify the effectiveness and superiority of the algorithm proposed in this paper. The traditional EKF-based cooperative navigation algorithm and the variational Bayesian-based cooperative navigation algorithm proposed in this paper are both used to estimate the location of the slave AUV and compare their performance. In addition, the Sage-Husa Extended Kalman Filter (SHEKF) and Maximum Likelihood Extended Kalman Filter (MLEKF) are also introduced to verify the superiority of the proposed navigation algorithm [31]. They are common methods for estimating unknown noise parameters in practical applications.

The initial state estimate is provided by GPS. And the initial state estimation error covariance is set as . The parameter values of the proposed algorithm and existing algorithms are shown in Table 2. They were carried out in MATLAB R2014 on a computer with an Intel® Core™ i5-3470 CPU 3.20 GHz and 4 GB of RAM. In order to compare the accuracy of the state estimation of the above nonlinear filtering algorithm, the position error and the average position error are chosen as performance indicators, which are defined as follows: where is the reference position of the slave AUV provided by GPS, is the estimated position at time , and is the time length.

Table 2

Parameter values of the proposed algorithm and existing algorithms.

Filters	Parameter	Value
SHEKF	Forgetting factor	1−exp(4)
MLEKF	Sliding window size	20
The proposed VBAEKF	Forgetting factor	ρ=1−exp(4)
	Tuning parameter	τ=2
	The iteration number	N=5

SHEKF: Sage-Husa Extended Kalman Filter; MLEKF: Maximum Likelihood Extended Kalman Filter; VBAEKF: Variational Bayesian Adaptive Extended Kalman Filter.

In order to better demonstrate the effectiveness and superiority of the algorithm, we consider two cases:

Case 1: In the first case, we assume that the nominal state noise covariance and measurement noise covariance are constant in EKF, and they are used as the initial values in the proposed VB-based adaptive filter. Then and are chosen as

The estimated errors from the slave AUV’s position are plotted in Figure 5. The results show that in the traditional EKF-based cooperative navigation algorithm, the state process noise error and measurement noise error do not obey the constant values, resulting in obvious deviation and a long period of time to regain the correct estimate position. In SHEKF, the performance of SHEKF is not ideal because the online estimation of noises is not accurate. From Figure 6, it can be seen that after 1100 s, the drastic decrease in the measured value leads to a large positioning error. The estimated noise covariance matrix of MLEKF is not accurate when an abnormal innovation occurs because the MLEKF relies heavily on the changes of new innovations, which may lead to divergence for the estimated solution. Compared with the traditional EKF, the Sage–Husa adaptive filter and the maximum likelihood adaptive filter, the proposed VB-based adaptive filter can quickly adjust and converge after the position estimation error peak.

Figure 5

Position estimation errors for Case 1 for the EKF, MLEKF, SHEKF and VBAEKF.

Figure 6

Illustration of the measurements received from the two master AUVs.

Table 3 shows the average positioning error and average execution time for the existing algorithm and the proposed algorithm. It can be seen that the execution time of the traditional EKF and Sage–Husa adaptive algorithm is almost the same, and the implementation time of the proposed VB-based adaptive algorithm is slightly longer than the traditional EKF, taking into account the time of the iteration is reasonable. For many practical applications, this increase in execution time is negligible considering the increase in accuracy.

Table 3

RMSE and the average execution time during Case 1 for the Extended Kalman filter (EKF), SHEKF and the VBAEKF.

Filters	RMSE (m)	Execution Time (s)
EKF	6.92 m	3.78×10^−5
SHEKF	6.64 m	3.97×10^−5
The proposed VBAEKF	3.83 m	1.38×10^−4

Case 2: In the second case, and are chosen as large value noises, which are set as: where denotes the test time, and is the time step.

Figure 7 shows the position estimation errors of the existing algorithm and the proposed algorithm. It can be seen that when SHEKF processes large value noise parameters, the estimation error is worse than EKF. The performance of MLEKF in the second case diverges faster than in the first case. From Table 4, it can be seen that the average positioning error of the traditional EKF-based cooperative navigation algorithm is 6.33 m. The use of the VBAEKF-based cooperative navigation algorithm reduces the average positioning error to 4.5 m, and the positioning accuracy is improved by . The algorithm presented in this paper has obvious improvement in positioning accuracy compared with the existing algorithms. This is because the proposed algorithm can better estimate the prediction covariance matrix and the measurement noise covariance matrix than the existing algorithms. Therefore, compared with other existing cooperative navigation algorithms, the proposed algorithm is more robust against large values of the process noise covariance matrix and measurement noise covariance matrix.

Figure 7

Position estimation errors for Case 2.

Table 4

RMSE and the average execution time for Case 2.

Filters	RMSE (m)	Execution Time (s)
EKF	6.33 m	3.52×10^−5
SHEKF	6.65 m	3.86×10^−5
The proposed VBAEKF	4.50 m	1.34×10^−4

5. Conclusions

This paper presents a variational Bayesian adaptive extended Kalman filter algorithm for cooperative navigation of master–slave AUVs. The predicted error covariance matrix and measurement noise covariance matrix are modeled as inverse Wishart priors and are inferred by the VB method together with the system state. In the proposed method, the predicted error covariance matrix is estimated instead of process noise parameters, so the state estimation considers not only the change of noise, but also the variation of the predicted covariance to better model the cooperative navigation of AUVs. It is compared with the typical cooperative navigation algorithms using real experimental data. Experimental results show that the proposed adaptive EKF algorithm is superior to the traditional EKF algorithm and the Sage-Husa adaptive algorithm in terms of positioning error and is suitable for application in multi-AUVs cooperative navigation.