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.


Introduction
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 [1]. 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 [2]. To improve the navigation accuracy of the INS, the INS/DVL and INS/acoustic positioning system (APS) are usually adopted in underwater navigation [3]. Among these integration solutions, the INS/DVL integrated system is widely used because of its convenient installation and easy maintenance [4].
Due to the complex environment and intrinsic system structure, the INS/DVL integrated system usually faces two major challenges: outlier contamination and nonlinearity [5]. 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. e mainstream approach to resist outliers is the robust filtering technology [8]. 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 [9].
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, χ 2 distribution, and F distribution [10]. 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 [11]. e 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 [12].
Since its inception, Huber's M-estimation has been widespread in the integrated navigation systems to attenuate measurement outliers. In [13], Huber's M-estimation-based iterated divided difference filter was proposed for cooperative localization of autonomous underwater vehicles. In [14], Huber's M-estimation-based process uncertainty robust filter was studied in the INS/GPS integrated system. In [15], Huber's M-estimation was introduced for the variational Bayesian-(VB-) based unscented Kalman filter (UKF) in the integrated navigation system. e study in [16] 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. e main methods are extended Kalman filter (EKF), UKF, cubature Kalman filter (CKF), and particle filter (PF) [17]. In [18], an EKF was employed for the INS/ global navigation satellite system (GNSS)/refreshed simultaneous localization and mapping (SLAM) integrated navigation. In [19], 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 [20], a robust fading CKF was proposed in initial alignment of the strapdown inertial navigation system (SINS). In [21], a square root cubature information filter was proposed for DVL-aided SINS inmotion initial alignment. e 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. e 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. erefore, the CKF is the most accurate and effective method for nonlinear problems in engineering application. eoretically, 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. e 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.

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. is is because the information provided by DVL is commonly the velocity of the vehicle instead of the four channels in DVL.
e coordinate frames are given as follows: i-frame: the Earth-centered initially fixed orthogonal reference frame n-frame: the orthogonal reference frame aligned with east-north-up (ENU) geodetic axes b-frame: the orthogonal reference frame aligned with IMU axes e-frame: the Earth-centered Earth-fixed orthogonal reference frame n ′ -frame: the calculated n-frame with small misalignment errors en, the feedback from the Kalman filter is added to the INS to correct navigation information. e 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.

State Equation.
e state equation of the INS/DVL integrated system is established by the error model of the INS.
e INS error equations of attitude, velocity, and position can be derived as follows [14]: where ϕ is the attitude error, v n is the velocity, L is the latitude, λ is the longitude, h is the height, ω n in is the angular rate of n-frame to i-frame in n-frame, f b is the specific in bframe, C w is the coefficient matrix of the Euler platform error angular differential equation, c is the roll angle, and φ is the heading angle. C n′ n is the direction cosine matrix from n-frame to n'-frame, C n′ b is the direction cosine matrix from b-frame to n'-frame, g n is the gravity, R M is the radius of curvature in meridian, R N is the radius of curvature in prime vertical, v E , v N , and v U are the velocities in the east, north, and upward directions, respectively, δ means the error, ε b is the gyroscope drift, ∇ b is the accelerometer bias, ω n ie is the earth rate vector in n-frame, and ω n en is the angular rate of nframe 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 x k is the state vector, f is the nonlinear function based on equation (1), and w k is the process noise vector. e state vector x k is defined as follows: where K d is the scale factor error of DVL.

Measurement Equation.
Considering the velocity difference of the INS and DVL as the measurement, the measurement equation can be obtained as follows [22]: where v b SINS is the velocity calculated by the INS in b-frame, v b DVL is the velocity measured by DVL, and v b DVL is the true velocity of DVL.
Based on equation (5), the discrete measurement equation can be written as follows: where z k is the measurement vector, H k is the measurement transfer matrix, and v k is the measurement information noise. e measurement transfer matrix H k is as follows: Mathematical Problems in Engineering DVL z are the velocities of DVL in x, y, and z directions.

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 [23]. Considering the measurement model, where y is the measurement, x is the state, H is the measurement matrix, and v is the measurement noise vector. e residual is defined as follows: where x is the estimated state. e maximum likelihood function can be presented as follows: where m is the dimension of the measurement and ζ i is the component of ζ.
In order to calculate the maximum of L(x; y), the objective function is constructed in the logarithm form: In f ζ i . (11) 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 [24]: where where c is the default parameter. Amounts of experiences show that the performance is best when c � 1.345. 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: e system equation can be rewritten as follows: Define the decoupling matrix as follows e measurement equation can be reconstructed as follows: e weighted matrix can be calculated with equations (13) and (17): Finally, the gain matrix and variance matrix of Huber's M-estimation-based Kalman filter are achieved as follows:

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 [25]. e basic steps of the CKF are as follows.

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:

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. e flow chart of the proposed Huber's M-estimation-based cubature Kalman filter is shown in Figure 2  e RCKF is based on the χ 2 distribution, the outlier detection equation is as follows:

Simulation and Vehicle Test
where T is set as 20 empirically. Also, 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. e 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 s there measurement information?
Measurement update If continue the navigation?      Mathematical Problems in Engineering 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. e differences of attitude errors in the KF, RCKF, and MCKF are negligible. e 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. is 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. erefore, it is concluded that the MCKF outperforms the KF and RCKF in navigation accuracy.

Vehicle Test.
e 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. e 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. e vehicle equipment and installation structure    are shown in Figure 9. e 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. e 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. e 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.

Conclusions
To deal with the problems of outliers and nonlinearity in the complex underwater environment, Huber's M-estimationbased 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.
Data Availability e data used in this study are used in other papers and cannot be made public at present.

Conflicts of Interest
e authors declare that there are no conflicts of interest regarding the publication of this article.