Huber’s M-Estimation-Based Cubature Kalman Filter for an INS/DVL Integrated System

Li, Yao; Hou, Lanhua; Yang, Yang; Tong, Jinwu

doi:https://doi.org/10.1155/2020/1060672

Mathematical Problems in Engineering

On this page

Abstract Introduction Conclusions Data Availability Conflicts of Interest Acknowledgments References Copyright Related Articles

Special Issue

Information Fusion for Integrated Navigation Systems

View this Special Issue

Research Article | Open Access

Volume 2020 | Article ID 1060672 | https://doi.org/10.1155/2020/1060672

Huber’s M-Estimation-Based Cubature Kalman Filter for an INS/DVL Integrated System

Yao Li,^1,2Lanhua Hou,^1,2Yang Yang,^1,2and Jinwu Tong³

Academic Editor: Gaoge Hu

Received04 Aug 2020

Revised02 Sept 2020

Accepted05 Sept 2020

Published26 Sept 2020

Abstract

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.

1. 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. The 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, 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]. 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 [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. The 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. The 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 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 [14]: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 [22]: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 [23].

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 [24]: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:

The 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:

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 [25]. 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,

4.1. Simulation

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)

(b)

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.

5. Conclusions

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.

Data Availability

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.

Acknowledgments

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.

References

J. Wang, T. Zhang, B. Jin, Y. Zhu, and J. Tong, “Student’s t-based robust kalman filter for a SINS/USBL integration navigation strategy,” IEEE Sensors Journal, vol. 20, no. 10, pp. 5540–5553, 2020.
View at: Publisher Site | Google Scholar
G. Hu, W. Wang, Y. Zhong, B. Gao, and C. Gu, “A new direct filtering approach to INS/GNSS integration,” Aerospace Science and Technology, vol. 77, pp. 755–764, 2018.
View at: Publisher Site | Google Scholar
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
Y. Yao, X. Xu, Y. Li, and T. Zhang, “A hybrid IMM based INS/DVL integration solution for underwater vehicles,” IEEE Transactions on Vehicular Technology, vol. 68, no. 6, pp. 5459–5470, 2019.
View at: Publisher Site | Google Scholar
J. González-García, A. Gómez-Espinosa, E. Cuan-Urquizo et al., “Autonomous underwater vehicles: localization, navigation, and communication for collaborative missions,” Applied Sciences, vol. 10, no. 4, Article ID 1256, p. 37, 2020.
View at: Publisher Site | Google Scholar
T. Matsuda, T. Maki, T. Sakamaki, and T. Ura, “Performance analysis on a navigation method of multiple AUVs for wide area survey,” Marine Technology Society Journal, vol. 46, no. 2, pp. 45–55, 2012.
View at: Publisher Site | Google Scholar
T. Li, H. Zhang, Z. Gao, Q. Chen, and X. Niu, “High-accuracy positioning in urban environments using single-frequency multi-GNSS RTK/MEMS-IMU integration,” Remote Sensing, vol. 10, no. 2, Article ID 205, p. 21, 2018.
View at: Publisher Site | Google Scholar
L. Yang, Y. Shen, and B. Li, “M-estimation using unbiased median variance estimate,” Journal of Geodesy, vol. 93, no. 6, pp. 911–925, 2018.
View at: Publisher Site | Google Scholar
G. Hu, L. Ni, B. Gao, X. Zhu, W. Wang, and Y. Zhong, “Model predictive based unscented kalman filter for hypersonic vehicle navigation with INS/GNSS integration,” IEEE Access, vol. 8, pp. 4814–4823, 2020.
View at: Publisher Site | Google Scholar
N. L. Knight and J. Wang, “A comparison of outlier detection procedures and robust estimation methods in GPS positioning,” Journal of Navigation, vol. 62, no. 4, pp. 699–709, 2009.
View at: Publisher Site | Google Scholar
K. Li, B. Hu, L. Chang, and Y. Li, “Robust square-root cubature Kalman filter based on Huber’s M-estimation methodology,” Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, vol. 229, no. 7, pp. 1236–1245, 2014.
View at: Publisher Site | Google Scholar
L. Yang and Y. Shen, “Robust M estimation for 3D correlated vector observations based on modified bifactor weight reduction model,” Journal of Geodesy, vol. 94, no. 3, pp. 1–17, 2020.
View at: Publisher Site | Google Scholar
W. Gao, Y. Liu, and B. Xu, “Robust Huber-based iterated divided difference filtering with application to cooperative localization of autonomous underwater vehicles,” Sensors, vol. 14, no. 12, pp. 24523–24542, 2014.
View at: Publisher Site | Google Scholar
L. Chang, K. Li, and B. Hu, “Huber’s M-estimation-based process uncertainty robust filter for integrated INS/GPS,” IEEE Sensors Journal, vol. 15, no. 6, pp. 3367–3374, 2015.
View at: Publisher Site | Google Scholar
K. Li, L. Chang, and B. Hu, “A variational bayesian-based unscented kalman filter with both adaptivity and robustness,” IEEE Sensors Journal, vol. 16, no. 18, pp. 6966–6976, 2016.
View at: Publisher Site | Google Scholar
X. Xu, X. Xu, Y. Yao, and Z. Wang, “In-motion coarse alignment method based on reconstructed observation vectors,” Rev Sci Instrum, vol. 88, no. 3, Article ID 035001, p. 12, 2017.
View at: Publisher Site | Google Scholar
C.-H. Tseng, S.-F. Lin, and D.-J. Jwo, “Robust huber-based cubature kalman filter for GPS navigation processing,” Journal of Navigation, vol. 70, no. 3, pp. 527–546, 2017.
View at: Publisher Site | Google Scholar
K.-W. Chiang, G.-J. Tsai, H.-J. Chu, and N. El-Sheimy, “Performance enhancement of INS/GNSS/Refreshed-SLAM integration for acceptable lane-level navigation accuracy,” IEEE Transactions on Vehicular Technology, vol. 69, no. 3, pp. 2463–2476, 2020.
View at: Publisher Site | Google Scholar
G. Hu, B. Gao, Y. Zhong, and C. Gu, “Unscented kalman filter with process noise covariance estimation for vehicular INS/GPS integration system,” Information Fusion, vol. 64, pp. 194–204, 2020.
View at: Publisher Site | Google Scholar
S. Guo, L. Chang, Y. Li, and Y. Sun, “Robust fading cubature Kalman filter and its application in initial alignment of SINS,” Optik, vol. 202, Article ID 163593, p. 10, 2020.
View at: Publisher Site | Google Scholar
L. Zhang, W. Wu, M. Wang, and Y. Guo, “DVL-aided SINS in-motion alignment filter based on a novel nonlinear attitude error model,” IEEE Access, vol. 7, pp. 62457–62464, 2019.
View at: Publisher Site | Google Scholar
A. Tal, I. Klein, and R. Katz, “Inertial navigation system/Doppler velocity log (INS/DVL) fusion with partial DVL measurements,” Sensors (Basel), vol. 17, no. 2, Article ID 415, p. 20, 2017.
View at: Publisher Site | Google Scholar
T. Yang, C. M. Gallagher, and C. S. McMahan, “A robust regression methodology via M-estimation,” Communications in Statistics - Theory and Methods, vol. 48, no. 5, pp. 1092–1107, 2019.
View at: Publisher Site | Google Scholar
P. J. Huber, “Robust regression: asymptotics, conjectures and Monte Carlo,” The Annals of Statistics, vol. 1, no. 5, pp. 799–821, 1973.
View at: Publisher Site | Google Scholar
X. Zhao, J. Li, X. Yan, and S. Ji, “Robust adaptive cubature kalman filter and its application to ultra-tightly coupled SINS/GPS navigation system,” Sensors, vol. 18, no. 7, Article ID 2352, p. 19, 2018.
View at: Publisher Site | Google Scholar

Copyright

Copyright © 2020 Yao Li et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

PDF Download Citation

Download other formats

Order printed copies

Views

1148

Downloads

875

Citations