# An Improved Adaptive Kalman Filter for Underwater SINS/DVL System

Authors: Di Wang; Xiaosu Xu; Lanhua Hou
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

The main challenge of Strap-down Inertial Navigation System (SINS)/Doppler velocity log (DVL) navigation system is the external measurement noise. Although the Sage–Husa adaptive Kalman filter (SHAKF) has been introduced in the integrated navigation field, the precision and stability of the SHAKF are still the tricky problems to be overcome. The primary aim of this paper is to improve the precision and stability of underwater SINS/DVL system. To attain this, a SINS/DVL tightly integrated model is established, where beam measurements are used without transforming them to 3D velocity. The proposed improved SHAKF algorithm is based on variable sliding window estimation and fading filter. The simulations and vehicle test results demonstrate the effectiveness of the proposed underwater SINS/DVL tightly integrated navigation method based on the improved SHAKF. In addition, the position accuracy of the designed method outperforms that of the SHAKF method.


---

## Body

## 1. Introduction

As humans explore the ocean, the demand for resource development, marine environmental surveys, and operations has also increased. Underwater vehicles can replace humans in deep ocean to complete the detection and development of marine resources, saving a lot of cost for production. Underwater vehicles have become one of the important tools for human ocean development [1]. With the gradual deepening of human beings in the field of ocean exploration, the tasks to be accomplished by underwater vehicles are becoming more and more complex and the working time is getting longer and longer. Now, the development of underwater vehicles faces many problems, such as the realization of high-precision autonomous navigation and positioning functions. The autonomous navigation system can provide navigation information for the underwater vehicle to ensure the correct travel.At present, there are many kinds of navigation systems used on underwater vehicles such as Strap-down Inertial Navigation System (SINS), Long Baseline (LBL), Doppler velocity log (DVL), and Ultrashort Baseline (USBL) [2]. The premise of acoustic navigation system applications such as Long Baselines and Ultrashort Baselines is that the reference matrix is placed in the working sea area beforehand. However, the debugging and arranging process would waste manpower and material resources. So they are not suitable for a wide range of navigation and position. An autonomous navigation system based on SINS requires navigation by means of inertial devices (gyroscopes and accelerometers) in the navigation system [3]. However, due to inertial measurement unit (IMU) installation errors and self-propelled problems, the navigation errors of the system will accumulate over time and will not be corrected in time. Therefore, the navigation accuracy will decrease after a period of time. DVL can provide relatively accurate speed information, which is self-contained, and the error does not accumulate over time. Therefore, SINS/DVL-based integrated navigation is a commonly used integrated navigation method for underwater vehicles [4].In integrated navigation systems, Kalman filter is a widely used information fusion algorithm. The wide application of Kalman filter began in the 1960s. The essence of this filter is a recursive process, which is the optimal minimum variance estimation of data. The filter can achieve an optimal estimate by fusing information from different sensors. The traditional Kalman filter can be applied to linear systems, but in fact many systems are nonlinear systems. So, extended Kalman filter (EKF) is proposed. This filter no longer requires the system to be linear, which compensates for the deficiency of traditional Kalman filter to some extent. Then, the unscented Kalman filter (UKF) based on the nonlinear system is proposed [5, 6]. The algorithm realizes the recursion of the state quantity of the system by setting the sampling point, and U transforms the sampling point to replace the original value. In order to obtain better filter estimation, the above methods all require the system model to be accurate and the external interference signal is uncorrelated white noise with statistical characteristics. Otherwise, the filtering accuracy cannot be guaranteed, and even the filter divergence may be caused. However, in the actual SINS/DVL system, due to the error drift of the navigation device, the dynamic error caused by the vehicle manipulation, and the influence of the complex environment in the water, the system model is difficult to be completely accurate and the noise statistical characteristics are also uncertain. In order to further study the above issues, some researchers have introduced the Sage–Husa adaptive Kalman filter algorithm [7] to estimate the statistical characteristics of noise in real time.Reference [8] pointed out the insufficiency of Sage–Husa adaptive filtering; that is, the measurement noise variance matrix R and the system noise variance matrix Q cannot be estimated online at the same time. Therefore, scholars have proposed a simplified Sage–Husa filter [9], which can estimate R when the Q is known, and the method has good adaptability. According to [10], a modified SHAKF is introduced to enhance the performance and increase the stability of the algorithm for the low cost EMES-INS/GPS tightly integrated system. The proposed Sage–Husa adaptive Kalman algorithm in Reference [11] is based on the adaptive weight that makes a better performance than KF for fiber optic gyroscope (FOG) drift error signal denoising. In order to solve the problem of reduced accuracy or the divergence of the filter, an adaptive Kalman filter (AKF) algorithm based on one-step smoothing filter is designed in [12] for integrated SINS/DVL systems. The variational Bayesian approach is introduced in Kalman filter in [13–15]. Parameter estimation and inference problems in Bayesian models are often very difficult. The adaptive Kalman model based on Sage–Husa is relatively simple and meets the real-time requirements of integrated navigation.To further improve the antijamming capability of the navigation system, some methods are proposed. On the one hand, a novel SINS/DVL tightly integrated model is built, which can work well when the number of DVL beam channels is less than 3. On the other hand, an improved Sage–Husa adaptive Kalman filter (ISHAKF) is introduced, which introduces the forgetting factor and variable sliding window.The structure of this paper is as follows: a SINS/DVL tightly integrated model is designed inSection 2. In Section 3, the ISHAKF method is presented, which is based on the fading factor and the variable sliding window method. The simulation and vehicle test are designed to illustrate the superiority of the method in Section 4. and 5 is devoted to the conclusions.


## 2. SINS/DVL Tightly Integrated System

The underwater SINS/DVL navigation system based on the loose combination takes the difference between the three-dimensional velocity of the DVL output and the three-dimensional velocity of the SINS. When the number of DVL beam channels is less than three, DVL will not be able to output 3D speed information, so the SINS/DVL integrated navigation system will not work well [16]. The tightly integrated navigation method proposed in this paper makes full use of DVL beam data to extend the measurement information to four dimensions. When the DVL beam information is less than three, the integrated navigation system can still work normally, further improving the fault tolerance of the system. A novel structure of the SINS/DVL tightly integrated navigation system is shown in Figure 1, where it can be seen that the differences among the original information of the four channels output by DVL and the SINS speed are used as the measurement information. At the same time, the three-dimensional SINS velocity information is extended to four dimensions through the transfer matrix. The system can estimate the DVL scale factor error, the gyro zero bias, and the add-on zero bias by Kalman filtering and finally feed back to the sensor output information to further improve the navigation accuracy of the underwater system.Figure 1
Structure of the proposed underwater SINS/DVL tightly integrated system.


### 2.1. State Equation

The state equation of the SINS/DVL tightly system can be shown as follows:(1)X˙=FX+GW,where F denotes the state transition matrix, G is the system noise matrix, W is the process noise vector, and X denotes state vector, which can be obtained as follows:(2)X=ϕxϕyϕzδVEnδVNnδVUnδλδLδhεxεyεz∇x∇y∇zKDbPST,where ϕ represents attitude error, δV represents velocity error, δλ,δL,δh denote position error, ε represents gyro bias, ∇ represents accelerometer bias, KD represents DVL scale factor, and bPS represents pressure sensor bias. The system state transition matrix F, the matrix G, and W can be expressed as follows [17]:(3)F=MaaMavMap−Cbn03×3MvaMvvMvp03×3Cbn03×203×3MpvMppCbn03×308×15,G=−Cbn03×303×3Cbn011×1011×1,W=wgbwab,where Maa,Mav,Map,Mva,Mvv,Mpv,Mpp,Mvp can be known in [17].


### 2.2. Measurement Equation

The measurement equation of the SINS/DVL tightly integrated system model is given as(4)Z=HX+V,where H is the measurement transfer matrix, V is the measurement information noise, and Z is the measured matrix, which can be expressed as(5)Z=V˜DVL_1d−V˜SINS_1dV˜DVL_2d−V˜SINS_2dV˜DVL_3d−V˜SINS_3dV˜DVL_4d−V˜SINS_4dH˜PS−HPS,where V˜SINSd=V˜SINS_1dV˜SINS_2dV˜SINS_3dV˜SINS_4dT denotes the velocity of SINS under the d frame and V˜DVLd=V˜DVL_1dV˜DVL_2dV˜DVL_3dV˜DVL_4dT denotes the velocity of DVL measurement. The noise V is(6)V=wDwPST.We give definitions as follows:n represents the navigation frame, b represents body frame, and d represents the DVL frame. The relationship between SINS velocity and DVL velocity can be expressed as(7)VDVLd=VSINSd=CbdCnbVSINSn,where Cnb is the direction cosine matrix and represents a transition from frame n to frame b and Cbd is the direction cosine matrix and represents a transition from frame b to frame d, which can be given as(8)Cbd=0cosα−sinαcosα0−sinα0−cosα−sinα−cosα0−sinα,α=70°,where α represents the horizontal angle between the beams and the underwater vehicle. Commonly, there is α=70°. Taking DVL beam 1 as an example, its conversion relationship is shown in Figure 2.Figure 2
The relationship between frameb and frame d.Figure2 is reproduced from [17]. (under the creative commons attribution license/public domain). We define the PS error model as(9)H˜PS=HPS+δbPS+wPS,where HPS represents the true value,δbPS represents the PS biases, and wPS represents the white noise. The DVL measurement error model can be expressed as [18](10)V˜DVLd=1+KDVDVLd+wD,where VDVLd represents the true value, KD represents the scale factor, and wD represents the white noise. Considering the installation angles error, the conversion relationship between frame b and frame d is(11)C˜bd=CbdI3×3+φ×,where ϕ=φxφyφzT denotes the installation angle error between IMU and DVL. According to the above analysis, the velocity of SINS under frame d can be expressed as(12)V˜SINSd=C˜bdC˜nbV˜SINSn=C˜bdC˜nbI3×3+φ×VSINSn+δVSINSn≈C˜bdC˜nbVSINSn+C˜bdC˜nbδVSINSn+C˜bdC˜nbφ×VSINSn.Substituting (7) into (12), we can get(13)V˜SINSd=VDVLd+C˜bdCnbδVSINSn−C˜bdCnbVSINSn×φ.The measurement information is(14)V˜DVLd−V˜SINSd=1+KDVDVLd+wD−VDVLd−C˜bdCnbδVSINSn+C˜bdCnbVSINSn×φ=VDVLdKD+wD−C˜bdCnbδVSINSn+C˜bdCnbVSINSn×φ=VDVLdKD+wD−CbdI3×3+φ×CnbδVSINSn+CbdI3×3+φ×CnbVSINSn×φ.So, the measurement transfer matrixH can be expressed as(15)H=MhbMha04×9VDVLd04×101×8−101×71,Mha=−CbdI3×3+φ×CnbMhb=CbdI3×3+φ×CnbVSINSn×.From (5), it can be seen that the DVL beam measurements are directly used without being transformed to frame b.


## 2.1. State Equation

The state equation of the SINS/DVL tightly system can be shown as follows:(1)X˙=FX+GW,where F denotes the state transition matrix, G is the system noise matrix, W is the process noise vector, and X denotes state vector, which can be obtained as follows:(2)X=ϕxϕyϕzδVEnδVNnδVUnδλδLδhεxεyεz∇x∇y∇zKDbPST,where ϕ represents attitude error, δV represents velocity error, δλ,δL,δh denote position error, ε represents gyro bias, ∇ represents accelerometer bias, KD represents DVL scale factor, and bPS represents pressure sensor bias. The system state transition matrix F, the matrix G, and W can be expressed as follows [17]:(3)F=MaaMavMap−Cbn03×3MvaMvvMvp03×3Cbn03×203×3MpvMppCbn03×308×15,G=−Cbn03×303×3Cbn011×1011×1,W=wgbwab,where Maa,Mav,Map,Mva,Mvv,Mpv,Mpp,Mvp can be known in [17].


## 2.2. Measurement Equation

The measurement equation of the SINS/DVL tightly integrated system model is given as(4)Z=HX+V,where H is the measurement transfer matrix, V is the measurement information noise, and Z is the measured matrix, which can be expressed as(5)Z=V˜DVL_1d−V˜SINS_1dV˜DVL_2d−V˜SINS_2dV˜DVL_3d−V˜SINS_3dV˜DVL_4d−V˜SINS_4dH˜PS−HPS,where V˜SINSd=V˜SINS_1dV˜SINS_2dV˜SINS_3dV˜SINS_4dT denotes the velocity of SINS under the d frame and V˜DVLd=V˜DVL_1dV˜DVL_2dV˜DVL_3dV˜DVL_4dT denotes the velocity of DVL measurement. The noise V is(6)V=wDwPST.We give definitions as follows:n represents the navigation frame, b represents body frame, and d represents the DVL frame. The relationship between SINS velocity and DVL velocity can be expressed as(7)VDVLd=VSINSd=CbdCnbVSINSn,where Cnb is the direction cosine matrix and represents a transition from frame n to frame b and Cbd is the direction cosine matrix and represents a transition from frame b to frame d, which can be given as(8)Cbd=0cosα−sinαcosα0−sinα0−cosα−sinα−cosα0−sinα,α=70°,where α represents the horizontal angle between the beams and the underwater vehicle. Commonly, there is α=70°. Taking DVL beam 1 as an example, its conversion relationship is shown in Figure 2.Figure 2
The relationship between frameb and frame d.Figure2 is reproduced from [17]. (under the creative commons attribution license/public domain). We define the PS error model as(9)H˜PS=HPS+δbPS+wPS,where HPS represents the true value,δbPS represents the PS biases, and wPS represents the white noise. The DVL measurement error model can be expressed as [18](10)V˜DVLd=1+KDVDVLd+wD,where VDVLd represents the true value, KD represents the scale factor, and wD represents the white noise. Considering the installation angles error, the conversion relationship between frame b and frame d is(11)C˜bd=CbdI3×3+φ×,where ϕ=φxφyφzT denotes the installation angle error between IMU and DVL. According to the above analysis, the velocity of SINS under frame d can be expressed as(12)V˜SINSd=C˜bdC˜nbV˜SINSn=C˜bdC˜nbI3×3+φ×VSINSn+δVSINSn≈C˜bdC˜nbVSINSn+C˜bdC˜nbδVSINSn+C˜bdC˜nbφ×VSINSn.Substituting (7) into (12), we can get(13)V˜SINSd=VDVLd+C˜bdCnbδVSINSn−C˜bdCnbVSINSn×φ.The measurement information is(14)V˜DVLd−V˜SINSd=1+KDVDVLd+wD−VDVLd−C˜bdCnbδVSINSn+C˜bdCnbVSINSn×φ=VDVLdKD+wD−C˜bdCnbδVSINSn+C˜bdCnbVSINSn×φ=VDVLdKD+wD−CbdI3×3+φ×CnbδVSINSn+CbdI3×3+φ×CnbVSINSn×φ.So, the measurement transfer matrixH can be expressed as(15)H=MhbMha04×9VDVLd04×101×8−101×71,Mha=−CbdI3×3+φ×CnbMhb=CbdI3×3+φ×CnbVSINSn×.From (5), it can be seen that the DVL beam measurements are directly used without being transformed to frame b.


## 3. Improved Adaptive Filter Method

### 3.1. Sage–Husa Adaptive Kalman Filter

In 1969, scholars A. P. Sage and G. W. Husa designed an adaptive filter algorithm, which can estimate the noise characteristics of the system in real time through the measurement output while performing state estimation [19]. Sage–Husa adaptive algorithm is based on the Kalman filter to add time-varying noise estimator to estimate and correct the statistical characteristics of system noise in real time, thereby reducing model error, suppressing filter divergence, and enhancing filter accuracy. However, it is impossible to estimate all noise parameters such as system noise mean and variance and measurement noise mean and variance. Considering that system noise generally has less impact, the effects of system noise can be ignored. The currently used Sage–Husa filter is an adaptive filter algorithm, which is based on the estimated measurement variance matrix R. The system model can be established as(16)Xk=Φk,k−1Xk−1+Γk−1Wk−1,Zk=HkXk+Vk,where Xk denotes state vector, Φk,k−1 denotes one-step transfer matrix from k–1 to k, Γk−1 denotes the system noise matrix, Zk denotes measurement vector, Hk is measurement matrix, Wk−1 denotes system noise, and Vk is measurement noise. The noise matrices Wk−1 and Vk satisfy the following statistical characteristics:(17)EWk=qk,CovWk,Wj=Qkδkj,EVk=rk,CovVk,Vj=Rkδkj,CovWk,Vj=0,where Qk denotes covariance of the system noise and Rk denotes covariance of the measurement noise. The standard Kalman process is as follows [20,21]:(18)Xk,k−1=Φk−1X^k−1+Γk−1qk−1,(19)Pk,k−1=Φk−1Pk−1Φk−1T+Γk−1Qk−1Γk−1T,(20)Kk=Pk,k−1ΗkTΗkPk,k−1ΗkT+Rk−1,(21)X^k=X^k,k−1+KkZk−HkX^k,k−1−rk,(22)Pk=I−KkHkPk,k−1.The simplified SHAKF is based on (18)–(22) to increase the measurement noise estimator.(23)εk=Zk−HkX^k,k−1−rk,(24)R^k=1−1kR^k−1+1kεkεkT−HkPk,k−1HkT.The SHAKF algorithm can measure the noiseR in real time and achieve the adaptive effect while estimating the state.


### 3.2. Improved SHAKF Method

In view of the deficiencies of SHAKF algorithm, this paper makes improvements from two aspects. Firstly, the variable fading factor is designed to improve the adaptive ability of the filtering. Secondly, the variable sliding window method is used to measure the estimation of noise, and the estimation accuracy of the measurement noise is further improved.(1) Based on the SHAKF algorithm, the fading factorλ is introduced. Therefore, considering the idea of combining a fading factor, (21) can be given as(25)X^k=X^k,k−1+λ⋅KkZk−HkX^k,k−1−rk.This paper proposes an adaptive adjustment function for fading factors. It can be known from formula (25) that when the error is large, λ should be reduced, thereby reducing the weight of the current measurement information. When the error is small, λ should increase, thereby increasing the weight of the current measurement information. Because the size of the fading factor can be determined by the error [22], we use the error mean Eεk as the error point. When it is less than the error mean Eεk, λ is adjusted closer to 1. When it is larger than the error mean Eεk, λ is adjusted closer to λmin. According to the above analysis, the proposed adaptive adjustment function is as follows:(26)λ=1−λminπarccotεk−Eεk+λmin.From (26), we know that the fading factor λ is controlled by the measurement error, which can ensure that the error swings around the error mean Eεk, which can improve the stability of the SHAKF. Normally, the value of λmin ranges from 0.7 to 0.8, and the value of λ ranges from λmin to 1. It can be seen from (26) that the forgetting factor λ is controlled by the error, which can ensure that the error swings around Een, and the change is slow, which can buffer the poor tracking effect caused by the λ change too fast.It can be known from (23) that if the measurement noise of the actual system is smaller than the theoretical model value, εkεkT will be smaller. If the state noise setting is too large, HkPk,k−1HkT will be larger. Both of the above cases may cause εkεkT−HkPk,k−1HkT<0, which makes it easy for R^k to lose positive definiteness and cause filtering abnormality. To solve this problem, this paper adopts the sequential filtering method and limits the size of each element of the R^k diagonal [23–25]. Assume R^k is a diagonal matrix. Sequential filtering is used to perform the ith scalar sequential measurement update. The scalar measurement equation is(27)Zki=HkiXk+Vki,ρki=Z˜k,k−1i2−HkiPk,k−1iHkiT.Set theR^k value range to meet the following conditions:(28)R˜ki=1−βkR˜k−1i+βkRmini,ρki<Rmini,Rmaxi,ρki>Rmaxi,1−βkR˜k−1i+βkRki,others,where i denotes the ith scalar. R˜ki represents the ith scalar element of the diagonal matrix R^k at time k. Through the above processing, the measurement noise R^k can be limited between Rmini and Rmaxi, thereby having better adaptive ability and reliability.(2) In order to enhance the estimation accuracy of measurement noise, a variable sliding window estimation method is designed in this paper. Through the measurement noise mean processing in the SHAKF, the estimation accuracy of the measurement noise is further improved. The following improvement is made to (24) in the filtering algorithm:(29)R^k=R^k−1+1m∑j=0mεk−jεk−jT−Hk−jPk−j,k−j−1Hk−jT,where m is the variable sliding window value. The range of values of m satisfies the following conditions:(30)m=m,m<N,N,m>N,where N is the preset window maximum. When a large change in the R value is detected, m = 0, and the sliding window is incremented from 0 to N. When the value of m is greater than N, the window m = N. This method is well adapted to the variation of the R value. The two processes of window adjustment are shown in Figures 3 and 4. Figure 3 is a window adjustment process when m < N. It can be seen from the figure that as the observation data increases, the window m also gradually increases.Figure 3
Window sliding adjustment process whenm < N.Figure 4
Window sliding adjustment process whenm > N.Figure4 is a window sliding adjustment process when m > N. It can be seen from the figure that as the observation data increases, the moving window size remains N.


### 3.3. ISHAKF for SINS/DVL Tight System Scheme

Figure5 is the structure of the improved Sage–Husa adaptive Kalman filter algorithm. The algorithm is based on the SINS/DVL tight combination navigation system designed in this paper, which can realize the noise estimation of four-dimensional velocity information. The ISHAKF algorithm is based on the SHAKF algorithm. Based on the original algorithm, the forgetting factor and variable sliding window method are introduced. The steps of implementing the ISHAKF algorithm are as follows.Figure 5
Process of the improved Sage–Husa adaptive filter for SINS/DVL.(1)
Perform ISHAKF algorithm status update.(2)
Calculate the forgetting factor according to the introduced forgetting factor adjustment function.(3)
Calculate the filter gain according to the forgetting factor, and perform the ISHAKF filter measurement update at the same time.(4)
Calculate the sliding window value, and perform the sliding window averaging processing on the measurement noise.(5)
Determine if theR value is outside the set range and further process the R value.


## 3.1. Sage–Husa Adaptive Kalman Filter

In 1969, scholars A. P. Sage and G. W. Husa designed an adaptive filter algorithm, which can estimate the noise characteristics of the system in real time through the measurement output while performing state estimation [19]. Sage–Husa adaptive algorithm is based on the Kalman filter to add time-varying noise estimator to estimate and correct the statistical characteristics of system noise in real time, thereby reducing model error, suppressing filter divergence, and enhancing filter accuracy. However, it is impossible to estimate all noise parameters such as system noise mean and variance and measurement noise mean and variance. Considering that system noise generally has less impact, the effects of system noise can be ignored. The currently used Sage–Husa filter is an adaptive filter algorithm, which is based on the estimated measurement variance matrix R. The system model can be established as(16)Xk=Φk,k−1Xk−1+Γk−1Wk−1,Zk=HkXk+Vk,where Xk denotes state vector, Φk,k−1 denotes one-step transfer matrix from k–1 to k, Γk−1 denotes the system noise matrix, Zk denotes measurement vector, Hk is measurement matrix, Wk−1 denotes system noise, and Vk is measurement noise. The noise matrices Wk−1 and Vk satisfy the following statistical characteristics:(17)EWk=qk,CovWk,Wj=Qkδkj,EVk=rk,CovVk,Vj=Rkδkj,CovWk,Vj=0,where Qk denotes covariance of the system noise and Rk denotes covariance of the measurement noise. The standard Kalman process is as follows [20,21]:(18)Xk,k−1=Φk−1X^k−1+Γk−1qk−1,(19)Pk,k−1=Φk−1Pk−1Φk−1T+Γk−1Qk−1Γk−1T,(20)Kk=Pk,k−1ΗkTΗkPk,k−1ΗkT+Rk−1,(21)X^k=X^k,k−1+KkZk−HkX^k,k−1−rk,(22)Pk=I−KkHkPk,k−1.The simplified SHAKF is based on (18)–(22) to increase the measurement noise estimator.(23)εk=Zk−HkX^k,k−1−rk,(24)R^k=1−1kR^k−1+1kεkεkT−HkPk,k−1HkT.The SHAKF algorithm can measure the noiseR in real time and achieve the adaptive effect while estimating the state.


## 3.2. Improved SHAKF Method

In view of the deficiencies of SHAKF algorithm, this paper makes improvements from two aspects. Firstly, the variable fading factor is designed to improve the adaptive ability of the filtering. Secondly, the variable sliding window method is used to measure the estimation of noise, and the estimation accuracy of the measurement noise is further improved.(1) Based on the SHAKF algorithm, the fading factorλ is introduced. Therefore, considering the idea of combining a fading factor, (21) can be given as(25)X^k=X^k,k−1+λ⋅KkZk−HkX^k,k−1−rk.This paper proposes an adaptive adjustment function for fading factors. It can be known from formula (25) that when the error is large, λ should be reduced, thereby reducing the weight of the current measurement information. When the error is small, λ should increase, thereby increasing the weight of the current measurement information. Because the size of the fading factor can be determined by the error [22], we use the error mean Eεk as the error point. When it is less than the error mean Eεk, λ is adjusted closer to 1. When it is larger than the error mean Eεk, λ is adjusted closer to λmin. According to the above analysis, the proposed adaptive adjustment function is as follows:(26)λ=1−λminπarccotεk−Eεk+λmin.From (26), we know that the fading factor λ is controlled by the measurement error, which can ensure that the error swings around the error mean Eεk, which can improve the stability of the SHAKF. Normally, the value of λmin ranges from 0.7 to 0.8, and the value of λ ranges from λmin to 1. It can be seen from (26) that the forgetting factor λ is controlled by the error, which can ensure that the error swings around Een, and the change is slow, which can buffer the poor tracking effect caused by the λ change too fast.It can be known from (23) that if the measurement noise of the actual system is smaller than the theoretical model value, εkεkT will be smaller. If the state noise setting is too large, HkPk,k−1HkT will be larger. Both of the above cases may cause εkεkT−HkPk,k−1HkT<0, which makes it easy for R^k to lose positive definiteness and cause filtering abnormality. To solve this problem, this paper adopts the sequential filtering method and limits the size of each element of the R^k diagonal [23–25]. Assume R^k is a diagonal matrix. Sequential filtering is used to perform the ith scalar sequential measurement update. The scalar measurement equation is(27)Zki=HkiXk+Vki,ρki=Z˜k,k−1i2−HkiPk,k−1iHkiT.Set theR^k value range to meet the following conditions:(28)R˜ki=1−βkR˜k−1i+βkRmini,ρki<Rmini,Rmaxi,ρki>Rmaxi,1−βkR˜k−1i+βkRki,others,where i denotes the ith scalar. R˜ki represents the ith scalar element of the diagonal matrix R^k at time k. Through the above processing, the measurement noise R^k can be limited between Rmini and Rmaxi, thereby having better adaptive ability and reliability.(2) In order to enhance the estimation accuracy of measurement noise, a variable sliding window estimation method is designed in this paper. Through the measurement noise mean processing in the SHAKF, the estimation accuracy of the measurement noise is further improved. The following improvement is made to (24) in the filtering algorithm:(29)R^k=R^k−1+1m∑j=0mεk−jεk−jT−Hk−jPk−j,k−j−1Hk−jT,where m is the variable sliding window value. The range of values of m satisfies the following conditions:(30)m=m,m<N,N,m>N,where N is the preset window maximum. When a large change in the R value is detected, m = 0, and the sliding window is incremented from 0 to N. When the value of m is greater than N, the window m = N. This method is well adapted to the variation of the R value. The two processes of window adjustment are shown in Figures 3 and 4. Figure 3 is a window adjustment process when m < N. It can be seen from the figure that as the observation data increases, the window m also gradually increases.Figure 3
Window sliding adjustment process whenm < N.Figure 4
Window sliding adjustment process whenm > N.Figure4 is a window sliding adjustment process when m > N. It can be seen from the figure that as the observation data increases, the moving window size remains N.


## 3.3. ISHAKF for SINS/DVL Tight System Scheme

Figure5 is the structure of the improved Sage–Husa adaptive Kalman filter algorithm. The algorithm is based on the SINS/DVL tight combination navigation system designed in this paper, which can realize the noise estimation of four-dimensional velocity information. The ISHAKF algorithm is based on the SHAKF algorithm. Based on the original algorithm, the forgetting factor and variable sliding window method are introduced. The steps of implementing the ISHAKF algorithm are as follows.Figure 5
Process of the improved Sage–Husa adaptive filter for SINS/DVL.(1)
Perform ISHAKF algorithm status update.(2)
Calculate the forgetting factor according to the introduced forgetting factor adjustment function.(3)
Calculate the filter gain according to the forgetting factor, and perform the ISHAKF filter measurement update at the same time.(4)
Calculate the sliding window value, and perform the sliding window averaging processing on the measurement noise.(5)
Determine if theR value is outside the set range and further process the R value.


## 4. Simulation and Vehicle Test

To illustrate the navigation performance of the proposed ISHAKF method, the simulation and vehicle experiment are designed. First, the trajectory is simulated, which simulates the underwater vehicle swinging motion. Second, the outperformance of the ISHAKF method is verified compared with Sage–Husa adaptive Kalman filter and Kalman filter in complex sea conditions. Finally, the vehicle test is designed, which simulates underwater SINS/DVL tightly integrated navigation system.


### 4.1. Simulation

The accelerometer biases and the random walk noise are set as100μg and100μg/Hz, respectively. The gyroscope biases and the random walk noise are set as 0.02°/h and 0.02°/h, respectively. The DVL scale factor is set as 0.002. The initial angle errors are set as 0.02°, −0.02°, and 0.5°, respectively. The DVL provides velocity information for four channels. The out frequencies of the IMU and DVL device are set as 200 Hz and 1 Hz, respectively. The vehicle movement start position is set to latitude 32.056°N and longitude 118.794°E. The curve of moving trajectory is shown in Figure 6. The altitude value of moving trajectory for underwater vehicle is 50 m, and the altitude value does not change during the whole movement.Figure 6
Curve of moving trajectory.Since the underwater vehicle movement is affected by the water flow, its motion should be a rocking motion. In order to simulate the swaying motion of the underwater vehicle, the attitude angle of the vehicle is given as(31)θ=θmsin2π5t+θ0,γ=γmsin2π7t+γ0,ψ=ψmsin2π6t+ψ0,where the amplitudes of the swaying motion are set as θm=3°,γm=4°, and ψm=3°, respectively, while the cycles of the swaying are 5 s, 7 s, and 6 s, respectively. The initial phases are set as θ0=0°,γ0=0°, and ψ0=0°, respectively. The curve of vehicle velocity and attitude is shown in Figure 7.Figure 7
Curve of velocity and attitude.
(a)(b)In order to illustrate the anti-interference ability of the proposed ISHAKF method to external noise, different noise levels are set. The measurement noise covariance is set asR=diag0.1m/s2,0.1m/s2,0.1m/s2,0.1m/s2 during 400 s∼700 s. Three methods are listed for SINS/DVL tightly integrated system. Figure 8 shows the curve of velocity error for KF, SHAKF, and ISHAKF.Figure 8
Curve of velocity error for three methods.The navigation accuracy and anti-interference ability of three methods (KF, SHAKF, and ISHAKF) are compared by calculating the root-mean-square error (RMSE) of the velocity:(32)RMSE=1n∑i=1nX˜i−Xi2,where Xi denotes the real velocity obtained by the simulation, n is the number of signals, and X˜i is the measurement information. The RMSE has a good reflection of the measurement precision. So, the error data in Figure 8 is processed by the RMSE, and the results are shown in Table 1.Table 1
RMSE for three methods.
KFSHAKFISHAKFΔVN (m/s)0.2460.1390.138ΔVE (m/s)0.2710.1450.135ΔVU (m/s)0.0610.0580.056From Table1 we know that the RMSE of north, east, and up velocity for ISHAKF are 0.138 m/s, 0.135 m/s, and 0.056 m/s, respectively, which has better performance than KF and SHAKF method. Figure 9 shows the curve of position error for KF, SHAKF, and ISHAKF. Compared with KF and SHAKF, it can be known that ISHAKF proposed in this paper has the smallest position error.Figure 9
Curve of position error for the three methods.According to navigation system, the positioning performance of the three methods is compared, the above position error is analyzed, and the max error is calculated, whose results are listed in Table2. We can see that the north and east position errors for ISHAKF are 14.303 m and 14.559 m, respectively, which reduced by 44.59% and 45.96% than SHAKF method.Table 2
Max error for three methods.
KFSHAKFISHAKFΔPN (m)113.13725.81614.303ΔPE (m)57.96626.94614.559ΔPU (m)0.6041.0141.014In order to further compare the positioning accuracy of the three methods, the horizontal position error is calculated for the KF, SHAKF, and ISHAKF as shown in Figure10. The max horizontal position errors for three methods are 126.30 m, 32.27 m, and 20.39 m, respectively, which shows that the ISHAKF method has the better accuracy than the KF and SHAKF method.Figure 10
The horizontal position error for the three methods.


### 4.2. Vehicle Test

The vehicle experiment is designed to simulate underwater vehicle due to the limitations of the experimental conditions. The vehicle experiment device includes a PHINS of IXSEA Corporation, a navigation computer, a Flex Park 6 GPS receiver, and an inertial measurement unit. Meanwhile, PHINS is used as a reference navigation system to provide the accurate navigation information in real time. The vehicle equipment is shown in Figure11. In vehicle experiment, the PHINS and GPS receiver are integrated, which works on the PHINS/GPS mode. So, PHINS provides relatively accurate navigation information including velocity position and attitude. The speed information output by the PHINS can be used to simulate the speed information provided by the DVL device. The instrument specifications are listed in Table 3.Figure 11
(a) Experimental vehicle and (b) equipment.
(a)(b)Table 3
Instrument specifications.
InstrumentsParametersAccuracyFrequency (Hz)IMUGyroscope bias≤0.02°/h200Gyroscope random walk≤0.005°/h200Accelerometer bias variation±50ug200Accelerometer output noise≤50ug/Hz200GPSVelocity (RMS)0.03 m/s1Position (L1)1.5 m1PHINSAttitude (GPS aided mode)≤0.01°200The entire test route is on the campus of Southeast University and lasts for 1000 s. Figure12 shows the motion trajectory. The curve of attitude and velocity is shown in Figure 13.Figure 12
Curve of moving trajectory.
(a)(b)Figure 13
Curve of attitude and velocity.In order to verify the fault-tolerant performance of the designed SINS/DVL tightly integrated navigation method, the loosely integrated navigation method is introduced in this section. The DVL scale factor is 0.002. The initial attitude error is0.1°,0.1°,0.5°. Two DVL beam missing regions are set: DVL beam channel 2 signal missing between 150 s and 200 s and DVL beam channel 4 signal missing between 600 s and 650 s.Figure14(a) shows the curve of velocity error, whose mean and standard deviation (STD) for two methods are shown in Figure 14(b). The position error for two methods is shown in Figure 15, where it can be known that the tightly integrated method proposed in this paper has better accuracy than the loosely integrated method. When the DVL beam channel information is less than 4, the loose combination cannot complete the combined navigation solution, so its speed error will continue to diverge. However, the tight combination method can still complete the combined navigation solution in the case of DVL beam loss and suppress the navigation error divergence. It can be know from the results that the tight combination method has better fault tolerance. The curve of the horizontal position error is shown in Figure 16, where it can be known that the max horizontal position error for tightly integrated method is 54.51 m, which is reduced by 63.9% compared to loosely integrated method.Figure 14
Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
(a)(b)Figure 15
Curve of position error for two methods.Figure 16
Curve of horizontal position error.The curve of velocity error for two methods is shown in Figure17(a), in which the SHAKF is marked by blue line and the ISHAKF is marked by red line. Mean and standard deviation (STD) of velocity errors for two methods are shown in Figure 17(b), which displays that the mean and STD of ISHAKF are better than SHAKF method.Figure 17
Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
(a)(b)The RMSE of velocity for SHAKF and ISHAKF method is listed in Table4. It can be seen that the north and east velocity errors for ISHAKF are 0.171 m/s and 0.204 m/s, respectively, which reduced by 12.7% and 7.2% compared to SHAKF method.Table 4
RMSE for two methods.
SHAKFISHAKFΔVN (m/s)0.1960.171ΔVE (m/s)0.2200.204ΔVU (m/s)0.1420.130Figure18 displays the curves of the north, east, and up position errors for SHAKF and ISHAKF. Compared with SHAKF, we know that the ISHAKF method has smaller position error. To further compare the positional accuracy of the two methods, the horizontal position error for the SHAKF and ISHAKF is displayed in Figure 19, which shows that the position accuracy based on ISHAKF is better than the SHAKF. The max horizontal position errors for SHAKF and ISHAKF are 49.76 m and 31.49 m. Compared with the SHAKF method, the accuracy of the ISHAKF method is improved by 36.71% which is listed in Table 5.Figure 18
Curve of position error for two methods.Figure 19
Curve of horizontal position error for two methods.Table 5
Maximum absolute error (MAE) for two methods.
SHAKFISHAKFΔPN (m)49.6830.85ΔPE (m)8.388.35ΔPU (m)4.373.05


## 4.1. Simulation

The accelerometer biases and the random walk noise are set as100μg and100μg/Hz, respectively. The gyroscope biases and the random walk noise are set as 0.02°/h and 0.02°/h, respectively. The DVL scale factor is set as 0.002. The initial angle errors are set as 0.02°, −0.02°, and 0.5°, respectively. The DVL provides velocity information for four channels. The out frequencies of the IMU and DVL device are set as 200 Hz and 1 Hz, respectively. The vehicle movement start position is set to latitude 32.056°N and longitude 118.794°E. The curve of moving trajectory is shown in Figure 6. The altitude value of moving trajectory for underwater vehicle is 50 m, and the altitude value does not change during the whole movement.Figure 6
Curve of moving trajectory.Since the underwater vehicle movement is affected by the water flow, its motion should be a rocking motion. In order to simulate the swaying motion of the underwater vehicle, the attitude angle of the vehicle is given as(31)θ=θmsin2π5t+θ0,γ=γmsin2π7t+γ0,ψ=ψmsin2π6t+ψ0,where the amplitudes of the swaying motion are set as θm=3°,γm=4°, and ψm=3°, respectively, while the cycles of the swaying are 5 s, 7 s, and 6 s, respectively. The initial phases are set as θ0=0°,γ0=0°, and ψ0=0°, respectively. The curve of vehicle velocity and attitude is shown in Figure 7.Figure 7
Curve of velocity and attitude.
(a)(b)In order to illustrate the anti-interference ability of the proposed ISHAKF method to external noise, different noise levels are set. The measurement noise covariance is set asR=diag0.1m/s2,0.1m/s2,0.1m/s2,0.1m/s2 during 400 s∼700 s. Three methods are listed for SINS/DVL tightly integrated system. Figure 8 shows the curve of velocity error for KF, SHAKF, and ISHAKF.Figure 8
Curve of velocity error for three methods.The navigation accuracy and anti-interference ability of three methods (KF, SHAKF, and ISHAKF) are compared by calculating the root-mean-square error (RMSE) of the velocity:(32)RMSE=1n∑i=1nX˜i−Xi2,where Xi denotes the real velocity obtained by the simulation, n is the number of signals, and X˜i is the measurement information. The RMSE has a good reflection of the measurement precision. So, the error data in Figure 8 is processed by the RMSE, and the results are shown in Table 1.Table 1
RMSE for three methods.
KFSHAKFISHAKFΔVN (m/s)0.2460.1390.138ΔVE (m/s)0.2710.1450.135ΔVU (m/s)0.0610.0580.056From Table1 we know that the RMSE of north, east, and up velocity for ISHAKF are 0.138 m/s, 0.135 m/s, and 0.056 m/s, respectively, which has better performance than KF and SHAKF method. Figure 9 shows the curve of position error for KF, SHAKF, and ISHAKF. Compared with KF and SHAKF, it can be known that ISHAKF proposed in this paper has the smallest position error.Figure 9
Curve of position error for the three methods.According to navigation system, the positioning performance of the three methods is compared, the above position error is analyzed, and the max error is calculated, whose results are listed in Table2. We can see that the north and east position errors for ISHAKF are 14.303 m and 14.559 m, respectively, which reduced by 44.59% and 45.96% than SHAKF method.Table 2
Max error for three methods.
KFSHAKFISHAKFΔPN (m)113.13725.81614.303ΔPE (m)57.96626.94614.559ΔPU (m)0.6041.0141.014In order to further compare the positioning accuracy of the three methods, the horizontal position error is calculated for the KF, SHAKF, and ISHAKF as shown in Figure10. The max horizontal position errors for three methods are 126.30 m, 32.27 m, and 20.39 m, respectively, which shows that the ISHAKF method has the better accuracy than the KF and SHAKF method.Figure 10
The horizontal position error for the three methods.


## 4.2. Vehicle Test

The vehicle experiment is designed to simulate underwater vehicle due to the limitations of the experimental conditions. The vehicle experiment device includes a PHINS of IXSEA Corporation, a navigation computer, a Flex Park 6 GPS receiver, and an inertial measurement unit. Meanwhile, PHINS is used as a reference navigation system to provide the accurate navigation information in real time. The vehicle equipment is shown in Figure11. In vehicle experiment, the PHINS and GPS receiver are integrated, which works on the PHINS/GPS mode. So, PHINS provides relatively accurate navigation information including velocity position and attitude. The speed information output by the PHINS can be used to simulate the speed information provided by the DVL device. The instrument specifications are listed in Table 3.Figure 11
(a) Experimental vehicle and (b) equipment.
(a)(b)Table 3
Instrument specifications.
InstrumentsParametersAccuracyFrequency (Hz)IMUGyroscope bias≤0.02°/h200Gyroscope random walk≤0.005°/h200Accelerometer bias variation±50ug200Accelerometer output noise≤50ug/Hz200GPSVelocity (RMS)0.03 m/s1Position (L1)1.5 m1PHINSAttitude (GPS aided mode)≤0.01°200The entire test route is on the campus of Southeast University and lasts for 1000 s. Figure12 shows the motion trajectory. The curve of attitude and velocity is shown in Figure 13.Figure 12
Curve of moving trajectory.
(a)(b)Figure 13
Curve of attitude and velocity.In order to verify the fault-tolerant performance of the designed SINS/DVL tightly integrated navigation method, the loosely integrated navigation method is introduced in this section. The DVL scale factor is 0.002. The initial attitude error is0.1°,0.1°,0.5°. Two DVL beam missing regions are set: DVL beam channel 2 signal missing between 150 s and 200 s and DVL beam channel 4 signal missing between 600 s and 650 s.Figure14(a) shows the curve of velocity error, whose mean and standard deviation (STD) for two methods are shown in Figure 14(b). The position error for two methods is shown in Figure 15, where it can be known that the tightly integrated method proposed in this paper has better accuracy than the loosely integrated method. When the DVL beam channel information is less than 4, the loose combination cannot complete the combined navigation solution, so its speed error will continue to diverge. However, the tight combination method can still complete the combined navigation solution in the case of DVL beam loss and suppress the navigation error divergence. It can be know from the results that the tight combination method has better fault tolerance. The curve of the horizontal position error is shown in Figure 16, where it can be known that the max horizontal position error for tightly integrated method is 54.51 m, which is reduced by 63.9% compared to loosely integrated method.Figure 14
Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
(a)(b)Figure 15
Curve of position error for two methods.Figure 16
Curve of horizontal position error.The curve of velocity error for two methods is shown in Figure17(a), in which the SHAKF is marked by blue line and the ISHAKF is marked by red line. Mean and standard deviation (STD) of velocity errors for two methods are shown in Figure 17(b), which displays that the mean and STD of ISHAKF are better than SHAKF method.Figure 17
Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
(a)(b)The RMSE of velocity for SHAKF and ISHAKF method is listed in Table4. It can be seen that the north and east velocity errors for ISHAKF are 0.171 m/s and 0.204 m/s, respectively, which reduced by 12.7% and 7.2% compared to SHAKF method.Table 4
RMSE for two methods.
SHAKFISHAKFΔVN (m/s)0.1960.171ΔVE (m/s)0.2200.204ΔVU (m/s)0.1420.130Figure18 displays the curves of the north, east, and up position errors for SHAKF and ISHAKF. Compared with SHAKF, we know that the ISHAKF method has smaller position error. To further compare the positional accuracy of the two methods, the horizontal position error for the SHAKF and ISHAKF is displayed in Figure 19, which shows that the position accuracy based on ISHAKF is better than the SHAKF. The max horizontal position errors for SHAKF and ISHAKF are 49.76 m and 31.49 m. Compared with the SHAKF method, the accuracy of the ISHAKF method is improved by 36.71% which is listed in Table 5.Figure 18
Curve of position error for two methods.Figure 19
Curve of horizontal position error for two methods.Table 5
Maximum absolute error (MAE) for two methods.
SHAKFISHAKFΔPN (m)49.6830.85ΔPE (m)8.388.35ΔPU (m)4.373.05


## 5. Conclusions

In order to improve the fault tolerance of SINS/DVL integrated navigation in complex environment, this paper proposes an improved Sage–Husa adaptive filtering SINS/DVL tight combination navigation method, which is improved from two aspects. Firstly, a new compact combined navigation model is derived based on the traditional SINS/DVL loose combination and the state equation and measurement equation are established. Secondly, based on the Sage–Husa adaptive Kalman filter algorithm, the forgetting factor and variable sliding window are introduced, which further improves the robustness and adaptive ability of the adaptive filter. Finally, simulation and in-vehicle tests verify that the proposed method can further enhance the robustness and navigation accuracy of the SINS/DVL system.



---

*Source: 5456961-2020-08-18.xml*