## Modelling, Control, and Information Processing Technology in Gyroscopes

View this Special IssueResearch Article | Open Access

Ruixin Liu, Fucheng Liu, Chunning Liu, Pengchao Zhang, "Modified Sage-Husa Adaptive Kalman Filter-Based SINS/DVL Integrated Navigation System for AUV", *Journal of Sensors*, vol. 2021, Article ID 9992041, 8 pages, 2021. https://doi.org/10.1155/2021/9992041

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

**Academic Editor:**Juntao Fei

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

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

#### 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 as and , the longitude error and latitude error as and , the east and north velocity as and , the east and north velocity error as and , the earth radius as , the angular velocity of the earth as , the output of accelerometer in the navigation coordinate system as , , and , the horizontal drift of the accelerometer in the navigation coordinate system as and , and the drift of the gyroscope in the navigation coordinate system as , , and . 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 is the attitude matrix, and the drift of the gyroscope in the body coordinate system can be described as with the correlation time , the drift of the gyroscope in the body coordinate system , , and , and the white noise , , and .

##### 2.2. Error Equations of the Doppler Velocimeter

For the four-beam Doppler velocimeter, the calibration coefficient error is defined as , the velocity deviation error is defined as , and the drift angle error is defined as . Then, the output of the Doppler velocimeter can be presented as where is the azimuth angle, is the track angle, and is the true velocity.

Since , , , and are small, ignoring the small quantities of the second order, equation (6) can be simplified as where with the correlation time and and the white noise 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 as. The state equation is presented as where is the noise matrix, is the state-transition matrix, is the noise distribution matrix, and with

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 and 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 is the state vector at time , is the measurement vector at time , is the measurement matrix at time , is the system measurement noise matrix at time , is the state vector at time , is the noise matrix at time , is the noise distribution matrix at time , is the state one-step transition matrix, and and are white Gaussian noises with , , and with the sampling period .

##### 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 Figure 1. The Sage-Husa adaptive Kalman filter is designed as where

with the forgetting factor and .

To suppress filter divergence, must be nonnegative definite and must be positive definite. However, considering the existence of and in the above equation, the positive definiteness of and cannot be guaranteed. Since and 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, and are modified as

From equation (21), it is obtained that both and are affected by the innovation vector . That means that the filter accuracy cannot be guaranteed when and a5re changing abnormally with . In response to this problem, is assumed to be known such that 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 as . For the gyroscopes of SINS, the constant drift is and the random drift is . For the accelerometers of SINS, the constant drift is and the random drift is . The velocity measurement error of DVL is . The initial covariance matrix is set as, the initial variance matrix of state noise is set as with , and the variance matrix of measurement noise is set as .

The simulations are presented in Figures 2–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.

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

#### Data Availability

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

#### Conflicts of Interest

The authors declare that they have no conflicts of interest.

#### References

- A. Razzaqi and G. Farid, “A review on optimal placement of sensors for cooperative localization of AUVs,”
*Journal of Sensors*, vol. 2019, Article ID 4276987, 12 pages, 2019. View at: Google Scholar - B. Wang, J. Liu, and P. Liu, “A review of SINS/DVL integrated navigation technology,”
*Journal of Navigation and Positioning*, vol. 8, no. 3, pp. 1–6, 2020. View at: Google Scholar - P. Tsai, R. Tsai, and S. Wang, “Hybrid localization approach for underwater sensor networks,”
*Journal of Sensors*, vol. 2017, Article ID 5768651, 13 pages, 2017. View at: Publisher Site | Google Scholar - D. Wang, X. Xu, Y. Yao, T. Zhang, and Y. Zhu, “A novel SINS/DVL tightly integrated navigation method for complex environment,”
*IEEE Transactions on Instrumentation and Measurement*, vol. 69, no. 7, pp. 5183–5196, 2020. View at: Publisher Site | Google Scholar - T. Yoo, M. Kim, S. Yoon, and D. Kim, “Performance enhancement for conventional tightly coupled INS/DVL navigation system using regeneration of partial DVL measurements,”
*Journal of Sensors*, vol. 2020, Article ID 5324349, 15 pages, 2020. View at: Publisher Site | Google Scholar - Z. Lv, K. Tang, and M. Wu, “Online estimation of DVL misalignment angle in SINS/DVL integrated navigation system,” in
*International Conference on Electronic Measurement and Instruments*, pp. 336–339, Chengdu, 2011. View at: Google Scholar - Q. Liu, X. Xu, and B. Han, “An integrated navigation method based on SINS/DVL-WT for AUV,”
*Measurement and Intelligent Materials*, pp. 895–898, 2013. View at: Google Scholar - L. Chang and B. Hu, “Robust initial attitude alignment for SINS/DVL,”
*IEEE/ASME Transactions on Mechatronics*, vol. 23, no. 4, pp. 2016–2021, 2018. View at: Publisher Site | Google Scholar - X. Xu, J. Gui, Y. Sun, Y. Yao, and T. Zhang, “A robust in-motion alignment method with inertial sensors and Doppler velocity log,”
*IEEE Transactions on Instrumentation and Measurement*, vol. 70, article 8500413, 2021. View at: Google Scholar - L. Luo, Y. Huang, Z. Zhang, and Y. Zhang, “A new Kalman filter-based in-motion initial alignment method for DVL-aided low-cost SINS,”
*IEEE Transactions on Vehicular Technology*, vol. 70, no. 1, pp. 331–343, 2021. View at: Publisher Site | Google Scholar - L. Hou, X. Xu, Y. Yao, and D. Wang, “An improved interacting multiple model algorithm for INS/DVL integrated navigation system,” in
*International Conference on Sensing, Measurement and Data Analytics in the era of Artificial Intelligence (ICSMD)*, pp. 39–44, Xi'an, China, 2020. View at: Google Scholar - Y. Guo, M. Wu, K. Tang, and L. Zhang, “Square-root unscented information filter and its application in SINS/DVL integrated navigation,”
*Sensors*, vol. 18, no. 7, p. 2069, 2018. View at: Publisher Site | Google Scholar - H. Zong, Z. Gao, W. Wei, Y. Zhong, and C. Gu, “Randomly weighted CKF for multisensor integrated systems,”
*Journal of Sensors*, vol. 2012, Article ID 1216838, 19 pages, 2019. View at: Google Scholar - A. Zhang and Y. Liu, “Research for navigation technology of underwater robot based on UKF federated filter,” in
*IEEE International Conference on Computer Science and Automation Engineering*, pp. 729–733, Shanghai, 2011. View at: Google Scholar - W. Li, J. Wang, L. Lu, and W. Wu, “A novel scheme for DVL-aided SINS in-motion alignment using UKF techniques,”
*Sensors*, vol. 13, no. 1, pp. 1046–1063, 2013. View at: Publisher Site | Google Scholar - Y. Yang, X. Yan, and Q. Luo, “A SINS/DVL integrated navigation positioning method based on improved adaptive filtering technology,” in
*IEEE International Conference on Smart Internet of Things (SmartIoT)*, pp. 262–268, Tianjin, China, 2019. View at: Google Scholar - L. Luo, Y. Zhang, T. Fang, and N. Li, “A new robust Kalman filter for SINS/DVL integrated navigation system,”
*IEEE Access*, vol. 7, pp. 51386–51395, 2019. View at: Publisher Site | Google Scholar - Z. Lu, W. Wenqi, and G. Yan, “On SINS/DVL integrated navigation based on an adaptive outlier-restrained Kalman filter,” in
*IEEE CSAA Guidance, Navigation and Control Conference (CGNCC)*, pp. 1–6, Xiamen, China, 2018. View at: Google Scholar - D. Wang, X. Xu, and L. Hou, “An improved adaptive Kalman filter for underwater SINS/DVL system,”
*Mathematical Problems in Engineering*, vol. 2020, Article ID 5456961, 14 pages, 2020. View at: Google Scholar - X. Yan, Y. Yang, Q. Luo, Y. Chen, and C. Hu, “A SINS/DVL integrated positioning system through filtering gain compensation adaptive filtering,”
*Sensors*, vol. 19, no. 20, p. 4576, 2019. View at: Publisher Site | Google Scholar

#### Copyright

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