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

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.


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][2][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][5][6][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 sys-tem, 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][15][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][18][19][20], where statistical characteristics of noises are online estimated in [17][18][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 Section 2. 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.

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 as L and λ, the longitude error and latitude error as δL and δλ, the east and north velocity as v x and v y , the east and north velocity error as δv x and δv y , the earth radius as R, the angular velocity of the earth as ω ie , the output of accelerometer in the navigation coordinate system as f x , f y , and f z , 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 The horizontal position error equation of SINS is calculated as The horizontal velocity error equation of SINS is calculated as The drift of the gyroscope in the navigation coordinate system can be described as where C n b is the attitude matrix, and the drift of the gyroscope in the body coordinate system can be described as with the correlation time β g , the drift of the gyroscope in the body coordinate system ε b x , ε b y , and ε b z , and the white noise ω x , ω y , and ω z .

Error Equations of the Doppler Velocimeter.
For the fourbeam Doppler velocimeter, the calibration coefficient error is defined as δC, the velocity deviation error is defined as δv d , and the drift angle error is defined as δΔ. Then, the output of the Doppler velocimeter can be presented as where φ z is the azimuth angle, K v is the track angle, and v d is the true velocity.
Since δC, δv d , δΔ, andφ z are small, ignoring the small quantities of the second order, equation (6) can be simplified as Journal of Sensors with the correlation time β d and β Δ and the white noise ω d and ω Δ .

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, δλ, δv The state equation is presented as where W = ½0, 0, a x , a y , 0, 0, 0, ω x , ω y , ω z , ω d , ω Δ , 0 T is the noise matrix, A is the state-transition matrix, B is the noise distribution matrix, and Since the velocity is selected as the measurement, the measurement equation is presented as where From equations (7) and (12), it is calculated that where η x and η y are the system measured noises. Thus, equation (12) can be rewritten as where By discretizing the state (equation (9)) and measurement (equation (15)), the Kalman filter equation of the SINS/DVL integrated navigation system is obtained as where X k is the state vector at time k, Z k is the measurement vector at time k, H k is the measurement matrix at time k, V k is the system measurement noise matrix at time k, X k−1 is the state vector at time k − 1, W k−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 W k and V k are white Gaussian noises with W k ∼ WNðq k , Q k Þ, V k ∼ WNðr k , R k Þ, and ϕ k/k−1 = I 13×13 + AT, with the sampling period T.

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, 3 Journal of Sensors whose calculation loop is presented in Figure 1. The Sage-Husa adaptive Kalman filter is designed aŝ wherer with the forgetting factor b and b ∈ ð0:95~0:999Þ.
To suppress filter divergence,Q k must be nonnegative definite andR k must be positive definite. However, considering the existence of −Φ k/k−1 P k−1 Φ T k/k−1 and −H k P k/k−1 H T k in the above equation, the positive definiteness ofQ k andR k cannot be guaranteed. Since −Φ k/k−1 P k−1 Φ T k/k−1 and −H k P k/k−1 H T k 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 andR k are modified aŝ From equation (21), it is obtained that bothQ k andR k are affected by the innovation vector e k . That means that the filter accuracy cannot be guaranteed whenQ k andR k a5re changing abnormally with e k . In response to this problem,Q k is assumed to be known such thatR k can be accurately estimated.

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 filterbased SINS/DVL integrated navigation system for AUV.

Conclusion
For AUV, a modified Sage-Husa adaptive Kalman filterbased 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.

Data Availability
The data supporting the findings of this study are available within the article.