Information Fusion for Integrated Navigation SystemsView this Special Issue
Huber’s M-Estimation-Based Cubature Kalman Filter for an INS/DVL Integrated System
To deal with the problems of outliers and nonlinearity in the complex underwater environment, a Huber’s M-estimation-based cubature Kalman filter (CKF) is proposed for an inertial navigation system (INS)/Doppler velocity log (DVL) integrated system. First, a loosely coupled INS/DVL integrated system is designed, and the nonlinear system model is established in the case of big misalignment angle. Then, Huber’s M-estimation is introduced for robust estimation to resist outliers. Meanwhile, the CKF is focused to handle the nonlinearity of the state equation. Finally, simulation and the vehicle test are conducted to evaluate the effectiveness of the proposed method. Results show that the proposed method outperforms the conventional Kalman filter (KF) and outlier detection-based robust cubature Kalman filter (RCKF) in terms of navigation accuracy in the complex underwater environment.
Recently, increasing attention has been given to underwater navigation for ocean exploration. Limited by the signal attenuation, unavailability of the global position system (GPS) makes it difficult to navigate in the underwater environment . As a traditional method, the inertial navigation system (INS) is considered as the main navigation method by virtue of its reliability, autonomy, and convenience. However, the navigation error of the INS accumulates over time . To improve the navigation accuracy of the INS, the INS/DVL and INS/acoustic positioning system (APS) are usually adopted in underwater navigation . Among these integration solutions, the INS/DVL integrated system is widely used because of its convenient installation and easy maintenance .
Due to the complex environment and intrinsic system structure, the INS/DVL integrated system usually faces two major challenges: outlier contamination and nonlinearity . Outliers are few data points deviating significantly from the most data points in the measurement data set. In underwater navigation, outliers probably occur in DVL observations because of the ocean currents and the marine organisms [6, 7]. It must be excluded or will lead to filter divergence. The mainstream approach to resist outliers is the robust filtering technology . Furthermore, the error equation of the INS is nonlinear in the case of a big misalignment angle, which significantly complicates the estimation problem. It is generally handled by the nonlinear filter .
A conventional robust filter can be characterized as an outlier detection method and robust estimation. In the outlier detection method, outliers are excluded according to the relationship of the threshold and the standard difference based on certain hypothesis testing of statistics, such as distribution, distribution, and F distribution . Nevertheless, the observation model, the separability among hypothesis models, the selected test statistics, and the predetermined test thresholds inevitably lead to missed detection, false alert, and wrong exclusion. Contrastively, robust estimation achieves the optimal estimation based on the actual distribution which conforms to the observed data rather than ideal distribution. In this sense, it makes full use of the observation information by minimizing the score function with higher robustness but not simply excluding the doubtful observation information . The common robust estimation method can be divided into several categories: M estimation, MM estimation, median estimation, L1 estimation, Msplit estimation, R estimation, S estimation, least-trimmed squares estimation, and sign-constraint robust least squares estimation. Among these, Huber’s M-estimation has become one of the main robust estimation methods by virtue of its simple calculation and convenience to implement .
Since its inception, Huber’s M-estimation has been widespread in the integrated navigation systems to attenuate measurement outliers. In , Huber’s M-estimation-based iterated divided difference filter was proposed for cooperative localization of autonomous underwater vehicles. In , Huber’s M-estimation-based process uncertainty robust filter was studied in the INS/GPS integrated system. In , Huber’s M-estimation was introduced for the variational Bayesian- (VB-) based unscented Kalman filter (UKF) in the integrated navigation system. The study in  employed Huber’s M-estimation to suppress outliers in integrated navigation.
In the INS/DVL integrated system, the error equation of the INS is nonlinear in the case of the big misalignment angle. Vast research studies are conducted to cope with this problem. The main methods are extended Kalman filter (EKF), UKF, cubature Kalman filter (CKF), and particle filter (PF) . In , an EKF was employed for the INS/global navigation satellite system (GNSS)/refreshed simultaneous localization and mapping (SLAM) integrated navigation. In , an adaptive UKF with process noise covariance estimation is proposed to enhance the UKF robustness against process noise uncertainty for vehicular INS/GPS integration. In , a robust fading CKF was proposed in initial alignment of the strapdown inertial navigation system (SINS). In , a square root cubature information filter was proposed for DVL-aided SINS in-motion initial alignment.
The EKF linearizes the nonlinear function to the form of Taylor series expansion and retains the first-order term to replace the state transfer matrix with the Jacobian matrix. However, estimation error is introduced by the omittance of the higher-order term. Differently, the UKF, CKF, and PF convert the nonlinear estimation to the probability distribution based on the Bayes estimation. UKF applies the unscented transform (UT) to obtain the sigma-point set with a certain sampling strategy and calculates the corresponding mean weight and variance weight to approximate the posterior mean and variance of the nonlinear function. Moreover, based on the third-order spherical-radial volume criterion, the CKF algorithm utilizes a set of volume points to approximate the state mean and covariance of the nonlinear system with the additive white Gaussian noise. The CKF is superior to the UKF for high-dimension state estimation in terms of calculating accuracy and efficiency. Furthermore, based on the Monte Carlo method, the PF employs vast sampling points to approximate nonlinear equations. With the complex calculation, the PF is not common in engineering application. Therefore, the CKF is the most accurate and effective method for nonlinear problems in engineering application.
Theoretically, the optimal estimation of the nonlinear filter is based on the Gaussian distribution assumption. However, at the appearance of outliers, the noise is normally distributed as heavier tail. To solve the problem, Huber’s M-estimation-based cubature Kalman filter (MCKF) is proposed for the INS/DVL integrated system in this paper. Huber’s M-estimation is introduced for robust estimation to resist outliers, such as heavier tail noise. Meanwhile, the CKF is focused to handle the nonlinearity of the state equation.
The structure of this paper is as follows. In Section 2, a loosely coupled INS/DVL integrated system is designed. In Section 3, Huber’s M-estimation is introduced for robust estimation to resist outliers. Meanwhile, the CKF is focused to handle the nonlinearity of the state equation. In Section 4, simulation and the vehicle test are comprehensively conducted to illustrate the superiority of the proposed MCKF method. Section 5 is devoted to conclusion.
2. INS/DVL Integrated System
In the INS/DVL integrated system, the navigation information of the INS and DVL is generally fused using the Kalman filter in the loosely coupled method. This is because the information provided by DVL is commonly the velocity of the vehicle instead of the four channels in DVL.
The coordinate frames are given as follows: -frame: the Earth-centered initially fixed orthogonal reference frame -frame: the orthogonal reference frame aligned with east-north-up (ENU) geodetic axes -frame: the orthogonal reference frame aligned with IMU axes -frame: the Earth-centered Earth-fixed orthogonal reference frame -frame: the calculated n-frame with small misalignment errors
Then, the feedback from the Kalman filter is added to the INS to correct navigation information. The diagram of the INS/DVL integrated system is shown in Figure 1. In this section, the nonlinear state equation derived by the error model of the INS is given and the measurement equation is established with the velocity difference of the INS and DVL in b-frame.
2.1. State Equation
The state equation of the INS/DVL integrated system is established by the error model of the INS. The INS error equations of attitude, velocity, and position can be derived as follows :where is the attitude error, is the velocity, is the latitude, is the longitude, is the height, is the angular rate of n-frame to i-frame in n-frame, is the specific in b-frame, is the coefficient matrix of the Euler platform error angular differential equation, is the roll angle, and is the heading angle. is the direction cosine matrix from n-frame to n’-frame, is the direction cosine matrix from b-frame to n’-frame, is the gravity, is the radius of curvature in meridian, is the radius of curvature in prime vertical, , , and are the velocities in the east, north, and upward directions, respectively, means the error, is the gyroscope drift, is the accelerometer bias, is the earth rate vector in n-frame, and is the angular rate of n-frame to e-frame in n-frame.
Based on equation (1), the discrete nonlinear state equation of the INS/DVL integrated navigation system can be established as follows:where is the state vector, is the nonlinear function based on equation (1), and is the process noise vector. The state vector is defined as follows:where is the scale factor error of DVL.
2.2. Measurement Equation
Considering the velocity difference of the INS and DVL as the measurement, the measurement equation can be obtained as follows :where is the velocity calculated by the INS in b-frame, is the velocity measured by DVL, and is the true velocity of DVL.
Based on equation (5), the discrete measurement equation can be written as follows:where is the measurement vector, is the measurement transfer matrix, and is the measurement information noise. The measurement transfer matrix is as follows:where , , and are the velocities of DVL in x, y, and z directions.
3. Huber’s M-Estimation-Based Cubature Kalman Filter
3.1. Huber’s M-Estimation
In underwater navigation, outliers inevitably occur and influence the navigation results. To address this, Huber’s M-estimation is introduced in this section. A weighted matrix is employed to limit the influences of the outliers to the system and make full use of the measurement information .
Considering the measurement model,where is the measurement, is the state, is the measurement matrix, and is the measurement noise vector.
The residual is defined as follows:where is the estimated state.
The maximum likelihood function can be presented as follows:where is the dimension of the measurement and is the component of .
In order to calculate the maximum of , the objective function is constructed in the logarithm form:
Derivation of the objective function is obtained as follows:
It is supposed that
Accordingly, it can be achieved that
Accordingly, the equation to calculate the maximum likelihood function can be written as follows:where is the weight matrix.
Using the weighted iteration method, the solution of this implicit function equation can be reached as follows:
For further applications, based on the generalized maximum likelihood estimation, a more general object function is given by Huber :wherewhere is the default parameter. Amounts of experiences show that the performance is best when .
Similarly, the solution of the generalized maximum likelihood function can be calculated according to equation (16).
Huber’s generalized maximum likelihood estimation is based on that the residual is independently identically distributed. Considering the residual of the integrated navigation system, a pseudo state vector is constructed as follows:
The system equation can be rewritten as follows:
Define the decoupling matrix as follows
The measurement equation can be reconstructed as follows:
Finally, the gain matrix and variance matrix of Huber’s M-estimation-based Kalman filter are achieved as follows:
3.2. Cubature Kalman Filter
To solve the nonlinear problem caused by the INS error equations in the case of the big misalignment angle, the CKF is studied in the section. It regards the estimation of the nonlinear equation as the estimation of the probability distribution based on the Bayes estimation, which extremely simplifies the nonlinear problem . The basic steps of the CKF are as follows.
3.2.1. Time Update
Step 1. Cholesky decomposition of the covariance matrix: Step 2. Calculation of the cubature point: Step 3. One-step update of the cubature point: Step 4. Prediction of the state with the weighted cubature point: Step 5. Prediction of the covariance matrix:
3.2.2. Measurement Update
Step 6. Transformation of the cubature point: Step 7. Prediction of the measurement with the weighted cubature point: Step 8. Calculation of the autocorrelation covariance: Step 9. Calculation of the cross-correlation covariance:
Step 10. Calculation of the gain matrix: Step 11. Estimation of the state: Step 12. Estimation of the covariance:
In the INS/DVL integrated navigation system, the measurement equation is linear. In this sense, the measurement update can be achieved similarly with the conventional Kalman filter. The flow chart of the proposed Huber’s M-estimation-based cubature Kalman filter is shown in Figure 2.
4. Simulation and Vehicle Test
The performances of the INS/DVL integrated system with Huber’s M-estimation-based cubature Kalman filter are evaluated based on simulation and the vehicle test. To illustrate the ability to deal with the nonlinearity and outliers, it is compared with the conventional Kalman filter (KF) and the robust cubature Kalman filter (RCKF) based on the outlier detection.
The RCKF is based on the distribution, the outlier detection equation is as follows:where T is set as 20 empirically. Also,
A trajectory lasting for about 3000 s is simulated, and the INS/DVL integrated system is applied to navigate. The simulated trajectory is shown in Figure 3. The start point is set as latitude and longitude . The initial attitude is set as [0°, 0°, −90°]. The initial attitude error is set as [3°, 3°, 30°]. The constant bias and the random walk noise of the accelerometer are set as and , and those of the gyroscope are set as and 0.01°/. The DVL scale factor error is set as 0.5%. The update rates of inertial measurement unit (IMU) and DVL are set as 200 Hz and 1 Hz, respectively.
In order to simulate the adverse effects of the underwater complex environment on DVL output, the outliers are generated in 600 s to 900 s, 1200 s to 1600 s, and 2500 s to 2800 s as shown in Figure 4. In other time, the noise is zero mean white noise with a standard deviation of 0.1 m/s. The comparisons of the attitude, velocity, and position errors of the three methods are shown in Figures 5–7, where the black line, blue line, and red line represent the KF, RCKF, and proposed MCKF, respectively. When outliers occur, the conventional KF immediately diverges. Conversely, with the robust estimator and cubature Kalman filter, the RCKF and MCKF are relatively stable.
In order to intuitively compare the performances of the three methods, the horizontal position errors of the KF, RCKF, and proposed MCKF are shown in Figure 8. It can be seen that the horizontal position error of the MCKF is more stable and smaller than that of others.
Quantitative analysis is carried out for navigation errors via the mean and root mean square (RMS). From 500 s to 3000 s, the mean and RMS of the attitude, velocity, and position errors are shown in Tables 1 and 2. It can be found that the mean and RMS of velocity and position errors in the MCKF are smaller than those of others. The differences of attitude errors in the KF, RCKF, and MCKF are negligible. The mean and RMS of horizontal position errors in the three methods are shown in Table 3. It is also obvious that the mean and RMS of horizontal position errors in the MCKF is smaller than those of others. This is because with Huber’s M-estimation, the system is stable when the noise occurs and with the CKF to handle the nonlinearity of the state equation, the system is more accurate. Therefore, it is concluded that the MCKF outperforms the KF and RCKF in navigation accuracy.
4.2. Vehicle Test
The proposed method is evaluated in the land vehicle field test to predict the feasibility in underwater environments. In the INS/DVL navigation system of the vehicle test, the inertial information is provided by IMU and the velocity information of DVL is replaced by PHINS which is produced by IXBlue Corporation of France. PHINS is combined with the GPS receiver and works on the INS/GPS integrated navigation mode. The velocity in PHINS is transformed from n-frame to b-frame using the true attitude information to substitute the information of DVL. A computer is utilized to perform a series of navigation operations. The vehicle equipment and installation structure are shown in Figure 9. The specifications of IMU and PHINS are listed in Table 4.
A land trial lasting for 3000 s is conducted near 31° 88′N, 118° 82′E, on the campus of Southeast University. The vehicle trajectory is shown in Figure 10.
Similarly, the outliers are generated in 600 s to 900 s, 1200 s to 1600 s, and 2500 s to 2800 s as shown in Figure 11. The attitude, velocity, and position errors of the three methods are shown in Figures 12–14. Meanwhile, horizontal position errors are shown in Figure 15. Consistent with the simulation results, the errors of the MCKF method are the most stable and least in the three methods that effectively verify the feasibility of the proposed method.
Simultaneously, quantitative analysis is performed. From 500 s to 3000 s, the mean and RMS of attitude, velocity, individual position, and horizontal position errors are shown in Tables 5–7. It can be found that the mean and RMS of all errors for the MCKF are smaller than those of the others.
To deal with the problems of outliers and nonlinearity in the complex underwater environment, Huber’s M-estimation-based CKF is proposed for the INS/DVL integrated system. Huber’s M-estimation is introduced for robust estimation at the appearance of outliers. A weighted matrix is employed to limit the influences of outliers to the system and make full use of the measurement information. Meanwhile, the CKF is employed to handle the nonlinearity of the state equation. It regards the estimation of the nonlinear equation as the estimation of the probability distribution based on the Bayes estimation. Finally, simulation and the vehicle test show that the proposed method outperforms the conventional KF method and outlier detection-based RCKF method in terms of navigation accuracy in the complex underwater environment.
The data used in this study are used in other papers and cannot be made public at present.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this article.
This research was funded in part by the Jiangsu Key Laboratory Fund for Green Ship Technology under Grant no. 2019Z01 and in part by the Fundamental Research Funds for the Central Universities under Grant nos. 2242019K40040 and 2242020k30043.
J. Wu, X. Xu, T. Zhang et al., “A misalignment angle error calibration method of underwater acoustic array in Strapdown Inertial Navigation System/Ultra Short Baseline integrated navigation system based on single transponder mode,” Review of Scientific Instruments, vol. 90, no. 8, Article ID 085001, p. 19, 2019.View at: Google Scholar