# Modified Sage-Husa Adaptive Kalman Filter-Based SINS/DVL Integrated Navigation System for AUV

Authors: Ruixin Liu; Fucheng Liu; Chunning Liu; Pengchao Zhang
Journal: Journal of Sensors
 (2021)

---

## Abstract

This paper presents a modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system for the autonomous underwater vehicle (AUV), where DVL is employed to correct the navigation errors of SINS that accumulate over time. When negative definite items are large enough, different from the positive definiteness of noise matrices which cannot be guaranteed for the conventional Sage-Husa adaptive Kalman filter, the proposed modified Sage-Husa adaptive Kalman filter deletes the negative definite items of adaptive update laws of the noise matrix to ensure the convergence of the Sage-Husa adaptive Kalman filter. In other words, this method sacrifices some filtering precision to ensure the stability of the filter. The simulation tests are implemented to verify that expected navigation accuracy for AUV can be obtained using the proposed modified Sage-Husa adaptive Kalman filter.


---

## Body

## 1. Introduction

Autonomous underwater vehicle (AUV) has been widely used in ocean exploration, where accurate navigation and positioning ability are essential to ensure the long voyage operation of AUV. Considering the underwater environment, various navigation methods cannot be applied to the AUV, such as optical navigation, radio navigation, and satellite navigation. In response to this problem, a lot of novel navigation methods are studied in [1–3].Since the strapdown inertial navigation (SINS) is with the advantages of strong autonomy, high precision, and full navigation parameters, it is widely used in the navigation system of AUV. However, the navigation error of SINS is accumulated over time, which results in low navigation accuracy. In view of this problem, the Doppler velocimeter (DVL) is combined with SINS in [4–7] to construct the SINS/DVL integrated navigation system, where the navigation error of SINS is corrected by the DVL. To provide the accurate initial attitude angles to the SINS/DVL integrated navigation system, the initial attitude alignment algorithms for SINS/DVL are studied. In [8, 9], the robust alignment methods are proposed to improve the alignment accuracy in complex working environment. In [10], a new Kalman filter-based alignment method is presented to modify the initial attitude alignment algorithm. But the state equations of above studies ignore the error of DVL, such as the calibration coefficient error, the velocity deviation error, and the drift angle error. That results in limited navigation accuracy.For the SINS/DVL integrated navigation system, how to integrate the output information of SINS and DVL to obtain high-precision navigation information is the core issue. Since the Kalman filter is able to estimate the system states in the presence of noises, it is widely applied in the SINS/DVL integrated navigation system. However, the inaccurate measurement model under the time-varying measurement noise results in substantial estimation errors or even filter divergence. To solve this problem, the interacting multiple model algorithm which uses more than one model is proposed in [11], where a variable model set based on the model probability weighted average of the model parameter is generated to pursue the real model. Considering the nonlinearity of the model of SINS/DVL, the square root unscented information filter is designed in [12], the randomly weighted cubature Kalman filter is discussed in [13], and the unscented Kalman filter (UKF) is employed in [14–16]. Furthermore, considering the influence of unknown environment and the inexact error model caused by model simplification, various adaptive Kalman filters are proposed in [17–20], where statistical characteristics of noises are online estimated in [17–19] and the recursive filtering gain is adaptively adjusted in [20]. However, the adaptive filter might be divergent when the positive definiteness of noise matrix cannot be guaranteed.In this paper, a modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system is proposed to provide the AUV with accurate navigation parameters, where the adaptive update laws of the noise matrix are modified by deleting the negative definite items. Therefore, the filtering convergence can be guaranteed, while the expected filtering accuracy can be obtained.The rest of this paper is organized as follows. The error equations of SINS and DVL are given in Section2. The Kalman filter equation and a modified Sage-Husa adaptive Kalman filter for the SINS/DVL integrated navigation system are presented in Section 3. Numerical simulations are conducted to verify the superiority of the proposed approach in Section 4. Conclusions are drawn in Section 5.


## 2. Error Equations of SINS And DVL

### 2.1. Error Equations of Strapdown Inertial Navigation

When the AUV moves on the water surface, considering the drift of the gyroscopes and accelerometer, the error equations of SINS are presented.Define the longitude and latitude asL and λ, the longitude error and latitude error as δL and δλ, the east and north velocity as vx and vy, the east and north velocity error as δvx and δvy, the earth radius as R, the angular velocity of the earth as ωie, the output of accelerometer in the navigation coordinate system as fx, fy, and fz, the horizontal drift of the accelerometer in the navigation coordinate system as ∇x and ∇y, and the drift of the gyroscope in the navigation coordinate system as εx, εy, and εz. Then, the attitude error equation is calculated as
(1)φ̇x=−δvyR+ωiesinL+vxRtanLφy−ωiecosL+vxRφz+εx,φ̇y=−ωiesinLδL+δvxR−ωiesinL+vxRφx−vyRφz+εy,φ̇z=ωiecosL+vxRsec2LδL+tanLRδvx+ωiecosL+vxRφx+vyRφy+εz.The horizontal position error equation of SINS is calculated as(2)δL̇=δvyR,δλ̇=δvxRsecL+vxRδLtanLsecL.The horizontal velocity error equation of SINS is calculated as(3)δv̇x=2ωievycosL+vxvyRsec2LδL+vyRtanLδvx+2ωiesinL+vyRtanLδvy−fzφy+fyφz+∇x,δv̇y=−2ωievxcosL+vx2Rsec2LδL−2ωiesinL+vxRtanLδvx+fzφx−fzφz+∇y.The drift of the gyroscope in the navigation coordinate system can be described as(4)εn=Cbnεb,where Cbn is the attitude matrix, and the drift of the gyroscope in the body coordinate system can be described as
(5)ε̇xb=−βgεxb+ωx,ε̇yb=−βgεyb+ωy,ε̇zb=−βgεzb+ωz,with the correlation time βg, the drift of the gyroscope in the body coordinate system εxb, εyb, and εzb, and the white noise ωx, ωy, and ωz.


### 2.2. Error Equations of the Doppler Velocimeter

For the four-beam Doppler velocimeter, the calibration coefficient error is defined asδC, the velocity deviation error is defined as δvd, and the drift angle error is defined as δΔ. Then, the output of the Doppler velocimeter can be presented as
(6)vdx′=1+δCvd+δvdsinKv+φz+δΔ,vdy′=1+δCvd+δvdcosKv+φz+δΔ,where φz is the azimuth angle, Kv is the track angle, and vd is the true velocity.SinceδC, δvd, δΔ, andφz are small, ignoring the small quantities of the second order, equation (6) can be simplified as
(7)vdx′=vx+vyφz+δΔ+δC⋅vx+δvdsinKv,vdy′=vy−vxφz+δΔ+δC⋅vy+δvdcosKv,where
(8)vx=vdsinKv,vy=vdcosKv,δvdx=δvdsinKv,δvdy=δvdcosKv,δv̇d=−βdδvd+ωd,δΔ̇=−βΔδΔ+ωΔ,δĊ=0,with the correlation time βd and βΔ and the white noise ωd and ωΔ.


## 2.1. Error Equations of Strapdown Inertial Navigation

When the AUV moves on the water surface, considering the drift of the gyroscopes and accelerometer, the error equations of SINS are presented.Define the longitude and latitude asL and λ, the longitude error and latitude error as δL and δλ, the east and north velocity as vx and vy, the east and north velocity error as δvx and δvy, the earth radius as R, the angular velocity of the earth as ωie, the output of accelerometer in the navigation coordinate system as fx, fy, and fz, the horizontal drift of the accelerometer in the navigation coordinate system as ∇x and ∇y, and the drift of the gyroscope in the navigation coordinate system as εx, εy, and εz. Then, the attitude error equation is calculated as
(1)φ̇x=−δvyR+ωiesinL+vxRtanLφy−ωiecosL+vxRφz+εx,φ̇y=−ωiesinLδL+δvxR−ωiesinL+vxRφx−vyRφz+εy,φ̇z=ωiecosL+vxRsec2LδL+tanLRδvx+ωiecosL+vxRφx+vyRφy+εz.The horizontal position error equation of SINS is calculated as(2)δL̇=δvyR,δλ̇=δvxRsecL+vxRδLtanLsecL.The horizontal velocity error equation of SINS is calculated as(3)δv̇x=2ωievycosL+vxvyRsec2LδL+vyRtanLδvx+2ωiesinL+vyRtanLδvy−fzφy+fyφz+∇x,δv̇y=−2ωievxcosL+vx2Rsec2LδL−2ωiesinL+vxRtanLδvx+fzφx−fzφz+∇y.The drift of the gyroscope in the navigation coordinate system can be described as(4)εn=Cbnεb,where Cbn is the attitude matrix, and the drift of the gyroscope in the body coordinate system can be described as
(5)ε̇xb=−βgεxb+ωx,ε̇yb=−βgεyb+ωy,ε̇zb=−βgεzb+ωz,with the correlation time βg, the drift of the gyroscope in the body coordinate system εxb, εyb, and εzb, and the white noise ωx, ωy, and ωz.


## 2.2. Error Equations of the Doppler Velocimeter

For the four-beam Doppler velocimeter, the calibration coefficient error is defined asδC, the velocity deviation error is defined as δvd, and the drift angle error is defined as δΔ. Then, the output of the Doppler velocimeter can be presented as
(6)vdx′=1+δCvd+δvdsinKv+φz+δΔ,vdy′=1+δCvd+δvdcosKv+φz+δΔ,where φz is the azimuth angle, Kv is the track angle, and vd is the true velocity.SinceδC, δvd, δΔ, andφz are small, ignoring the small quantities of the second order, equation (6) can be simplified as
(7)vdx′=vx+vyφz+δΔ+δC⋅vx+δvdsinKv,vdy′=vy−vxφz+δΔ+δC⋅vy+δvdcosKv,where
(8)vx=vdsinKv,vy=vdcosKv,δvdx=δvdsinKv,δvdy=δvdcosKv,δv̇d=−βdδvd+ωd,δΔ̇=−βΔδΔ+ωΔ,δĊ=0,with the correlation time βd and βΔ and the white noise ωd and ωΔ.


## 3. SINS/DVL Integrated Navigation System

### 3.1. State Equation and Measurement Equation

Since the navigation error of SINS is accumulated over time, the four-beam Doppler velocimeter is used to correct the navigation information of SINS. Define the state vector asX=δL,δλ,δvx,δvy,φx,φy,φz,εx,εy,εz,δVd,δΔ,δCT. The state equation is presented as
(9)Ẋ=AX+BW,where W=0,0,ax,ay,0,0,0,ωx,ωy,ωz,ωd,ωΔ,0T is the noise matrix, A is the state-transition matrix, B is the noise distribution matrix, and
(10)A=ASINS4×704×304×3ASINS3×7Cb3×3n03×303×7AGyro3×303×303×703×3ADVL3×3,B=I13×13,with
(11)AGyro3×3=−βg000−βg000−βg,ADVL3×3=−βd000−βΔ0000.Since the velocity is selected as the measurement, the measurement equation is presented as(12)Z=ZxZy=vsx−vdx′vsy−vdy′,where
(13)vsx=vx+δvx,vsy=vy+δvy.From equations (7) and (12), it is calculated that
(14)Zx=δvx−vyφz+δΔ−δVdsinKv−δCvx+ηx,Zy=δvy+vxφz+δΔ−δVdcosKv−δCvy+ηy,where ηx and ηy are the system measured noises.Thus, equation (12) can be rewritten as
(15)Z=HX+V,where
(16)H=001000−vy000−sinKv−vy−vx000100vx000−cosKvvx−vyV=ηx,ηyT.By discretizing the state (equation (9)) and measurement (equation (15)), the Kalman filter equation of the SINS/DVL integrated navigation system is obtained as
(17)Xk=ϕk/k−1Xk−1+Γk−1Wk−1,Zk=HkXk+Vk,where Xk is the state vector at time k, Zk is the measurement vector at time k, Hk is the measurement matrix at time k, Vk is the system measurement noise matrix at time k, Xk−1 is the state vector at time k−1, Wk−1 is the noise matrix at time k−1, Γk−1 is the noise distribution matrix at time k−1, ϕk/k−1 is the state one-step transition matrix, and Wk and Vk are white Gaussian noises with Wk∼WNqk,Qk, Vk∼WNrk,Rk, and
(18)ϕk/k−1=I13×13+AT,Γk−1=BT,Hk=H,with the sampling period T.


### 3.2. Modified Sage-Husa Adaptive Kalman Filter

Due to the fact that the measurement noises of the Kalman filter equation are usually time-varying and difficult to be accurately predicted in the practical environment, the filter accuracy of the conventional Kalman filter is easy to be decreased and even be divergent. In response to this problem, the Sage-Husa adaptive Kalman filter is proposed to improve the filter performance by estimating the unknown noises, whose calculation loop is presented in Figure1. The Sage-Husa adaptive Kalman filter is designed as
(19)X^k/k−1=Φk/k−1X^k−1+q^k−1,Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Q^k−1,ek=Zk−HkX^k/k−1−r^k,Kk=Pk/k−1HkTHkPk/k−1HkT+R∧k−1,X^k=X^k/k−1+Kkek,Pk=1−KkHkPk/k−1,where
(20)r^k=1−Dkr^k−1+DkZk−HkX^k/k−1,R^k=1−DkR^k−1+DkekekT−HkPk/k−1HkT,q^k=1−Dkq^k−1+DkX^k−Φk/k−1X^k−1,Q^k=1−DkQ^k−1+DkKkekekTKkT+Pk−Φk/k−1Pk−1Φk/k−1T,Dk=Dk−1Dk−1+b,D0=1,Figure 1
Calculation loop of the modified Sage-Husa adaptive Kalman filter.with the forgetting factorb and b∈0.95~0.999.To suppress filter divergence,Q^k must be nonnegative definite and R^k must be positive definite. However, considering the existence of −Φk/k−1Pk−1Φk/k−1T and −HkPk/k−1HkT in the above equation, the positive definiteness of Q^k and R^k cannot be guaranteed. Since −Φk/k−1Pk−1Φk/k−1T and −HkPk/k−1HkT have little influence on filter accuracy, they can be deleted to ensure the convergence of the Sage-Husa adaptive Kalman filter. In other words, this method sacrifices some filtering precision to ensure the stability of the filter.Thus,Q^k and R^k are modified as
(21)R^k=1−DkR^k−1+DkekekT,Q^k=1−DkQ^k−1+DkKkekekTKkT+Pk.From equation (21), it is obtained that both Q^k and R^k are affected by the innovation vector ek. That means that the filter accuracy cannot be guaranteed when Q^k and R^k a5re changing abnormally with ek. In response to this problem, Q^k is assumed to be known such that R^k can be accurately estimated.


## 3.1. State Equation and Measurement Equation

Since the navigation error of SINS is accumulated over time, the four-beam Doppler velocimeter is used to correct the navigation information of SINS. Define the state vector asX=δL,δλ,δvx,δvy,φx,φy,φz,εx,εy,εz,δVd,δΔ,δCT. The state equation is presented as
(9)Ẋ=AX+BW,where W=0,0,ax,ay,0,0,0,ωx,ωy,ωz,ωd,ωΔ,0T is the noise matrix, A is the state-transition matrix, B is the noise distribution matrix, and
(10)A=ASINS4×704×304×3ASINS3×7Cb3×3n03×303×7AGyro3×303×303×703×3ADVL3×3,B=I13×13,with
(11)AGyro3×3=−βg000−βg000−βg,ADVL3×3=−βd000−βΔ0000.Since the velocity is selected as the measurement, the measurement equation is presented as(12)Z=ZxZy=vsx−vdx′vsy−vdy′,where
(13)vsx=vx+δvx,vsy=vy+δvy.From equations (7) and (12), it is calculated that
(14)Zx=δvx−vyφz+δΔ−δVdsinKv−δCvx+ηx,Zy=δvy+vxφz+δΔ−δVdcosKv−δCvy+ηy,where ηx and ηy are the system measured noises.Thus, equation (12) can be rewritten as
(15)Z=HX+V,where
(16)H=001000−vy000−sinKv−vy−vx000100vx000−cosKvvx−vyV=ηx,ηyT.By discretizing the state (equation (9)) and measurement (equation (15)), the Kalman filter equation of the SINS/DVL integrated navigation system is obtained as
(17)Xk=ϕk/k−1Xk−1+Γk−1Wk−1,Zk=HkXk+Vk,where Xk is the state vector at time k, Zk is the measurement vector at time k, Hk is the measurement matrix at time k, Vk is the system measurement noise matrix at time k, Xk−1 is the state vector at time k−1, Wk−1 is the noise matrix at time k−1, Γk−1 is the noise distribution matrix at time k−1, ϕk/k−1 is the state one-step transition matrix, and Wk and Vk are white Gaussian noises with Wk∼WNqk,Qk, Vk∼WNrk,Rk, and
(18)ϕk/k−1=I13×13+AT,Γk−1=BT,Hk=H,with the sampling period T.


## 3.2. Modified Sage-Husa Adaptive Kalman Filter

Due to the fact that the measurement noises of the Kalman filter equation are usually time-varying and difficult to be accurately predicted in the practical environment, the filter accuracy of the conventional Kalman filter is easy to be decreased and even be divergent. In response to this problem, the Sage-Husa adaptive Kalman filter is proposed to improve the filter performance by estimating the unknown noises, whose calculation loop is presented in Figure1. The Sage-Husa adaptive Kalman filter is designed as
(19)X^k/k−1=Φk/k−1X^k−1+q^k−1,Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Q^k−1,ek=Zk−HkX^k/k−1−r^k,Kk=Pk/k−1HkTHkPk/k−1HkT+R∧k−1,X^k=X^k/k−1+Kkek,Pk=1−KkHkPk/k−1,where
(20)r^k=1−Dkr^k−1+DkZk−HkX^k/k−1,R^k=1−DkR^k−1+DkekekT−HkPk/k−1HkT,q^k=1−Dkq^k−1+DkX^k−Φk/k−1X^k−1,Q^k=1−DkQ^k−1+DkKkekekTKkT+Pk−Φk/k−1Pk−1Φk/k−1T,Dk=Dk−1Dk−1+b,D0=1,Figure 1
Calculation loop of the modified Sage-Husa adaptive Kalman filter.with the forgetting factorb and b∈0.95~0.999.To suppress filter divergence,Q^k must be nonnegative definite and R^k must be positive definite. However, considering the existence of −Φk/k−1Pk−1Φk/k−1T and −HkPk/k−1HkT in the above equation, the positive definiteness of Q^k and R^k cannot be guaranteed. Since −Φk/k−1Pk−1Φk/k−1T and −HkPk/k−1HkT have little influence on filter accuracy, they can be deleted to ensure the convergence of the Sage-Husa adaptive Kalman filter. In other words, this method sacrifices some filtering precision to ensure the stability of the filter.Thus,Q^k and R^k are modified as
(21)R^k=1−DkR^k−1+DkekekT,Q^k=1−DkQ^k−1+DkKkekekTKkT+Pk.From equation (21), it is obtained that both Q^k and R^k are affected by the innovation vector ek. That means that the filter accuracy cannot be guaranteed when Q^k and R^k a5re changing abnormally with ek. In response to this problem, Q^k is assumed to be known such that R^k can be accurately estimated.


## 4. Simulation

In this section, numerical simulations are investigated to verify that expected navigation accuracy can be obtained under the proposed modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system for AUV.The forgetting factor of the modified Sage-Husa adaptive Kalman filter is set asb=0.99. For the gyroscopes of SINS, the constant drift is 0.01°/h and the random drift is 0.001°/h. For the accelerometers of SINS, the constant drift is 10μg and the random drift is 3μg/s. The velocity measurement error of DVL is 0.5%. The initial covariance matrix is set asP0=diag1.5679×10−5rad,1.5679×10−5rad,5m/s,5m/s,10°,10°,10°,0.1°/h,0.1°/h,0.1°/h,0.005m/s,1′,0.00012, the initial variance matrix of state noise is set asQ0=diag0,0,5×10−4g,2×10−4g,0.1°/h,0.1°/h,0.1°/h,0,0,0,0.005×1−e−2βdTm/s,1×1−e−2βΔT′,0.00012 with βd−1=5min, and the variance matrix of measurement noise is set as R0=diag0.01m/s,0.01m/s2.The simulations are presented in Figures2–6. From the system states of the Kalman filter for the SINS/DVL integrated navigation system presented in Figure 2, it is observed that all the system states can converge to the constants. Then, velocities and positions of AUV under the SINS and SINS/DVL integrated navigation system are presented in Figures 3–6. From Figures 3 and 4, velocity measurements under two navigation systems are with local differences. Furthermore, the velocity errors shown in Figure 4 depicted that under the SINS/DVL integrated navigation system, the velocity errors converge to zero, where velocity errors of SINS are diverging. Positions of AUV under the SINS and SINS/DVL integrated navigation system are presented in Figure 5, and then, the position errors shown in Figure 6 depict that under the SINS/DVL integrated navigation system, the position errors of SINS/DVL are much smaller than those of SINS. In a word, the expected navigation accuracy is obtained using the proposed modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system.Figure 2
System states of the Kalman filter for the SINS/DVL integrated navigation system.Figure 3
Velocities of the SINS and SINS/DVL integrated navigation system.Figure 4
Velocity errors of the SINS and SINS/DVL integrated navigation system.Figure 5
Positions of the SINS and SINS/DVL integrated navigation system.Figure 6
Position errors of the SINS and SINS/DVL integrated navigation system.


## 5. Conclusion

For AUV, a modified Sage-Husa adaptive Kalman filter-based SINS/DVL integrated navigation system is designed in this paper to obtain expected navigation accuracy. The negative definite items of adaptive update laws of the noise matrix are deleted to guarantee the positive definiteness of noise matrices, such that the filter stability can be ensured. Simulations are presented to verify that expected navigation accuracy for AUV can be obtained using the proposed filter method.



---

*Source: 9992041-2021-07-17.xml*