# Navigation Switching Strategy-Based SINS/GPS/ADS/DVL Fault-Tolerant Integrated Navigation System

Authors: Weiqi Li; Rui Zhang; Hongjie Lei
Journal: Journal of Sensors
 (2021)

---

## Abstract

Considering the conventional federated filtering-based fault-tolerant integrated navigation system is difficult to be implemented by serial data processing circuits, this paper presents navigation switching strategy-based SINS/GPS/ADS/DVL fault-tolerant integrated navigation system to guarantee the reliability of integrated navigation system under sensor faults. When sensor failure appears, SINS and fault-free sensors are selected successively to form an integrated navigation system, such that reliable navigation parameters can be obtained. The simulation tests are implemented to verify that the SINS/GPS/ADS/DVL integrated navigation system can provide reliable navigation parameters when ADS and DVL are disabled.


---

## Body

## 1. Introduction

At present, the navigation systems commonly used on aircraft include strapdown inertial navigation system (SINS), global satellite navigation system, and Doppler navigation system. Navigation technology mainly provides direction, position, speed, time, and other information for the motion carrier, which has been widely used in vehicle, ship, airliner, and other civil fields, and missile, aerospace, and other military fields, and is an important national information and strategic resource. In [1], the current space technology requires higher and higher reliability of navigation system. A single type of navigation system cannot meet the requirements of an engineering application. In this case, integrated navigation technology arises at the historic moment. In [2], an improved adaptive Kalman filter method for GPS signal misalignment is proposed, which solved the problem of serious discrepancy between the innovation covariance and the actual situation. In [3], a matrix weighted multisensor data fusion method with a two-level structure is adopted to realize INS/GNSS/CNS integrated navigation. However, in the centralized Kalman information fusion, the high-dimensional matrix operation is very complex, and once a navigation device fails, the whole system will be polluted and the filtering will be divergent [4].Fault-tolerant design is an important way to improve the reliability of an integrated navigation system [5–9]. In [10], it pointed out the application of INS/USBL/DVL integrated navigation federated filtering in three information fusion algorithms for the positioning error accumulation and fault tolerance of the underwater submarine inertial navigation system. In [11], aiming at the requirements of cruise missile for high precision and autonomy of navigation system and its uniform flight characteristics, a strapdown inertial/celestial navigation/synthetic aperture radar integrated navigation system is designed for cruise missile. The radar integrated navigation system adopts nonlinear modeling for integrated navigation, and the nonlinear unscented Kalman filter method is used to filter each subfilter, which greatly enhances the system’s fault tolerance and real-time performance. In [12], a distributed federated Kalman filter is proposed to enhance the system’s fault isolation and recovery capabilities. In [13], an integrated navigation system based on SINS/GPS/DVL by using a Kalman filter is proposed, which reduced the calculation time and enhanced the fault-tolerant performance of the system. The decentralized estimation method and its application in the fault-tolerant integrated navigation system are studied in [14]. The theory of intelligent fault-tolerant integrated navigation system is researched in [15]. A scheme of fault-tolerant integrated navigation system based on multisensor information fusion is proposed in [16]. The optimal estimation of all parameters and fault-tolerant processing of the system state are carried out. In [17], a distributed optimal fusion method combining the volume rules of motion model error identification and prediction is proposed to solve the nonlinear integration problem of MIMU/GNSS/CNS systems. [18] proposed a robust fault-tolerant federated filter for SINS/GNSS/CNS/DVL integrated navigation. In [19], an actuator fault detection and reconstruction scheme based on fault classification applied to hexagonal rotor UAV is designed. In [20], a fault detection and isolation (FDI) scheme based on distributed interactive multimodel is proposed to solve the problem of fault detection and isolation (FDI) of navigation sensors composed of inertial devices and camera sensors in robot systems.In addition, new technologies such as artificial intelligence, neural network, and computer vision have been used in multi-information fusion technology to improve the intelligence and visualization of navigation system. In the design of a federated filter, information distribution coefficient is a key factor, which directly affects the performance of federated filter. The influence of the information distribution coefficient on the system under the condition of subsystem failure is studied in [21] and proposed a method for robust information distribution based on the probability of each subsystem failure. In [22], the accuracy of the local filter is improved by estimating the Frobenius norm of the mean square error matrix and adaptively adjusting the information distribution coefficient. In order to improve the estimation quality, in [23], an ordered weighted average (OWA) operator integrated multi-rate Kalman filter is designed. A method of residual life prediction based on distributed neural network is proposed in [24]. The method has higher accuracy than centralized neural network. SAPD, a data-driven algorithm, is introduced into the anomaly detection and output reconstruction of the redundant inertial measurement unit (RIMU), which improves the detection accuracy of small faults [25]. Application of ensemble learning to industrial gas turbine fault detection and isolation strategy in [26].In practical engineering applications, considering that serial data processing circuits are difficult to implement the abovementioned federated filtering and system reliability cannot be guaranteed under the centralized Kalman filtering, an integrated fault-tolerant system based on navigation switching strategy is proposed in this paper, such that the reliability of SINS/GPS/ADS/DVL integrated navigation system is improved.The paper is organized as follows. The navigation switching strategy of the SINS/GPS/DVL/ADS integrated navigation system is designed in Section2. The Kalman filter equation and Kalman filter are given in Section 3. Simulations are conducted to verify the superiority of the proposed approach in Section 4. Conclusions are drawn in Section 5.


## 2. Navigation Switching Strategy of SINS/GPS/ADS/DVL Fault-Tolerant Integrated Navigation System

Considering the advantages of SINS, such as high navigation accuracy, full navigation parameters, strong autonomy, and strong anti-interference ability, it is used as the main equipment in navigation.The integrated fault-tolerant model based on navigation switching strategy is shown in Figure1. The system has four working modes, which are SINS/GPS/ADS/DVL integrated navigation system, SINS/GPS integrated navigation system, SINS/ADS integrated navigation system, and SINS/DVL integrated navigation system, respectively. When the system works normally, the four modes automatically switch to each other, and the main working mode of the system is the working mode of the SINS/GPS/ADS/DVL integrated navigation system. The working conditions and data accuracy of the three sensors are used as the switching criterion. When all the sensors are working properly, the SINS/GPS/ADS/DVL integrated navigation system is working. When the error data provided by GPS due to fault or the number of observable satellites (less than 4) cannot work, it is switched to SINS/ADS integrated navigation mode. When the error data provided by ADS due to fault, it is switched to SINS/DVL integrated navigation mode. The specific steps are as follows:Step 1.
Determine whether the GPS can receive more than 4 satellites (including 4) and whether ADS and DVL signals can be received normally. If all the signals are received, select SINS/GPS/ADS/DVL integrated navigation working mode. Otherwise, compare the difference between GPS, ADS, and DVL and inertial navigation measurement sequence.Step 2.
If GPS can work normally and the difference between GPS and inertial navigation measurement sequence is minimum, SINS/GPS combination mode is selected. If the number of GPS observable satellites is less than 4 or the difference between GPS and INS measurement sequence is too large, the SINS/GPS filtering is abandoned.Step 3.
If the ADS sensors work normally, the self-checking and fault detection system module shows that they work normally and the difference between ADS and INS measurement sequence is minimal, the SINS/ADS combination mode is selected; otherwise, the SINS/DVL combination mode is switched.Step 4.
Continue to judge the working status of GPS, ADS, and DVL in real-time and compare the difference between GPS, ADS, and DVL and INS measurement sequence in real-time.Figure 1
SINS/GPS/ADS/DVL fault-tolerant integrated navigation system based on navigation switching strategy.Figure 2
Attitudes of SINS/GPS/ADS/DVL fault-tolerant integrated navigation system.


## 3. Integrated Navigation System

### 3.1. State Equation of SINS/GPS, SINS/ADS, and SINS/DVL

Select the state variable of SINS as(1)XSINS=φE,φN,φU,δvE,δvN,δvU,δLSINS,δλSINS,δhSINS,εbx,εby,εbz,εrx,εry,εrz,∇bx,∇by,∇bzT,where φE, φN, φU, δvE, δvN, δvU, δLSINS, δλSINS, and δhSINS are SINS attitude error, speed error, and position error, respectively; εbx, εby, and εbz are gyro bias; εrx, εry, and εrz are first-order Markov processes. ∇bx, ∇by, and ∇bz are the constant drift of the accelerometer.Based on the error equation of SINS, there exists(2)ẊSINS=FSINSXSINS+GSINSWSINS,where WSINS∈R9×1 is the system noise, FSINS∈R18×18 is the state matrix, the nonzeros of FSINS is presented as(3)FSINS1,2=ωiesinL+ve/RN+htanL,FSINS1,3=−ωiecosL+ve/RN+h,FSINS1,5=−1/RM+h,FSINS1,9=RM+h2,FSINS1,10=−cosγcosψ+sinψsinθ,FSINS1,11=cosγsinψ−sinγcosψsinθ,FSINS1,12=sinγcosθ,FSINS1,13=Cbn1,1,FSINS1,14=Cbn1,2,FSINS1,15=Cbn1,3,FSINS2,1=−ωiesinL+ve/RN+htanL,FSINS2,3=−vn/RM+h,FSINS2,4=1/RN+h,FSINS2,7=−ωiesinL,FSINS2,9=−ve/RN+h2,FSINS2,10=−sinψcosθ,FSINS2,11=−cosψcosθ,FSINS2,12=−sinθ,FSINS2,13=Cbn2,1,FSINS2,14=Cbn2,2,FSINS2,15=Cbn2,3,FSINS3,1=ωiecosL+veRN+h,FSINS3,2=vnRM+h,FSINS3,4=tanLRN+h,FSINS3,7=ωiecosL+veRN+hsec2L,FSINS3,9=vetanLRN+h2,FSINS3,10=−sinγcosψ+cosγsinψsinθ,FSINS3,11=sinγsinψ+cosγcosψsinθ,FSINS3,12=−cosγcosθεbz,FSINS3,13=Cbn3,1,FSINS3,14=Cbn3,2,FSINS3,15=Cbn3,3,FSINS4,2=−fU,FSINS4,3=fN,FSINS4,4=vntanL−vuRN+h,FSINS4,5=2ωiesinL+veRN+htanL,FSINS4,6=−2ωiecosL+veRN+h,FSINS4,7=2ωievusinL+vncosLvevnRM+hsec2L,FSINS4,9=vevu−vevntanLRN+h2,FSINS4,16=cosγcosψ+sinψsinθ,FSINS4,17=−cosγsinψ+sinγcosψsinθ,FSINS4,18=−sinγcosθ,FSINS5,1=fU,FSINS5,3=−fE,FSINS5,4=−2ωiesinL−veRN+htanL,FSINS5,7=−2veωiecosL−ve2RN+hsec2L,FSINS5,5=−vuRM+h,FSINS5,6=−vnRM+h,FSINS5,9=vevnRM+h2+ve2tanLRN+h2,FSINS5,16=sinψcosθ,FSINS5,17=cosψcosθ,FSINS5,18=sinθ,FSINS6,1=−fN,FSINS6,2=fE,FSINS6,4=2ωiecosL+2veRN+h,FSINS6,5=2vnRM+h,FSINS6,7=−2veωiesinL,FSINS6,9=−vn2RM+h2−ve2RN+h2,FSINS6,16=sinγcosψ−cosγsinψsinθ,FSINS6,17=−sinγsinψ−cosγcosψsinθ,FSINS6,18=cosγcosθ,FSINS7,5=1RM+h,FSINS7,9=−vnRM+h2,FSINS8,4=secLRN+h,FSINS8,7=veRN+htanLsecL,FSINS8,9=−vesecLRN+h2,FSINS9,3=1,FSINS13,13=−1τg,FSINS14,14=−1τg,FSINS15,15=−1τg,and(4)GSINS=03×303×3Cbn06×306×306×303×303×303×303×3I3×303×3I3×303×303×3.Select the GPS state variable as(5)XGPS=σvGe,σvGn,σvGu,σLG,σλG,σHGT,where σvGe, σvGn, and σvGu are the speed error; σLG, σλG, and σHG are position error.Based on the error equation of GPS, there exists(6)ẊGPS=FGPSXGPS+GGPSWGPS,where FGPS=−diag1/τSAL1/τSAλ1/τSAh1/τSAvE1/τSAvN1/τSAvU, WGPS∈R6×1 is the system noise and GGPS=I6×6.Select the state variable of ADS as(7)XADS=vwrE,vwrN,vwrU,δhbT,where vwrE,vwrN,vwrU are the speed error; δhb is the height error.Based on the error equation of ADS, there exists(8)ẊADS=FADSXADS+GADSWADS,where FADS=−diag1/τwE1/τwN1/τwU1/τb, WADS∈R4×1 is the system noise and GADS=I4×4.Select the state variable of DVL as(9)XDVL=δvdx,δvdy,δvdzT,where δvdx, δvdy, and δvdz are the speed measurement errors caused by terrain changes.Based on the error equation of DVL, there exists(10)ẊDVL=FDVLXDVL+GDVLWDVL,where FDVL=−diag1/τdx1/τdy1/τdz, WDVL∈R3×1 is the system noise and GDVL=I3×3.The state equation of the SINS/GPS integrated navigation subsystem is obtained as(11)ẊSINSẊGPS=F1XSINSXGPS+G1WSINSWGPS,where(12)F1=FSINS018×606×18FGPS,G1=GSINS018×606×9GGPS.The state equation of the SINS/ADS integrated navigation subsystem is obtained as(13)ẊSINSẊADS=F2XSINSXADS+G2WSINSWADS,where(14)F2=FSINS018×404×18FADS,G2=GSINS018×404×9GADS.The state equation of the SINS/DVL integrated navigation subsystem is obtained as(15)ẊSINSẊDVL=F3XSINSXDVL+G3WSINSWDVL,where(16)F3=FSINS018×303×18FDVL,G3=GSINS018×303×9GDVL.


### 3.2. Measurement Equation of SINS/GPS, SINS/ADS, and SINS/DVL

The SINS/GPS integrated navigation subsystem makes the difference between the speed and position information output by the GPS navigator and the SINS speed and position information to obtain the measurement equation as(17)ZGPS=vESINS−vEGPSvNSINS−vNGPSvUSINS−vUGPSLSINS−LGPSλSINS−λGPShSINS−hGPS=δvE−δvESA+VG1δvN−δvNSA+VG2δvU−δvUSA+VG3δLSINS−δLGPS+VG4δλSINS−δλGPS+VG5δhSINS−δhGPS+VG6=HSINSHGPSXSINSXGPS+ΔVG,where HSINS=06×3I6×606×9, HGPS=−I6×6, and ΔVG=VG1VG2VG3VG4VG5VG6T are the noise.The SINS/ADS integrated navigation subsystem makes the difference between the speed and altitude information output by ADS and the SINS speed and altitude information to obtain the measurement equation as(18)ZADS=vUSINS−vUADShSINS−hADS=δvU−φNve+φEvn−vwrU+V1δhSINS−δhb+V2=HSINS∗HADSXSINSXADS+ΔVA,where(19)HSINS∗=vn−ve000100001×1100000000101×11HADS=−I2×2, and(20)ΔVA=V1V2are the measurement noise.The SINS/DVL integrated navigation subsystem makes the difference between the speed information output by DVL and the speed information of SINS and obtains the measurement equation as(21)ZDVL=vESINS−vEDVSvNSINS−vNDVSvUSINS−vUDVS=δvE+φNvU−φUvN+VD1δvN+φUvE−φEvU+VD2δvU+φNvE−φEvN+VD3−Cbn1,1δvdx+Cbn1,2δvdy+Cbn1,3δvdzCbn2,1δvdx+Cbn2,2δvdy+Cbn2,3δvdzCbn3,1δvdx+Cbn3,2δvdy+Cbn3,3δvdz=HSINS#HDVLXSINSXDVL+ΔVD,where(22)HSINS#=0vu−vn10001×12−vu0ve01001×12−vnve000101×12,HDVL=−03×18Cbn, and ΔVD=VD1VD2VD3T are the measurement noise.


### 3.3. Kalman Filter

Discretizing the state equations (11)–(15) and the measurement equations (17)–(21), the Kalman filter equation of each integrated navigation system can be obtained as(23)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, Wk and Vk are white Gaussian noises with Wk∼WNqk,Qk, Vk∼WNrk,Rk, and(24)ϕk/k−1=I+FTΓk−1=GTHk=Hwith the sampling period T.The Kalman filter is designed as(25)X^k/k−1=Φk/k−1X^k−1,Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1T,ek=Zk−HkX^k/k−1,Kk=Pk/k−1HkTHkPk/k−1HkT+Rk−1−1,X^k=X^k/k−1+Kkek,Pk=1−KkHkPk/k−11−KkHkT+KkRkKkT,


## 3.1. State Equation of SINS/GPS, SINS/ADS, and SINS/DVL

Select the state variable of SINS as(1)XSINS=φE,φN,φU,δvE,δvN,δvU,δLSINS,δλSINS,δhSINS,εbx,εby,εbz,εrx,εry,εrz,∇bx,∇by,∇bzT,where φE, φN, φU, δvE, δvN, δvU, δLSINS, δλSINS, and δhSINS are SINS attitude error, speed error, and position error, respectively; εbx, εby, and εbz are gyro bias; εrx, εry, and εrz are first-order Markov processes. ∇bx, ∇by, and ∇bz are the constant drift of the accelerometer.Based on the error equation of SINS, there exists(2)ẊSINS=FSINSXSINS+GSINSWSINS,where WSINS∈R9×1 is the system noise, FSINS∈R18×18 is the state matrix, the nonzeros of FSINS is presented as(3)FSINS1,2=ωiesinL+ve/RN+htanL,FSINS1,3=−ωiecosL+ve/RN+h,FSINS1,5=−1/RM+h,FSINS1,9=RM+h2,FSINS1,10=−cosγcosψ+sinψsinθ,FSINS1,11=cosγsinψ−sinγcosψsinθ,FSINS1,12=sinγcosθ,FSINS1,13=Cbn1,1,FSINS1,14=Cbn1,2,FSINS1,15=Cbn1,3,FSINS2,1=−ωiesinL+ve/RN+htanL,FSINS2,3=−vn/RM+h,FSINS2,4=1/RN+h,FSINS2,7=−ωiesinL,FSINS2,9=−ve/RN+h2,FSINS2,10=−sinψcosθ,FSINS2,11=−cosψcosθ,FSINS2,12=−sinθ,FSINS2,13=Cbn2,1,FSINS2,14=Cbn2,2,FSINS2,15=Cbn2,3,FSINS3,1=ωiecosL+veRN+h,FSINS3,2=vnRM+h,FSINS3,4=tanLRN+h,FSINS3,7=ωiecosL+veRN+hsec2L,FSINS3,9=vetanLRN+h2,FSINS3,10=−sinγcosψ+cosγsinψsinθ,FSINS3,11=sinγsinψ+cosγcosψsinθ,FSINS3,12=−cosγcosθεbz,FSINS3,13=Cbn3,1,FSINS3,14=Cbn3,2,FSINS3,15=Cbn3,3,FSINS4,2=−fU,FSINS4,3=fN,FSINS4,4=vntanL−vuRN+h,FSINS4,5=2ωiesinL+veRN+htanL,FSINS4,6=−2ωiecosL+veRN+h,FSINS4,7=2ωievusinL+vncosLvevnRM+hsec2L,FSINS4,9=vevu−vevntanLRN+h2,FSINS4,16=cosγcosψ+sinψsinθ,FSINS4,17=−cosγsinψ+sinγcosψsinθ,FSINS4,18=−sinγcosθ,FSINS5,1=fU,FSINS5,3=−fE,FSINS5,4=−2ωiesinL−veRN+htanL,FSINS5,7=−2veωiecosL−ve2RN+hsec2L,FSINS5,5=−vuRM+h,FSINS5,6=−vnRM+h,FSINS5,9=vevnRM+h2+ve2tanLRN+h2,FSINS5,16=sinψcosθ,FSINS5,17=cosψcosθ,FSINS5,18=sinθ,FSINS6,1=−fN,FSINS6,2=fE,FSINS6,4=2ωiecosL+2veRN+h,FSINS6,5=2vnRM+h,FSINS6,7=−2veωiesinL,FSINS6,9=−vn2RM+h2−ve2RN+h2,FSINS6,16=sinγcosψ−cosγsinψsinθ,FSINS6,17=−sinγsinψ−cosγcosψsinθ,FSINS6,18=cosγcosθ,FSINS7,5=1RM+h,FSINS7,9=−vnRM+h2,FSINS8,4=secLRN+h,FSINS8,7=veRN+htanLsecL,FSINS8,9=−vesecLRN+h2,FSINS9,3=1,FSINS13,13=−1τg,FSINS14,14=−1τg,FSINS15,15=−1τg,and(4)GSINS=03×303×3Cbn06×306×306×303×303×303×303×3I3×303×3I3×303×303×3.Select the GPS state variable as(5)XGPS=σvGe,σvGn,σvGu,σLG,σλG,σHGT,where σvGe, σvGn, and σvGu are the speed error; σLG, σλG, and σHG are position error.Based on the error equation of GPS, there exists(6)ẊGPS=FGPSXGPS+GGPSWGPS,where FGPS=−diag1/τSAL1/τSAλ1/τSAh1/τSAvE1/τSAvN1/τSAvU, WGPS∈R6×1 is the system noise and GGPS=I6×6.Select the state variable of ADS as(7)XADS=vwrE,vwrN,vwrU,δhbT,where vwrE,vwrN,vwrU are the speed error; δhb is the height error.Based on the error equation of ADS, there exists(8)ẊADS=FADSXADS+GADSWADS,where FADS=−diag1/τwE1/τwN1/τwU1/τb, WADS∈R4×1 is the system noise and GADS=I4×4.Select the state variable of DVL as(9)XDVL=δvdx,δvdy,δvdzT,where δvdx, δvdy, and δvdz are the speed measurement errors caused by terrain changes.Based on the error equation of DVL, there exists(10)ẊDVL=FDVLXDVL+GDVLWDVL,where FDVL=−diag1/τdx1/τdy1/τdz, WDVL∈R3×1 is the system noise and GDVL=I3×3.The state equation of the SINS/GPS integrated navigation subsystem is obtained as(11)ẊSINSẊGPS=F1XSINSXGPS+G1WSINSWGPS,where(12)F1=FSINS018×606×18FGPS,G1=GSINS018×606×9GGPS.The state equation of the SINS/ADS integrated navigation subsystem is obtained as(13)ẊSINSẊADS=F2XSINSXADS+G2WSINSWADS,where(14)F2=FSINS018×404×18FADS,G2=GSINS018×404×9GADS.The state equation of the SINS/DVL integrated navigation subsystem is obtained as(15)ẊSINSẊDVL=F3XSINSXDVL+G3WSINSWDVL,where(16)F3=FSINS018×303×18FDVL,G3=GSINS018×303×9GDVL.


## 3.2. Measurement Equation of SINS/GPS, SINS/ADS, and SINS/DVL

The SINS/GPS integrated navigation subsystem makes the difference between the speed and position information output by the GPS navigator and the SINS speed and position information to obtain the measurement equation as(17)ZGPS=vESINS−vEGPSvNSINS−vNGPSvUSINS−vUGPSLSINS−LGPSλSINS−λGPShSINS−hGPS=δvE−δvESA+VG1δvN−δvNSA+VG2δvU−δvUSA+VG3δLSINS−δLGPS+VG4δλSINS−δλGPS+VG5δhSINS−δhGPS+VG6=HSINSHGPSXSINSXGPS+ΔVG,where HSINS=06×3I6×606×9, HGPS=−I6×6, and ΔVG=VG1VG2VG3VG4VG5VG6T are the noise.The SINS/ADS integrated navigation subsystem makes the difference between the speed and altitude information output by ADS and the SINS speed and altitude information to obtain the measurement equation as(18)ZADS=vUSINS−vUADShSINS−hADS=δvU−φNve+φEvn−vwrU+V1δhSINS−δhb+V2=HSINS∗HADSXSINSXADS+ΔVA,where(19)HSINS∗=vn−ve000100001×1100000000101×11HADS=−I2×2, and(20)ΔVA=V1V2are the measurement noise.The SINS/DVL integrated navigation subsystem makes the difference between the speed information output by DVL and the speed information of SINS and obtains the measurement equation as(21)ZDVL=vESINS−vEDVSvNSINS−vNDVSvUSINS−vUDVS=δvE+φNvU−φUvN+VD1δvN+φUvE−φEvU+VD2δvU+φNvE−φEvN+VD3−Cbn1,1δvdx+Cbn1,2δvdy+Cbn1,3δvdzCbn2,1δvdx+Cbn2,2δvdy+Cbn2,3δvdzCbn3,1δvdx+Cbn3,2δvdy+Cbn3,3δvdz=HSINS#HDVLXSINSXDVL+ΔVD,where(22)HSINS#=0vu−vn10001×12−vu0ve01001×12−vnve000101×12,HDVL=−03×18Cbn, and ΔVD=VD1VD2VD3T are the measurement noise.


## 3.3. Kalman Filter

Discretizing the state equations (11)–(15) and the measurement equations (17)–(21), the Kalman filter equation of each integrated navigation system can be obtained as(23)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, Wk and Vk are white Gaussian noises with Wk∼WNqk,Qk, Vk∼WNrk,Rk, and(24)ϕk/k−1=I+FTΓk−1=GTHk=Hwith the sampling period T.The Kalman filter is designed as(25)X^k/k−1=Φk/k−1X^k−1,Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1T,ek=Zk−HkX^k/k−1,Kk=Pk/k−1HkTHkPk/k−1HkT+Rk−1−1,X^k=X^k/k−1+Kkek,Pk=1−KkHkPk/k−11−KkHkT+KkRkKkT,


## 4. Simulation

In this section, simulations are presented to verify that the reliability of SINS/GPS/ADS/DVL fault-tolerant integrated navigation system can be guaranteed under in the presence of sensor faults.


### 4.1. Fault Scenarios

The flight data comes from the cooperative project with the AVIC Chengdu Aircraft Design and Research Institute. For the gyroscopes of SINS, the constant drift is0.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. For GPS, the velocity measurement error is 0.5m/s, and the position measurement error is 1m. For ADS, the velocity measurement error is 2m/s, and the height measurement error is 100m. For DVL, the velocity measurement error is 0.5%. The flight process of the aircraft is divided into three stages: climb, steady flight, and descent. In each stage, faults are imposed on the altitude data of the sensors, respectively. The fault type and size of sensors are presented in Table 1.Figure 3
Velocities of SINS/GPS/ADS/DVL fault-tolerant integrated navigation system.Table 1
Fault type and size.
Time (t)660 s ~900 s1080s~1200s1350s~1725s3000 s ~3120 sFault sensorGPSGPSGPSADS, DVLFault typeSoft faultLack of satellitesSoft faultHard faultFault size0.001t−660(m)~0.005t−1350(m)5000‐h(m)Figure 4
Positions of SINS/GPS/ADS/DVL fault-tolerant integrated navigation systemFigure 5
Heights of all integrated navigation system.


### 4.2. Simulation Results

The simulations are presented in Figures2–5. From Figures 2–4, the attitudes, velocities, and position measurement of the integrated navigation system can track the expected navigation parameters, where pitch angle tracking is with much errors due to the original data is with measurement error. Since the slow varying faults increase over time, the error is very small at the beginning, and the switching system will not switch at this time. Because there is a certain error between the height data output by ADS and the real data itself, the switching is carried out when the error increases to a larger error than that of the SINS/GPS navigation system. As shown in Figure 4, in the steady flight phase, the GPS altitude data is slowly changed from the 1080s to 1692s. At about 1693s, the GPS altitude data error is greater than the ADS altitude data error. At this time, switch to select the SINS/ADS integrated navigation system. As for the hard fault in 3000 s ~3120 s, SINS/GPS integrated navigation system is selected. In a word, the expected navigation reliability of the SINS/GPS/ADS/DVL integrated navigation system is ensured using the proposed navigation switching strategy.


## 4.1. Fault Scenarios

The flight data comes from the cooperative project with the AVIC Chengdu Aircraft Design and Research Institute. For the gyroscopes of SINS, the constant drift is0.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. For GPS, the velocity measurement error is 0.5m/s, and the position measurement error is 1m. For ADS, the velocity measurement error is 2m/s, and the height measurement error is 100m. For DVL, the velocity measurement error is 0.5%. The flight process of the aircraft is divided into three stages: climb, steady flight, and descent. In each stage, faults are imposed on the altitude data of the sensors, respectively. The fault type and size of sensors are presented in Table 1.Figure 3
Velocities of SINS/GPS/ADS/DVL fault-tolerant integrated navigation system.Table 1
Fault type and size.
Time (t)660 s ~900 s1080s~1200s1350s~1725s3000 s ~3120 sFault sensorGPSGPSGPSADS, DVLFault typeSoft faultLack of satellitesSoft faultHard faultFault size0.001t−660(m)~0.005t−1350(m)5000‐h(m)Figure 4
Positions of SINS/GPS/ADS/DVL fault-tolerant integrated navigation systemFigure 5
Heights of all integrated navigation system.


## 4.2. Simulation Results

The simulations are presented in Figures2–5. From Figures 2–4, the attitudes, velocities, and position measurement of the integrated navigation system can track the expected navigation parameters, where pitch angle tracking is with much errors due to the original data is with measurement error. Since the slow varying faults increase over time, the error is very small at the beginning, and the switching system will not switch at this time. Because there is a certain error between the height data output by ADS and the real data itself, the switching is carried out when the error increases to a larger error than that of the SINS/GPS navigation system. As shown in Figure 4, in the steady flight phase, the GPS altitude data is slowly changed from the 1080s to 1692s. At about 1693s, the GPS altitude data error is greater than the ADS altitude data error. At this time, switch to select the SINS/ADS integrated navigation system. As for the hard fault in 3000 s ~3120 s, SINS/GPS integrated navigation system is selected. In a word, the expected navigation reliability of the SINS/GPS/ADS/DVL integrated navigation system is ensured using the proposed navigation switching strategy.


## 5. Conclusion

In this paper, a navigation switching strategy-based SINS/GPS/ADS/DVL fault-tolerant integrated navigation system is designed to guarantee navigation reliability. Considering the soft faults and the hard faults, simulations are presented to verify that expected navigation reliability can be obtained using the proposed navigation switching strategy.



---

*Source: 9943370-2021-08-03.xml*