Abstract
The main challenges of sequential estimations of underwater navigation applications are the internal/external measurement noise and the missing measurement situations. A quadratic interpolation-based variational Bayesian filter (QIVBF) is proposed to solve the underwater navigation problem of measurement information missing or insufficiency. The quadratic interpolation is used to improve the observed vector for the precision and stability of sequential estimations when the environment is changed or the measurement information is lost. The state vector, the predicted error covariance matrix, and the measurement noise matrix are derived based on the variational Bayesian method. Simulation results demonstrate the superiority of the proposed QIVBF compared with the traditional algorithm under the condition of measurement information lost by autonomous underwater vehicles.
1. Introduction
Ocean has already become the strategic goal of many countries because of its underdeveloped resources, marine environments, and high-tech fields [1, 2]. The autonomous underwater vehicles (AUVs) have become one of the important tools for underwater detection, environment survey, and underwater reconnaissance in the ocean [3, 4]. The navigation and positioning methods with high accuracy for AUV are the necessary conditions to acquire effective information [5], and also, they are the key technologies to determine whether AUV can work normally and return back safely [6, 7]. The more accurate navigation and positioning methods for AUV are the necessary conditions for the underwater vehicle to ensure the correct travel [8, 9].
The development of underwater vehicles faces many problems; for instance, the ocean environment is complex and changeable due to all sorts of noise interferences, and the AUV may fail to represent the true sensor measurements in many actual applications, like the presence of ocean current, salt cliffs, and ships around. The measurements are sometimes unobservable or insufficient so that positions calculated are erroneous [10–12]. On the other hand, a few navigation methods based on electromagnetic transmitting cannot be used underwater since the signal attenuates quickly underwater [13]. What is more, the localization error of the MEMS system accumulates along with time [14–17]. Those above weaknesses will cause the measurement missing, which has an adverse impact on the positioning of AUV [18]. However, the debugging and arranging process would waste time and material resources. Therefore, the performances of estimation algorithms are vital to working normally for AUV [19, 20].
The Kalman filter (KF), which is a widely and classical recursive filter, could provide the optimal state estimates in the linear dynamic system in the scenarios of known noise models and system models [21–24]. Additionally, the accurate measurements are difficult for the underwater integrated navigation system to describe because the performance of sensors varies with the change in the environment and measurement information probability lost [25, 26]. The joint probability density function (PDF) is used to model the information of the AUV. Then, the information of the joint PDF is determined by using the variational Bayesian (VB) method [27] in recursive process, which is the optimal. To further improve the capability of the navigation with the missing measurements, some strategies are required.
The recently proposed variational Bayesian- (VB-) based adaptive KF is a deterministic approximate Bayesian method that transforms the solution of the posterior probability density function based on Bayes’ theorem into the solution of the functional extreme value [28, 29]. It has more ideal approximate estimation results and computational overhead [30, 31]. It significantly reduces the difficulty of calculation and makes high-precision filtering possible [32–35].
Aiming to mitigate the underwater navigation problem of measurement information missing or insufficiency, this paper proposes a quadratic interpolation-based variational Bayesian filter (QIVBF) algorithm. The QIVBF makes better use of the quadratic interpolation (QI) method and the VB method, so that the predicted error covariance matrix and the measurement noise matrix are derived to estimate the state vector more accurately. The quadratic interpolation improves the observed vector for the precision and stability of sequential estimations when the environment is changed or the measurement information is lost.
The rest of the paper is presented as follows. In Section 2, the situation of the measurement lost in underwater navigation is described in detail. In Section 3, the QI method is used in measurement missing situations. In Section 4, the VB method is used to estimate the accurately predicted error covariance matrix, measurement error matrix, and state vector. Section 5 shows the simulation in underwater navigation. Section 6 gives the main conclusions of the paper.
2. Problem Statement
An underwater navigation function with measurement information lost is described as follows [36]:where is the state posterior given the observation at time ; and denote the positions of the AUV at the time in x and y directions, respectively. and denote the velocities of the AUV at the time in x and y directions, respectively. The is the measurement vector at the time , with and representing the position measurements of the AUV at the time in x and y directions, respectively, and and are the velocity measurements of the AUV at the time in x and y directions, respectively. The denotes the process noise, which often follows the Gaussian distribution with zero mean vector and process error covariance . Similarly, means the measurement noise, which follows the Gaussian distribution with zero mean vector and measurement error covariance . The is the known process function at the time . The is the measurement model at the time .
However, the underwater environment is complex and time-varying. Hence, the measurement information is unavoidable lost, which is described as follows:where is the observation vector and pro means the probability.
3. Cubature Unscented Kalman Filter (CUKF)
3.1. State Model and Measurement Models of a Linear System
For linear systems, the state transfer model is [37]where can be established from system transfer models; is the system input noise, assumed to be white with zero mean. The state vector is defined as
with , , and denoting the latitude, longitude, and height position error in ENU axes, respectively; , , and are the east, north, and upward velocity errors in ENU axes, respectively; , , and represent the heading, pitch, and roll errors in ENU axes, respectively; , , and set as the biases of the accelerometer projections onto the ENU axes; , , and for the gyroscope drift projections onto the ENU axes, respectively; , , and are the measured magnetic field vector in the ENU axes, respectively.
The linear measurement model is defined aswhere , , and are the estimated velocities by DR (Dead Reckoning) along the east, north, and upward direction, respectively; , , and represent the measurement velocity of INS (Inertial Navigation System) along the east, north, and upward direction, respectively. and are the heading measured by gyroscopes and magnetometers, respectively; and denote the pitch measured by gyroscopes and accelerometers, respectively; and describe the roll measured by gyroscopes and accelerometers, respectively; is the observation matrix; is a white Gaussian measurement noise with zero mean value.
3.2. CUKF Models
Generally, for underwater navigation, the state and measurement models of multisensor fusion are nonlinear [38, 39]. How to derive a nonlinear sequential state estimation from equations (3) and (4) is the main task. One can draw the nonlinear state and measurement models as follows:where and represent the state vector and measurement at time step , respectively. is n-dimensional nonlinear function for state transition and is the m-dimensional nonlinear measurement function. and are zero mean process noise and measurement noise with covariance matrix and , respectively. Through discretization, it is straightforward to convert the process and measurement equations in (12) and (13) to the nonlinear process function and measurement function in (5) and (6).
The degree of nonlinearity in the practical underwater navigation is much bigger than the theoretical analysis, especially for MEMS-grade sensors [40, 41]. It may not achieve the desired effect for traditional UKF, and this paper proposes a new algorithm of cubature unscented Kalman filter (CUKF) considering the trade-off among different aspects to improve further accuracy, real-time, and stability.
The procedure for implementing the CUKF can be summarized as follows: Step 1:Sample. The n-dimensional random variable with mean and covariance is approximated by sigma points selected using the following equations: where is a turning parameter denoting the spread of the sigma points around and is often set to a small positive value. This parameter only affects errors caused by more than second-order matrices. The is the column of the matrix square root of which is real symmetric positive definite matrix obtained through Cholesky resolution for . The sample points are composed of sigma points set , and the corresponding weight with these sample points is described as , , is the weight of sigma point, and . Step 2:The first prediction. Each point is instantiated through the process model to yield a set of transformed samples The predicted mean and covariance are computed as Step 3:The first update, with the process of measurement update, is as follows: The measurement vector is The first predicted measurement vector is The first covariance matrix of the innovations is The first cross covariance matrix between the predicted state estimate errors and innovations is The gain matrix is The updated state estimates are The covariance matrix of errors in the updated state estimates is Step 4:Cubature points generation Update the covariance matrix of error Generate the cubature points Calculate the propagated cubature points Step 5:The second prediction. The predicted mean and covariance are calculated as Step 6:The second update. Factorize Generate the cubature points Calculate the propagated cubature points: The second predicted measurement The second covariance matrix of the innovations Step 7:Compute gain matrix, estimate state, and its covariance matrix secondly: Step 8:Repeat Steps 1 to 7 for the next sample.
4. Quadratic Interpolation-Based Variational Bayesian Filter
4.1. Lagrange Interpolation Polynomial
When the measurement information is lost, the Lagrange interpolation polynomial (LIP) can be used to calculate the predicted observed vector. The QI method is used because the Runge phenomenon needs to be avoided [42, 43].
Assumption 1. (a) The measurement information is lost at the time k. (b) The measurement information is observed from the time 1 to k-1, as the method function in Figure 1.
The QI polynomial is described aswhere , , and .
Hence, can be factored as follows:where is the predicted observed vector at the time k and means the time of k.
(a)
(b)
(c)
4.2. The Variational Bayesian Approach
In order to estimate the state vector with the measurement information lost, the joint PDF is considered to calculate the optimal solutions of , and . However, this PDF cannot be obtained, so the VB method is used to find some independent PDFs , which are used to approximate .
Set . Then, the optimal solutions of , which is satisfied, are as follows:where is the expectation of the except for . is a constant about . In Bayesian statistics theory, the covariance matrix is defined to follow the inverse Wishart (IW) distribution.
The distributions of and are set inwhere is the variable that follows IW distribution with degree of freedom (DOF) and inverse scale matrix. is the variable that follows IW distribution with DOF and inverse scale matrix;
can be rewritten as follows:
Hence, is expended in
The optimal solution is defined as follows:
When choosing , the parameters of are given by usingwhere .
When choosing , the parameters of are given by usingwhere is calculated as follows:
Then, and at l+1th step are calculated as follows, respectively:
When choosing , is modelled as follows:where denotes the variable that follows Gaussian distribution with mean vector and error covariance matrix, and the measurement update is written as follows:where the Kalman gain at the l+1th step is calculated by using (41) at time k.
After fixed-point iteration, the VB approximate of PDF is derived by
The pseudocode is presented in Table 1. If the measurement information vector is lost, the LIP method is used to calculate the predicted observed vector . Then, the predicted state vector and predicted error covariance matrix are calculated by using the state vector , error covariance matrix and process noise matrix . Then, , , , , and are initialized for the subsequent variational Bayesian measurement estimation. The predicted error covariance matrix and the measurement noise matrix at the l+1 th step are calculated in order to estimate the state vector and error covariance matrix at the l+1 th step, respectively. Finally, after th iteration, the accurate state vector and error covariance matrix are determined at the time k.
5. Simulations and Results
The proposed QIVBF is compared with the traditional KF in a simulation with measurement missing scenarios, and the root mean squared error (RMSE) is defined to compare the performances of these algorithms.where and are the predicted position and reference position, respectively. The parameters are set in Table 2.
According to the navigation system, the accuracy of the methods is compared, and the above position estimation errors are analyzed. From Figures 2 and 3, the proposed QIVBF performs closer to the reference than the traditional KF. The proposed QIVBF results in a much better estimation accuracy than the existing filter on handling the problems of underwater navigation in the scenarios of measurement information missing. In particular, KF can easily lose tracking of the reference values by reason that the accumulated errors of iteration. On the other hand, the quadratic interpolation improves the observation vector for the precision and stability of estimation iterations when the environments change, or the measurement information is lost. Therefore, the results demonstrate that the proposed QIVBF outperforms the traditional KF in the condition of measurement information missing.
The navigation accuracy and anti-interference ability of the KF and QIVBF are compared by calculating the root-mean-square error (RMSE) of the position estimation. The RMSE has a good reflection of the measurement precision. It can be seen from Figure 4 that the QIVBF performs better than the traditional KF. When the measurement information is missing, the traditional KF cannot estimate the accurate Kalman gain, which leads to the filter divergence. However, the proposed QIVBF uses the QI method to estimate the lost observed vector, which can help estimate the accurate information about cross error covariance matrix. Then, the VB method can help calculate the accurate state vector and error covariance matrix. The RMSE of QIVBF is lower than KF. Hence, the performance of QIVBF is better than KF.
6. Conclusions
The paper proposes a quadratic interpolation-based variational Bayesian filter for underwater navigation when measurement information is lost, where the quadratic interpolation method is used to calculate the observed vector when the measurement information is lost. Then, the state vector together with the predicted error covariance matrix and the measurement noise matrix is estimated based on the VB method. Simulation results demonstrated the effectiveness of the proposed QIVBF algorithm as compared with the traditional algorithm in the case of measurement information missing. The idea of the proposed QIVBF can be extended to design a nonlinear filter for the non-Gaussian noises with measurement information lost. In future work, we plan to apply multiple datasets of underwater navigation measurements, so that the position and attitude can be estimated based on the fusion of various data sources for better accuracy.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was funded by the projects of the National Natural Science Foundation of China (Grant nos. 62073078 and 61802428).