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

Authors: Yao Li; Lanhua Hou; Yang Yang; Jinwu Tong
Journal: Mathematical Problems in Engineering
 (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.


---

## Body

## 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, χ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]. 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. InSection 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:i-frame: the Earth-centered initially fixed orthogonal reference framen-frame: the orthogonal reference frame aligned with east-north-up (ENU) geodetic axesb-frame: the orthogonal reference frame aligned with IMU axese-frame: the Earth-centered Earth-fixed orthogonal reference framen′-frame: the calculated n-frame with small misalignment errorsThen, 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 Figure1. 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.Figure 1
Diagram of the INS/DVL integrated system.


### 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]:(1)ϕ˙=Cw−1I−Cnn′ω˜inn+Cnn′δωinn−Cbn′εb,δv˙n=I−Cnn′TCbn′f˜b+Cnn′TCbn′∇b−2δωien+δωenn×vn−2ωien+ωenn×δvn+δgn,δL˙=1RM+hδvN−vNRM+h2δh,δλ˙=secLRN+hδvE+vEsecLtanLRN+hδL−vERN+h2δh,δh˙=δvU,(2)Cw=cosγ0−sinγcosφ01sinφsinγ0cosγcosφ,where ϕ is the attitude error, vn is the velocity, L is the latitude, λ is the longitude, h is the height, ω˜inn is the angular rate of n-frame to i-frame in n-frame, f˜b is the specific in b-frame, Cw is the coefficient matrix of the Euler platform error angular differential equation, γ is the roll angle, and φ is the heading angle. Cnn′ is the direction cosine matrix from n-frame to n’-frame, Cbn′ is the direction cosine matrix from b-frame to n’-frame, gn is the gravity, RM is the radius of curvature in meridian, RN is the radius of curvature in prime vertical, vE, vN, and vU are the velocities in the east, north, and upward directions, respectively, δ means the error, εb is the gyroscope drift, ∇b is the accelerometer bias, ωien is the earth rate vector in n-frame, and ωenn 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:(3)xk+1=fxk+wk,where xk is the state vector, f is the nonlinear function based on equation (1), and wk is the process noise vector. The state vector xk is defined as follows:(4)xk=ϕxϕyϕzδVEnδVNnδVUnδλδLδhεxεyεz∇x∇y∇zKdT,where Kd 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]:(5)z=v^SINSb−v^DVLb=CnbCn′nvn+δvn−vDVLb1+Kd≈Cnbϕ×vn+Cnbδvn−KdvDVLb,where v^SINSb is the velocity calculated by the INS in b-frame, v^DVLb is the velocity measured by DVL, and vDVLbis the true velocity of DVL.Based on equation (5), the discrete measurement equation can be written as follows:(6)zk=Hkxk+vk,where zk is the measurement vector, Hk is the measurement transfer matrix, and vk is the measurement information noise. The measurement transfer matrix Hk is as follows:(7)Hk=C31vN−C21vUC11vU−C31vEC21vE−C11vN−vDVL_xbC32vN−C22vUC12vU−C32vEC22vE−C12vNCnb03×9−vDVL_ybC33vN−C23vUC13vU−C33vEC23vE−C13vN−vDVL_zb,where vDVL_xb, vDVL_yb, and vDVL_zbare the velocities of DVL in x, y, and z directions.


## 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]:(1)ϕ˙=Cw−1I−Cnn′ω˜inn+Cnn′δωinn−Cbn′εb,δv˙n=I−Cnn′TCbn′f˜b+Cnn′TCbn′∇b−2δωien+δωenn×vn−2ωien+ωenn×δvn+δgn,δL˙=1RM+hδvN−vNRM+h2δh,δλ˙=secLRN+hδvE+vEsecLtanLRN+hδL−vERN+h2δh,δh˙=δvU,(2)Cw=cosγ0−sinγcosφ01sinφsinγ0cosγcosφ,where ϕ is the attitude error, vn is the velocity, L is the latitude, λ is the longitude, h is the height, ω˜inn is the angular rate of n-frame to i-frame in n-frame, f˜b is the specific in b-frame, Cw is the coefficient matrix of the Euler platform error angular differential equation, γ is the roll angle, and φ is the heading angle. Cnn′ is the direction cosine matrix from n-frame to n’-frame, Cbn′ is the direction cosine matrix from b-frame to n’-frame, gn is the gravity, RM is the radius of curvature in meridian, RN is the radius of curvature in prime vertical, vE, vN, and vU are the velocities in the east, north, and upward directions, respectively, δ means the error, εb is the gyroscope drift, ∇b is the accelerometer bias, ωien is the earth rate vector in n-frame, and ωenn 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:(3)xk+1=fxk+wk,where xk is the state vector, f is the nonlinear function based on equation (1), and wk is the process noise vector. The state vector xk is defined as follows:(4)xk=ϕxϕyϕzδVEnδVNnδVUnδλδLδhεxεyεz∇x∇y∇zKdT,where Kd 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]:(5)z=v^SINSb−v^DVLb=CnbCn′nvn+δvn−vDVLb1+Kd≈Cnbϕ×vn+Cnbδvn−KdvDVLb,where v^SINSb is the velocity calculated by the INS in b-frame, v^DVLb is the velocity measured by DVL, and vDVLbis the true velocity of DVL.Based on equation (5), the discrete measurement equation can be written as follows:(6)zk=Hkxk+vk,where zk is the measurement vector, Hk is the measurement transfer matrix, and vk is the measurement information noise. The measurement transfer matrix Hk is as follows:(7)Hk=C31vN−C21vUC11vU−C31vEC21vE−C11vN−vDVL_xbC32vN−C22vUC12vU−C32vEC22vE−C12vNCnb03×9−vDVL_ybC33vN−C23vUC13vU−C33vEC23vE−C13vN−vDVL_zb,where vDVL_xb, vDVL_yb, and vDVL_zbare 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,(8)y=Hx+v,where y is the measurement, x is the state, H is the measurement matrix, and v is the measurement noise vector.The residual is defined as follows:(9)ζ=Hx^−y,where x^ is the estimated state.The maximum likelihood function can be presented as follows:(10)Lx;y=∏i=1mfζi,where m is the dimension of the measurement and ζi is the component of ζ.In order to calculate the maximum ofLx;y, the objective function is constructed in the logarithm form:(11)Jx=−∑i=1mInfζi.Derivation of the objective function is obtained as follows:(12)∑i=1m−f′ζifζi∂ζi∂x=0.It is supposed that(13)ψζi=−f′ζifζiζi.Accordingly, it can be achieved that(14)∑i=1mψζiζi∂ζi∂x=0,∑i=1mψζiζi∂Hix−yi∂x=0,∑i=1mψζiζiHi=0.Accordingly, the equation to calculate the maximum likelihood function can be written as follows:(15)HTψHx−y=0,where ψ is the weight matrix.Using the weighted iteration method, the solution of this implicit function equation can be reached as follows:(16)x0=HTH−1HTy,j=0,xj+1=HTψjH−1HTψjy,j≠0.For further applications, based on the generalized maximum likelihood estimation, a more general object function is given by Huber [24]:(17)Jx=∑i=1mρζi,where(18)ρζi=ζi22,ζi<γ,γζi−ζi22,ζi≥γ,where γ is the default parameter. Amounts of experiences show that the performance is best when γ=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:(19)xk,k−1=xk−δk.The system equation can be rewritten as follows:(20)ykxk,k−1=HkIxk+vk−δk.Define the decoupling matrix as follows(21)Sk=Rk00Pk,k−1,zk=Sk−1/2ykxk,k−1,Mk=Sk−1/2HkI,ζk=Sk−1/2vk−δk.The measurement equation can be reconstructed as follows:(22)zk=Mkxk+ζk.The weighted matrix can be calculated with equations (13) and (17):(23)ψ=ψyψx.Finally, the gain matrix and variance matrix of Huber’s M-estimation-based Kalman filter are achieved as follows:(24)P⌣kk−1=Pk|k−11/2ψx−1Pk|k−1T/2,R⌣k=Rk1/2ψy−1RkT/2,Kk=P⌣kk−1HkTHkP⌣kk−1HkT+R⌣k−1,Pk=I−KkHkP⌣kk−1.


### 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:(25)Pk−1/k−1=Sk−1/k−1Sk−1/k−1T.Step 2. Calculation of the cubature point:(26)Xi,k−1/k−1=Sk−1/k−1ξi+x^k−1/k−1.Step 3. One-step update of the cubature point:(27)Xi,k−1/k−1∗=fXi,k−1/k−1.Step 4. Prediction of the state with the weighted cubature point:(28)x^k/k−1=12n∑i=12nXi,k/k−1∗.Step 5. Prediction of the covariance matrix:(29)Pk/k−1=1m∑i=1mXi,k/k−1∗Xi,k/k−1∗T−x^k/k−1x^k/k−1T+Qk−1.


#### 3.2.2. Measurement Update

Step 6. Transformation of the cubature point:(30)Zi,k/k−1=hXi,k/k−1.Step 7. Prediction of the measurement with the weighted cubature point:(31)z^k/k−1=1m∑i=1mZi,k/k−1.Step 8. Calculation of the autocorrelation covariance:(32)Pzz,k/k−1=12n∑i=12nZi,k/k−1Zi,k/k−1T−Z^k/k−1Z^k/k−1T+Rk.Step 9. Calculation of the cross-correlation covariance:(33)Pxz,k/k−1=12n∑i=12nXi,k/k−1Zi,k/k−1T−x^k/k−1z^k/k−1T.Step 10. Calculation of the gain matrix:(34)Kk=Pxz,k/k−1Pzz,k/k−1−1.Step 11. Estimation of the state:(35)x^k/k=x^k/k−1+Kkzk−z^k/k−1.Step 12. Estimation of the covariance:(36)Pk/k=Pk/k−1+KkPzz,k/k−1KkT.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 Figure2.Figure 2
The flow chart of the MCKF.


## 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,(8)y=Hx+v,where y is the measurement, x is the state, H is the measurement matrix, and v is the measurement noise vector.The residual is defined as follows:(9)ζ=Hx^−y,where x^ is the estimated state.The maximum likelihood function can be presented as follows:(10)Lx;y=∏i=1mfζi,where m is the dimension of the measurement and ζi is the component of ζ.In order to calculate the maximum ofLx;y, the objective function is constructed in the logarithm form:(11)Jx=−∑i=1mInfζi.Derivation of the objective function is obtained as follows:(12)∑i=1m−f′ζifζi∂ζi∂x=0.It is supposed that(13)ψζi=−f′ζifζiζi.Accordingly, it can be achieved that(14)∑i=1mψζiζi∂ζi∂x=0,∑i=1mψζiζi∂Hix−yi∂x=0,∑i=1mψζiζiHi=0.Accordingly, the equation to calculate the maximum likelihood function can be written as follows:(15)HTψHx−y=0,where ψ is the weight matrix.Using the weighted iteration method, the solution of this implicit function equation can be reached as follows:(16)x0=HTH−1HTy,j=0,xj+1=HTψjH−1HTψjy,j≠0.For further applications, based on the generalized maximum likelihood estimation, a more general object function is given by Huber [24]:(17)Jx=∑i=1mρζi,where(18)ρζi=ζi22,ζi<γ,γζi−ζi22,ζi≥γ,where γ is the default parameter. Amounts of experiences show that the performance is best when γ=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:(19)xk,k−1=xk−δk.The system equation can be rewritten as follows:(20)ykxk,k−1=HkIxk+vk−δk.Define the decoupling matrix as follows(21)Sk=Rk00Pk,k−1,zk=Sk−1/2ykxk,k−1,Mk=Sk−1/2HkI,ζk=Sk−1/2vk−δk.The measurement equation can be reconstructed as follows:(22)zk=Mkxk+ζk.The weighted matrix can be calculated with equations (13) and (17):(23)ψ=ψyψx.Finally, the gain matrix and variance matrix of Huber’s M-estimation-based Kalman filter are achieved as follows:(24)P⌣kk−1=Pk|k−11/2ψx−1Pk|k−1T/2,R⌣k=Rk1/2ψy−1RkT/2,Kk=P⌣kk−1HkTHkP⌣kk−1HkT+R⌣k−1,Pk=I−KkHkP⌣kk−1.


## 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:(25)Pk−1/k−1=Sk−1/k−1Sk−1/k−1T.Step 2. Calculation of the cubature point:(26)Xi,k−1/k−1=Sk−1/k−1ξi+x^k−1/k−1.Step 3. One-step update of the cubature point:(27)Xi,k−1/k−1∗=fXi,k−1/k−1.Step 4. Prediction of the state with the weighted cubature point:(28)x^k/k−1=12n∑i=12nXi,k/k−1∗.Step 5. Prediction of the covariance matrix:(29)Pk/k−1=1m∑i=1mXi,k/k−1∗Xi,k/k−1∗T−x^k/k−1x^k/k−1T+Qk−1.


### 3.2.2. Measurement Update

Step 6. Transformation of the cubature point:(30)Zi,k/k−1=hXi,k/k−1.Step 7. Prediction of the measurement with the weighted cubature point:(31)z^k/k−1=1m∑i=1mZi,k/k−1.Step 8. Calculation of the autocorrelation covariance:(32)Pzz,k/k−1=12n∑i=12nZi,k/k−1Zi,k/k−1T−Z^k/k−1Z^k/k−1T+Rk.Step 9. Calculation of the cross-correlation covariance:(33)Pxz,k/k−1=12n∑i=12nXi,k/k−1Zi,k/k−1T−x^k/k−1z^k/k−1T.Step 10. Calculation of the gain matrix:(34)Kk=Pxz,k/k−1Pzz,k/k−1−1.Step 11. Estimation of the state:(35)x^k/k=x^k/k−1+Kkzk−z^k/k−1.Step 12. Estimation of the covariance:(36)Pk/k=Pk/k−1+KkPzz,k/k−1KkT.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 Figure2.Figure 2
The flow chart of the MCKF.


## 3.2.1. Time Update

Step 1. Cholesky decomposition of the covariance matrix:(25)Pk−1/k−1=Sk−1/k−1Sk−1/k−1T.Step 2. Calculation of the cubature point:(26)Xi,k−1/k−1=Sk−1/k−1ξi+x^k−1/k−1.Step 3. One-step update of the cubature point:(27)Xi,k−1/k−1∗=fXi,k−1/k−1.Step 4. Prediction of the state with the weighted cubature point:(28)x^k/k−1=12n∑i=12nXi,k/k−1∗.Step 5. Prediction of the covariance matrix:(29)Pk/k−1=1m∑i=1mXi,k/k−1∗Xi,k/k−1∗T−x^k/k−1x^k/k−1T+Qk−1.


## 3.2.2. Measurement Update

Step 6. Transformation of the cubature point:(30)Zi,k/k−1=hXi,k/k−1.Step 7. Prediction of the measurement with the weighted cubature point:(31)z^k/k−1=1m∑i=1mZi,k/k−1.Step 8. Calculation of the autocorrelation covariance:(32)Pzz,k/k−1=12n∑i=12nZi,k/k−1Zi,k/k−1T−Z^k/k−1Z^k/k−1T+Rk.Step 9. Calculation of the cross-correlation covariance:(33)Pxz,k/k−1=12n∑i=12nXi,k/k−1Zi,k/k−1T−x^k/k−1z^k/k−1T.Step 10. Calculation of the gain matrix:(34)Kk=Pxz,k/k−1Pzz,k/k−1−1.Step 11. Estimation of the state:(35)x^k/k=x^k/k−1+Kkzk−z^k/k−1.Step 12. Estimation of the covariance:(36)Pk/k=Pk/k−1+KkPzz,k/k−1KkT.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 Figure2.Figure 2
The flow chart of the MCKF.


## 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χ2 distribution, the outlier detection equation is as follows:(37)IFrkTAk−1rk>TTHEN There is outlier,IFrkTAk−1rk<TTHEN There is no outlier,where T is set as 20 empirically. Also,(38)rk=Zk−HkX^k,k−1,Ak=HkPk,k−1HkT+Rk.


### 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 Figure3. The start point is set as latitude 34°N and longitude 108°E. 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 50μg and 50μg/Hz, and those of the gyroscope are set as0.01°/h and 0.01°/√h. 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.Figure 3
Trajectory of the vehicle.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 Figure4. 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.Figure 4
Velocity information of DVL.Figure 5
Comparison of attitude errors.Figure 6
Comparison of velocity errors.Figure 7
Comparison of position errors.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 Figure8. It can be seen that the horizontal position error of the MCKF is more stable and smaller than that of others.Figure 8
Comparison of horizontal position errors.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 Tables1 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.Table 1
Mean of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0046930.0040460.1490950.0695770.0639260.05348513.62650326.22470154.782361RCKF0.0028370.0026790.0915910.0443870.0382620.00933510.17610311.8710162.485870MCKF0.0017700.0015010.0900680.0188870.0134750.0070065.7525778.8367242.479304Table 2
RMS of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0065430.0057520.2688020.0951140.0965300.06475418.67226029.43227860.497953RCKF0.0033140.0032620.1357600.0500720.0455460.01165311.95943512.4037722.807491MCKF0.0020690.0018180.1340400.0271380.0192050.0087919.8088249.6006012.714385Table 3
Mean and RMS of the horizontal position errors in three methods.
MethodKFRCKFMCKFMean(m)31.42547216.38285811.855896RMS(m)34.85559217.23025313.725325


### 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 fromn-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.Figure 9
Vehicle equipment and installation structure.
(a)(b)Table 4
Specifications of IMU and PHINS.
SensorParametersAccuracyRate (Hz)IMUGyroscope constant drift≤0.02°/h200Gyroscope random walk noise≤0.005°/h200Accelerometer constant bias±50ug200Accelerometer random walk noise≤50ug/Hz200PHINSAttitude (GPS-aided mode)≤0.01°200A 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 Figure10.Figure 10
Trajectory of the vehicle.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 Figure11. 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.Figure 11
Velocity information of DVL.Figure 12
Comparison of attitude errors in the vehicle test.Figure 13
Comparison of velocity errors in the vehicle test.Figure 14
Comparison of position errors in the vehicle test.Figure 15
Comparison of horizontal position errors in the vehicle test.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 Tables5–7. It can be found that the mean and RMS of all errors for the MCKF are smaller than those of the others.Table 5
Mean of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0532060.0505380.0844510.0478380.0587140.06099058.0398317.21291512.061803RCKF0.0520090.0500920.0819700.0153090.0307790.0304927.8634413.6873101.131346MCKF0.0517480.0498730.0808710.0123430.0135100.0204845.9954872.7175091.097752Table 6
RMS of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0567480.0541190.1235100.0607040.0922250.06545764.2677768.99905015.949531RCKF0.0557700.0539080.0845490.0197510.0355550.0344209.1483014.3326631.426899MCKF0.0554410.0535780.0822190.0170430.0173430.0239717.3645213.5423591.215638Table 7
Mean and RMS of the horizontal position errors in three methods.
MethodKFRCKFMCKFMean(m)58.9610748.7992956.696143RMS(m)64.89476110.1224208.172177


## 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 Figure3. The start point is set as latitude 34°N and longitude 108°E. 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 50μg and 50μg/Hz, and those of the gyroscope are set as0.01°/h and 0.01°/√h. 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.Figure 3
Trajectory of the vehicle.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 Figure4. 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.Figure 4
Velocity information of DVL.Figure 5
Comparison of attitude errors.Figure 6
Comparison of velocity errors.Figure 7
Comparison of position errors.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 Figure8. It can be seen that the horizontal position error of the MCKF is more stable and smaller than that of others.Figure 8
Comparison of horizontal position errors.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 Tables1 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.Table 1
Mean of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0046930.0040460.1490950.0695770.0639260.05348513.62650326.22470154.782361RCKF0.0028370.0026790.0915910.0443870.0382620.00933510.17610311.8710162.485870MCKF0.0017700.0015010.0900680.0188870.0134750.0070065.7525778.8367242.479304Table 2
RMS of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0065430.0057520.2688020.0951140.0965300.06475418.67226029.43227860.497953RCKF0.0033140.0032620.1357600.0500720.0455460.01165311.95943512.4037722.807491MCKF0.0020690.0018180.1340400.0271380.0192050.0087919.8088249.6006012.714385Table 3
Mean and RMS of the horizontal position errors in three methods.
MethodKFRCKFMCKFMean(m)31.42547216.38285811.855896RMS(m)34.85559217.23025313.725325


## 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 fromn-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.Figure 9
Vehicle equipment and installation structure.
(a)(b)Table 4
Specifications of IMU and PHINS.
SensorParametersAccuracyRate (Hz)IMUGyroscope constant drift≤0.02°/h200Gyroscope random walk noise≤0.005°/h200Accelerometer constant bias±50ug200Accelerometer random walk noise≤50ug/Hz200PHINSAttitude (GPS-aided mode)≤0.01°200A 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 Figure10.Figure 10
Trajectory of the vehicle.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 Figure11. 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.Figure 11
Velocity information of DVL.Figure 12
Comparison of attitude errors in the vehicle test.Figure 13
Comparison of velocity errors in the vehicle test.Figure 14
Comparison of position errors in the vehicle test.Figure 15
Comparison of horizontal position errors in the vehicle test.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 Tables5–7. It can be found that the mean and RMS of all errors for the MCKF are smaller than those of the others.Table 5
Mean of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0532060.0505380.0844510.0478380.0587140.06099058.0398317.21291512.061803RCKF0.0520090.0500920.0819700.0153090.0307790.0304927.8634413.6873101.131346MCKF0.0517480.0498730.0808710.0123430.0135100.0204845.9954872.7175091.097752Table 6
RMS of attitude, velocity, and position errors in three methods.
MethodAttitudeAttitudeAttitudeVelocityVelocityVelocityPositionPositionPosition(pitch)(°)(roll)(°)(yaw)(°)(E) (m/s)(N) (m/s)(U) (m/s)(E) (m)(N) (m)(U) (m)KF0.0567480.0541190.1235100.0607040.0922250.06545764.2677768.99905015.949531RCKF0.0557700.0539080.0845490.0197510.0355550.0344209.1483014.3326631.426899MCKF0.0554410.0535780.0822190.0170430.0173430.0239717.3645213.5423591.215638Table 7
Mean and RMS of the horizontal position errors in three methods.
MethodKFRCKFMCKFMean(m)58.9610748.7992956.696143RMS(m)64.89476110.1224208.172177


## 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.



---

*Source: 1060672-2020-09-26.xml*