A Fault-Tolerant Polar Grid SINS/DVL/USBL Integrated Navigation Algorithm Based on the Centralized Filter and Relative Position Measurement

Navigation is a precondition for ocean space vehicles to work safely in polar regions. The traditional polar algorithms employ the grid strapdown inertial navigation system (SINS) as the backbone and Doppler velocity log (DVL) output velocity as measurements to constitute the integrated navigation system, of which, however, the position errors still accumulate with time. The ultra-short baseline (USBL) position system can provide position information that can be used to improve the performance of the SINS/DVL integrated system. Therefore, a grid SINS/DVL/USBL integrated algorithm for polar navigation is proposed in this paper. In order to extend the availability of the USBL and improve integration accuracy in polar regions, the USBL observation model is established based on the relative position measurement firstly. Then, a grid SINS/DVL/USBL integrated algorithm is proposed to fuse the information of these sensors with a modified Kalman filter (MKF) dealing with the sparse USBL output. Finally, a vector fault detection method, which takes the measurements as detection objects instead of the filter, is designed to locate the measurement fault and can be employed by the centralized filter to improve the fault-tolerant. Simulation and experiment results show that the proposed grid SINS/DVL/USBL integrated navigation system can further restrain SINS errors especially the position errors effectively. Meanwhile, the vector fault detection method can detect and isolate the fault measurements of centralized filter immediately and accurately. Therefore, the proposed fault-tolerant grid SINS/DVL/USBL integrated navigation algorithm can improve the reliability and accuracy of polar navigation for ocean space application.


Introduction
More and more significant scientific research is carried out in the ocean of the polar regions [1,2]. The high-precision and highly reliable navigation is the precondition for ocean space vehicles to operate normally and safely [3]. The strapdown inertial navigation system (SINS) has been widely used for polar vehicles, especially for the underwater vehicles due to its highly autonomous [4]. However, the SINS output contains periodic oscillation errors and accumulated errors. Navigation with a single sensor or a single system is often insufficient, so the SINS-based integrated navigation technology is a potential method for polar navigation [5][6][7][8].
The traditional integrated navigation algorithms are based on a north-oriented geographic framework and will lose efficacy in polar regions because of the meridian convergence [9]. A grid frame and the grid SINS algorithm have been proposed in [10], which are employed in this paper to solve the problem of meridian convergence. In polar regions, the geomagnetic navigations may abundant as in the low and middle latitude regions, and the effective measurements should be utilized fully to obtain a proper navigation performance. Therefore, in order to obtain proper accuracy and maintain high reliability, the centralized filters together with a novel detection method are more proper choice for polar vehicles than the federal filter with traditional scalar detection methods. The novel detection method should take the measurement as object to deal with measurement fault and improve the utilization of effective measurements.
In this paper, a fault-tolerant grid SINS/DVL/USBL integrated navigation algorithm is proposed to improve the navigation reliability and accuracy in polar regions for ocean space application. The DVL and USBL outputs are introduced as measurements, and the USBL observation model is designed based on the grid frame with relative positions as measurements to overcome the position task. Then the grid SINS/DVL/USBL centralized filter model is proposed for polar navigation. Besides, to deal with the sparse USBL output measurements, an MKF is employed to maintain the high and stable filter update frequency and accuracy. Finally, a vector fault detection method is designed to improve the fault-tolerant of integration. In the vector method, the fault detection vector is designed based on the classified measurements. Each element of the fault detection vector represents the quality of the corresponding measurement group. Therefore, the measurement is taken as the object to be detected and isolated instead of the filter, which can improve the utilization of effective measurements and fault-tolerant of the filter, especially the centralized filter. All of the design in the above will improve the accuracy and reliability of the integrated navigation system for vehicles in polar regions, and some experiments are conducted to validate the performance of the proposed integrated navigation algorithm.
The remainder sections are organized as follows. Section 2 describes the grid SINS and USBL position system. The grid SINS/DVL/USBL integrated filter model is proposed in Section 3. Section 4 explores the MKF filter algorithm and vector fault detection method. Simulations and semi-physical experiments are conducted in Section 5 to validate the performance of the proposed integrated navigation algorithm. Finally, Section 6 outlines the conclusions and future works.

The Grid SINS and USBL Position System
The grid SINS, which is chosen as a backbone navigation system, is employed with the relative position from the USBL and velocity from the DVL to compose an integrated system for the polar navigation. Besides, some different frames are reviewed here, i.e., the inertial frame i, the earth centred earth fixed frame ECEF(e), the geographic frame g, the grid frame G, the body frame b, and the USBL array frame u.

The Grid Frame and Grid SINS Mechanization
As shown in Figure 1, the grid SINS chooses the right-handed grid frame as the navigation frame. utilized fully to obtain a proper navigation performance. Therefore, in order to obtain proper accuracy and maintain high reliability, the centralized filters together with a novel detection method are more proper choice for polar vehicles than the federal filter with traditional scalar detection methods. The novel detection method should take the measurement as object to deal with measurement fault and improve the utilization of effective measurements. In this paper, a fault-tolerant grid SINS/DVL/USBL integrated navigation algorithm is proposed to improve the navigation reliability and accuracy in polar regions for ocean space application. The DVL and USBL outputs are introduced as measurements, and the USBL observation model is designed based on the grid frame with relative positions as measurements to overcome the position task. Then the grid SINS/DVL/USBL centralized filter model is proposed for polar navigation. Besides, to deal with the sparse USBL output measurements, an MKF is employed to maintain the high and stable filter update frequency and accuracy. Finally, a vector fault detection method is designed to improve the fault-tolerant of integration. In the vector method, the fault detection vector is designed based on the classified measurements. Each element of the fault detection vector represents the quality of the corresponding measurement group. Therefore, the measurement is taken as the object to be detected and isolated instead of the filter, which can improve the utilization of effective measurements and fault-tolerant of the filter, especially the centralized filter. All of the design in the above will improve the accuracy and reliability of the integrated navigation system for vehicles in polar regions, and some experiments are conducted to validate the performance of the proposed integrated navigation algorithm.
The remainder sections are organized as follows. Section 2 describes the grid SINS and USBL position system. The grid SINS/DVL/USBL integrated filter model is proposed in Section 3. Section 4 explores the MKF filter algorithm and vector fault detection method. Simulations and semi-physical experiments are conducted in Section 5 to validate the performance of the proposed integrated navigation algorithm. Finally, Section 6 outlines the conclusions and future works.

The Grid SINS and USBL Position System
The grid SINS, which is chosen as a backbone navigation system, is employed with the relative position from the USBL and velocity from the DVL to compose an integrated system for the polar navigation. Besides, some different frames are reviewed here, i.e. the inertial frame i , the earth centred earth fixed frame ( ) ECEF e , the geographic frame g , the grid frame G, the body frame b , and the USBL array frame u .

The Grid Frame and Grid SINS Mechanization
As shown in Figure 1, the grid SINS chooses the right-handed grid frame as the navigation frame.   A grid north axis (y G -axis), which is parallel to the Greenwich plane, is employed instead of the geographic north, and the angle between the grid north and geographic north is σ. The other detail descriptions of the grid frame can be found in [10].
The mechanization of grid SINS attitude, velocity, and position are: . .

The USBL Position System
As shown in Figure 2, the USBL position system consists of four vehicle-mounted hydrophones and a known-location transponder (denoted as r). The four hydrophones are arranged in two orthogonal baselines and make up the USBL array. The intersection point of two baselines is the origin of the USBL array frame (u frame) and the right-handed u frame are then shown as follows: A grid north axis ( G y -axis), which is parallel to the Greenwich plane, is employed instead of the geographic north, and the angle between the grid north and geographic north isσ . The other detail descriptions of the grid frame can be found in [10]. The mechanization of grid SINS attitude, velocity, and position are: The detailed processing and physical variables of (1), (2) and (3) can refer to [10].

The USBL Position System
As shown in Figure 2, the USBL position system consists of four vehicle-mounted hydrophones and a known-location transponder (denoted as r ). The four hydrophones are arranged in two orthogonal baselines and make up the USBL array. The intersection point of two baselines is the origin of the USBL array frame ( u frame) and the right-handed u frame are then shown as follows: It is assumed that the sound velocity and wavelength under water are known. Then, the sound signals transmit between the USBL array and the transponder to measurement the relative position between them. As shown in Figure 2, the relative position between the USBL array and the transponder can be presented as [ ] R α β , where α is the altitude angle, and β is the azimuth angle, and R is the range.

Design of the Grid SINS/DVL/USBL Integrated Filter Model with Relative Position Measurements
As discussed above, the single navigation device cannot maintain the accuracy of polar navigation. Therefore, a polar grid SINS/DVL/USBL integrated navigation algorithm is proposed to enhance the navigation performance for vehicles. In this section, the grid SINS/DVL/USBL filter model is designed for the proposed integrated algorithm. The DVL provides the absolute velocity It is assumed that the sound velocity and wavelength under water are known. Then, the sound signals transmit between the USBL array and the transponder to measurement the relative position between them. As shown in Figure 2, the relative position between the USBL array and the transponder can be presented as α β R , where α s the altitude angle, and β is the azimuth angle, and R is the range.

Design of the Grid SINS/DVL/USBL Integrated Filter Model with Relative Position Measurements
As discussed above, the single navigation device cannot maintain the accuracy of polar navigation. Therefore, a polar grid SINS/DVL/USBL integrated navigation algorithm is proposed to enhance the navigation performance for vehicles. In this section, the grid SINS/DVL/USBL filter model is designed for the proposed integrated algorithm. The DVL provides the absolute velocity while the USBL provides the relative position α β R as the external navigation information. Figure 3 shows the diagram of the proposed grid SINS/DVL/USBL integrated navigation algorithm. The design details of the algorithm will be discussed in the following subsections and Section 4. while the USBL provides the relative position [ ] R α β as the external navigation information. Figure 3 shows the diagram of the proposed grid SINS/DVL/USBL integrated navigation algorithm. The design details of the algorithm will be discussed in the following subsections and Section 4.

Design of the Dynamic Models
The dynamic models of the integrated navigation system include the grid SINS, DVL and USBL models, which can be described by state equations as follows: ; G is the noise transition matrix and  The detail of the dynamic models will be described in the follow sub-sections, where the dynamic model of the USBL based on relative position measurements will be discussed in particular.

Dynamic Model of the Grid SINS and DVL
According to [17], the state of grid SINS to be estimated is chosen as δ δ and the differential equations of grid SINS state can be described as: ∇ is the accelerometer bias, φ is the attitude error, G δV is the velocity error, and e δ R is the position error in ECEF frame. The V_φ and R_R C are the sub-state transition matrixes, which can refer to [17].
Besides, the DVL state is random velocity error, i.e., V , which is assumed as the one-order Markov process:

Design of the Dynamic Models
The dynamic models of the integrated navigation system include the grid SINS, DVL and USBL models, which can be described by state equations as follows: where the filter state is X = x SINS , x DVL and x USBL are the system states of SINS, DVL and USBL respectively. w SINS , w DVL and w USBL are the system noises of SINS, DVL and USBL respectively. F SINS , F DVL and F USBL are the system matrixes of SINS, DVL and USBL respectively. G SINS , G DVL and G USBL are the noise transition matrixes of SINS, DVL and USBL respectively. The detail of the dynamic models will be described in the follow sub-sections, where the dynamic model of the USBL based on relative position measurements will be discussed in particular.

Dynamic Model of the Grid SINS and DVL
According to [17], the state of grid SINS to be estimated is chosen as x SINS = φ δV G δR e ε b ∇ b T and the differential equations of grid SINS state can be described as: where ε b is the gyroscope drift, ∇ b is the accelerometer bias, φ is the attitude error, δV G is the velocity error, and δR e is the position error in ECEF frame. The C V_φ , C R_φ , C V_V , C R_V , C V_R , and C R_R are the sub-state transition matrixes, which can refer to [17]. Besides, the DVL state is random velocity error, i.e., x DVL = δV m DVL , which is assumed as the one-order Markov process: δ where τ V is the correlation time of the Markov process and w V is the zero-mean Gaussian white noise. Therefore, the grid SINS and DVL dynamic models can be described as (7) and (8), respectively. .

Dynamic Model of the USBL Based on Relative Position Measurements
The USBL output direction errors δα U δβ U T , the range factor error δK U , and the USBL installation error ψ U are chosen as the USBL state, and it can be expressed as The range R between the transponder and USBL array can be obtained as: where c is the sound velocity. T is the signal's roundtrip time between the transponder and USBL array.
Considering of the range error δR, the range R can be rewritten as: where δK U is the range factor error, which is related to the scale factor error. After the calibration, the USBL installation error ψ U and the range factor error δK U are considered as the random constants. The direction measurement errors δα U δβ U T are considered as the one-order Markov processes: where τ is the correlation time of Markov process and w is the zero-mean Gaussian white noise. The USBL dynamic model is described as: where F USBL is the system matrix and F USBL = diag( −1/τ α −1/τ β 0 0 0 0 ); G USBL is the noise transition matrix and G USBL = I 2×2 0 4×2 T .

Design of the Observation Models
The observation models of the grid SINS/DVL/USBL integrated navigation system include the DVL model and the USBL model, which can be described as follows: where Z = z DVL z USBL The detail of observation models will be described in the following sub-sections where the observation model of the USBL based on relative position measurements will be discussed in particular.

Observation Model of the DVL
Both the grid SINS and DVL can output vehicle velocity, which is chosen as one of the system observations. After calibration, the DVL installation error and scale factor error are compensated and neglected in this paper. The DVL output in G frame can be obtained as follows [17]: where V G is the actual velocity and δV G DVL is the random velocity error of DVL in the G frame. Besides, the grid SINS output velocity can be described as: (15) where δV G SINS is the SINS velocity error in G frame. Therefore, the DVL observation model of the integrated system can be given as: where H DVL can be described as:

Observation Model of the USBL Based on Relative Position Measurements
The USBL position system can provide the direction and range between the USBL array and transponder expressed as α U β U R U . The direction and range between USBL array and transponder can also be calculated by the grid SINS output and expressed as α S β S R S . Then the USBL observation is expressed as: where the δα U δβ U δR U and δα S δβ S δR S are relative position errors of the USBL output and grid SINS output, respectively.
The δα U δβ U δR U can be described as: The position coordinate of transponder r relative to USBL array in u frame can be calculated and obtained from the grid SINS output, which is defined as P u urS = x u S y u S z u S T . P u urS can be obtained by: where P e r is the position of the transponder expressed in e frame as a known quantity. P e b S is the position of b frame expressed in e frame and provided by the grid SINS. C u b and δP b bu are the calibration matrix and the level arm between the b frame and u frame and obtained by off-line calibrations.
α S β S R S can be obtained by: The first order total differential of (20) is written as: The error of P u urS is denoted as Considering of δP u urS , equation (19) can be rewritten as: Then δP u urS can be expressed as: where C R2δθ can refer to [17], and equation (21) can be rewritten as: Finally, the USBL observation model of the integrated system can be given as: where

The Modified Kalman Filter for Sparse Measurement Signals
The Kalman filter (KF) is a linear recursive filter algorithm based on the linear minimum variance. The KF filter runs in real time so it has numerous applications in navigation.
It is supposed that there are N measurements with different update frequencies involved in the KF, and their update periods are T 1 , T 2 , . . . , T N . The KF update period T K is designed as the common multiple in the conventional algorithms, and it can be expressed as: where n i is an integer that defines the update period. The employed integrated filter model is an approximate linear model and the KF is a linear algorithm. If the interval between two adjacent filter updates is too long, the filter may lose its precision especially for the high dynamic range. However, the output of the USBL is one kind of sparse measurement signals which will decrease the filter update frequency. Therefore, the conventional KF is not the optimum filter algorithm for the grid SINS/DVL/USBL integrated navigation.
In this section, an MKF is employed to improve the update frequency of filter in this paper. The MKF update period T K is designed as the common divisor of measurements, and it can be expressed as: where n i is an integer that defines the update period.
If the USBL output has not been updated, the filter works in the time update mode. In this case, the filter steps are expressed as:X When the USBL output has been updated, the filter works in the time and measurement update mode. In this case, the filter steps are expressed as: The update frequency of the MKF will not be influenced by the uncertain and low update frequency of the USBL. The MKF can also be employed by other integrated navigation systems to effectively deal with the sparse measurement signal.

The Vector Fault Detection Method for Filters
To improve the fault tolerance of the polar grid SINS/DVL/USBL integrated navigation system, the fault detection vector is designed and the vector fault detection method is proposed in this paper. With the vector, the fault detection method can take the measurements as objects to detect and isolate the fault, and simultaneously improve the utilization of effective measurements, which can also improve the integration performance. In this section, a vector fault detection method based on the residual χ 2 method, i.e., the residual vector χ 2 fault detection method, is proposed as an example to detect and locate the mutant fault measurement.
Firstly, the measurements as the detection object are classified based on the independence of fault's occurrence, and can be rewritten as Z = z 1 z 2 · · · z n T , where z i (i = 1, 2, · · · , n) is the measurement group which may contain more than one measurement. In the same group, the fault probabilities of different measurements are dependent. Whereas, the fault probabilities of different groups are independent from each other. The fault detection model of z i (i = 1, 2, · · · , n) is then established as:ẑ where H i k (i = 1, 2, · · · , n) is part of the observation matrix corresponding with z i k , and the subscript k denotes the k th time-step, andẑ i k is the estimation of z i k . Taking the grid SINS/DVL/USBL integration as an example, the measurements can be classified as Z = z DVL z αβ z R T . The fault probabilities of z DVL , z αβ and z R are independent, but the fault probabilities of α and β are dependent. The fault detection models of z DVL , z αβ and z R are established as: Then, the residual of the measurement estimation r k can be obtained as: If no fault occurs, r i k (i = 1, 2, · · · , n) can be considered as white noise which obeys normal distribution, i.e., r i k ∼ N(0, A i k )(i = 1, 2, · · · , n). The residual r i k (i = 1, 2, · · · , n) has zero mean and the variance A i k (i = 1, 2, · · · , n) is: When the measurement fault occurs, the statistical characteristics of measurement noise will change, and the residual r i k is no longer white noise, which means the statistical characteristics of r i k will change and the mean of r i k is no longer zero. Then the statistical characteristics change of r i k can be used to detect the measurement fault.
The fault detection vector is designed as: where Γ is the fault detection vector, and γ i (i = 1, 2, · · · , n) is the fault detection value of z i . γ i obeys the χ 2 distribution and its degree of freedom is m, i.e., r i ∼ χ 2 (m). m is the dimension of measurement group z i . A detection flag vector F = f 1 f 2 · · · f n is defined, and the fault detection criteria is: where T D is the threshold determined according to the false alarm rate. According to the detection flag vector F, if F is a zero vector, all the measurements perform well. However, if f i = 1, the fault occurs in the measurement group z i and the filter model should be reconstructed to isolate the measurement z i .
Taking the grid SINS/DVL/USBL integration as an example, Figure 4 shows the flow diagram of the vector fault detection method for the integrated navigation system.
where D T is the threshold determined according to the false alarm rate.
According to the detection flag vector F , if F is a zero vector, all the measurements perform well. However, if Taking the grid SINS/DVL/USBL integration as an example, Figure 4 shows the flow diagram of the vector fault detection method for the integrated navigation system. By designing and employing the detection vector, the vector detection method can locate the fault measurement instead of the fault filter, and fault measurement will be isolated by the reconstruction of observation matrix but the effective measurements will be kept. Therefore, the vector detection method can also be employed by the centralized filter. Except for the residual 2 χ fault detection method, some other effective traditional detection methods can also employ a proper detection vector to improve the accuracy of location fault and meanwhile the performance of integrated navigation.

Experiment Results and Discussions
To validate the performance of the proposed grid SINS/DVL/USBL integrated algorithm, the simulations and experiments are conducted in this section. Besides, the sea state and vehicle motion status are considered. The sea state is set as moderate condition, and the motion status includes static, motion with constant velocity, motion with constant acceleration, diving and turning a corner. The main inertial measurement unit (IMU) and motion parameters of the simulation experiment are described as follows.
Firstly, the gyroscopes drifts are set as 0.05 degrees per hour and the random errors of gyroscopes are set up as zero-mean Gaussian white noises. The accelerometer biases are set as 6 × 10 −5 g and the random errors of accelerometers are set up as zero-mean Gaussian white noises. By designing and employing the detection vector, the vector detection method can locate the fault measurement instead of the fault filter, and fault measurement will be isolated by the reconstruction of observation matrix but the effective measurements will be kept. Therefore, the vector detection method can also be employed by the centralized filter. Except for the residual χ 2 fault detection method, some other effective traditional detection methods can also employ a proper detection vector to improve the accuracy of location fault and meanwhile the performance of integrated navigation.

Experiment Results and Discussions
To validate the performance of the proposed grid SINS/DVL/USBL integrated algorithm, the simulations and experiments are conducted in this section. Besides, the sea state and vehicle motion status are considered. The sea state is set as moderate condition, and the motion status includes static, motion with constant velocity, motion with constant acceleration, diving and turning a corner. The main inertial measurement unit (IMU) and motion parameters of the simulation experiment are described as follows.
Firstly, the gyroscopes drifts are set as 0.05 degrees per hour and the random errors of gyroscopes are set up as zero-mean Gaussian white noises. The accelerometer biases are set as 6 × 10 −5 g and the random errors of accelerometers are set up as zero-mean Gaussian white noises.
Secondly, the attitude of the vehicle is set as a sine function to simulate the influence of the moderate sea state. The amplitude and period (denoted as amplitude/period) of pitch angle, roll angle and yaw angle are set as 2 • /7 s, 3 • /9 s and 1 • /8 s, respectively.
Thirdly, the acceleration of the vehicle is set as 0.2 m/s 2 and the vehicle has the maximum speed 5 m/s. The latitude and longitude of the initial position P is set as (75 • N, 126 • E). To facilitate understanding, the position errors of navigation algorithms are expressed as the position errors along the longitude, latitude and altitude direction respectively, and the unit of position errors is meter.
Finally, the DVL output frequency is 1 Hz, and the USBL output frequency is 0.2 Hz. The DVL velocity error is less than 0.3 m/s. The USBL range measurement error is less than 20 m, and the angle error is less than 1.5 • .

Simulation Results and Discussions
In this section, the simulation experiments are conducted to assess the performance of the proposed grid SINS/DVL/USBL integrated navigation algorithm, including the integrated navigation algorithm and the vector fault detection method.

Comparisons of the Integrated Navigation Algorithms
There are three polar integrated navigation algorithms discussed in this section, i.e., the traditional grid SINS/DVL integrated algorithm, the traditional grid SINS/DVL/USBL integrated algorithm 1 and the proposed grid SINS/DVL/USBL integrated algorithm 2. The traditional grid SINS/DVL/USBL 1 employs the absolute position as measurements, and the proposed grid SINS/DVL/USBL 2 employs the relative position as measurements. All the three integrated navigation algorithms choose the MKF as the filter algorithm.
The navigation errors of the grid SINS and three integrated algorithms are depicted in Figure 5. Besides, Table 1 shows the position error statistics of the grid SINS/DVL/USBL integrated navigation algorithms with different USBL position measurements.
speed 5m / s . The latitude and longitude of the initial position P is set as (75 N, 126 E)°°. To facilitate understanding, the position errors of navigation algorithms are expressed as the position errors along the longitude, latitude and altitude direction respectively, and the unit of position errors is meter.
Finally, the DVL output frequency is 1 Hz, and the USBL output frequency is 0.2 Hz. The DVL velocity error is less than 0.3m/s. The USBL range measurement error is less than 20 meters, and the angle error is less than 1.5°.

Simulation Results and Discussions
In this section, the simulation experiments are conducted to assess the performance of the proposed grid SINS/DVL/USBL integrated navigation algorithm, including the integrated navigation algorithm and the vector fault detection method.

Comparisons of the Integrated Navigation Algorithms
There are three polar integrated navigation algorithms discussed in this section, i.e., the traditional grid SINS/DVL integrated algorithm, the traditional grid SINS/DVL/USBL integrated algorithm 1 and the proposed grid SINS/DVL/USBL integrated algorithm 2. The traditional grid SINS/DVL/USBL 1 employs the absolute position as measurements, and the proposed grid SINS/DVL/USBL 2 employs the relative position as measurements. All the three integrated navigation algorithms choose the MKF as the filter algorithm.
The navigation errors of the grid SINS and three integrated algorithms are depicted in Figure 5. Besides, Table 1 shows the position error statistics of the grid SINS/DVL/USBL integrated navigation

0.0528
As shown in Figure 5, the grid frame can effectively restrain the navigation error amplification, and all the three integrated navigation algorithms can restrain the grid SINS errors effectively. As shown in Figure 5a,b, three integrated algorithms have almost the same attitude and velocity accuracy. However, the position errors of the grid SINS/DVL integrated navigation algorithm, shown as the blue curves in Figure 5c, still accumulate with time, which cannot maintain the necessary of high-precision polar navigation, and the grid SINS/DVL/USBL integrated navigation algorithms have higher position accuracy than the grid SINS/DVL algorithm. As shown in Figure 5c and Table 1, the position errors of both two grid SINS/DVL/USBL algorithms do not accumulate with time, and the proposed grid SINS/DVL/USBL algorithm (algorithm2) has higher accuracy than the traditional SINS/DVL/USBL algorithm (algorithm1). Compared to the traditional algorithm1, the latitude, longitude and height accuracies of the proposed algorithm2 are improved 88.44%, 57.34%, 41.41% (maximum error), and 79.43%, 55.83%, 87.56% (RMS), respectively. Therefore, compared to the traditional integrated algorithms, the proposed grid SINS/DVL/USBL integrated algorithm with relative position measurements can ensure the safety operation of ocean space vehicles in polar regions with high navigation accuracy.

Simulation Experiments of the Vector Fault Detection Method
In this section, the performance of the vector fault detection method and the fault-tolerant grid SINS/DVL/USBL integration are discussed. The centralized MKF is the filter algorithm and the residual vector 2 χ fault detection method is employed for the centralized filter. Some possible mutant fault measurements are involved in this experiment. The starting and termination times of the mutant measurement fault are shown in Table 2 and the measurement errors and the fault detection flags are shown in Figure 6.  As shown in Figure 5, the grid frame can effectively restrain the navigation error amplification, and all the three integrated navigation algorithms can restrain the grid SINS errors effectively. As shown in Figure 5a,b, three integrated algorithms have almost the same attitude and velocity accuracy. However, the position errors of the grid SINS/DVL integrated navigation algorithm, shown as the blue curves in Figure 5c, still accumulate with time, which cannot maintain the necessary of high-precision polar navigation, and the grid SINS/DVL/USBL integrated navigation algorithms have higher position accuracy than the grid SINS/DVL algorithm. As shown in Figure 5c and Table 1, the position errors of both two grid SINS/DVL/USBL algorithms do not accumulate with time, and the proposed grid SINS/DVL/USBL algorithm (algorithm2) has higher accuracy than the traditional SINS/DVL/USBL algorithm (algorithm1). Compared to the traditional algorithm1, the latitude, longitude and height accuracies of the proposed algorithm2 are improved 88.44%, 57.34%, 41.41% (maximum error), and 79.43%, 55.83%, 87.56% (RMS), respectively. Therefore, compared to the traditional integrated algorithms, the proposed grid SINS/DVL/USBL integrated algorithm with relative position measurements can ensure the safety operation of ocean space vehicles in polar regions with high navigation accuracy.

Simulation Experiments of the Vector Fault Detection Method
In this section, the performance of the vector fault detection method and the fault-tolerant grid SINS/DVL/USBL integration are discussed. The centralized MKF is the filter algorithm and the residual vector χ 2 fault detection method is employed for the centralized filter. Some possible mutant fault measurements are involved in this experiment. The starting and termination times of the mutant measurement fault are shown in Table 2 and the measurement errors and the fault detection flags are shown in Figure 6. As description in the Section 4.2, the measurements are classified as three measurement groups according to the independence of fault's occurrence, i.e., the range R from the USBL, the direction angles α β    As the flag lines showing in Figure 6, the DVL fault during 1800 s to 2250 s and the USBL fault during 3600 s to 4050 s both can be detected and located by the detection vector. During the time 5400 s to 5850 s β and α contain mutant fault but R works normally. So the angle flag becomes 1 and the R flag is still 0, and only the angle measurements, i.e., α and β , are isolated to reconstruct the filter. Then, during the time 7200 s to 7650 s, R contains mutant fault but α and β work normally. So the R flag becomes 1 and the angle flag is still 0, and only the R measurement is isolated to reconstruct the filter. Owing to the detection vector, the fault measurements of the centralized filter can be detected and located effectively. Especially, when the measurement device contains two kinds of navigation information, the fault occurrence of which is mutually independent, such as the angle [ ] α β and R from the USBL in this experiment, the fault measurements can also be located. The vector detection method successfully takes the measurement as object to locate the fault, so the fault location has more accuracy than the traditional scalar method and the utilization of effective measurements has been improved. The navigation errors in this experiment are shown in Figure 7. The blue curves indicate the As the flag lines showing in Figure 6, the DVL fault during 1800 s to 2250 s and the USBL fault during 3600 s to 4050 s both can be detected and located by the detection vector. During the time 5400 s to 5850 s β and α contain mutant fault but R works normally. So the angle flag becomes 1 and the R flag is still 0, and only the angle measurements, i.e., α and β, are isolated to reconstruct the filter. Then, during the time 7200 s to 7650 s, R contains mutant fault but α and β work normally. So the R flag becomes 1 and the angle flag is still 0, and only the R measurement is isolated to reconstruct the filter. Owing to the detection vector, the fault measurements of the centralized filter can be detected and located effectively. Especially, when the measurement device contains two kinds of navigation information, the fault occurrence of which is mutually independent, such as the angle α β and R from the USBL in this experiment, the fault measurements can also be located. The vector detection method successfully takes the measurement as object to locate the fault, so the fault location has more accuracy than the traditional scalar method and the utilization of effective measurements has been improved.   As shown by the blue curves in Figure 7, when the measurements fault occurs, the integrated navigation algorithm without the fault detection loses its accuracy. However, with the fault detection, the fault can be detected and located by the fault detection vector, and then the filter is reconstructed to isolate the fault measurement immediately. In this case, as shown by the red curves in Figure 7, the integrated navigation can maintain high accuracy because the reconstructed filter keeps updating uninterruptedly and accurately. On the other hand, the uninterruped filter ensures the uninterrupted fault detection. Therefore, when the measurements return to normal, the recovery is detected, and the isolated measurements are introduced into the integrated navigation filter again by another reconstruction without an obvious convergence time.
As a whole, the proposed vector fault detection method chooses the measurement as objects to locate the fault by employing the detection vector, so it also can detect and locate the fault measurement effectively when employed by the centralized filter. With the help of the vector fault detection method, the grid SINS/DVL/USBL integrated navigation algorithm can improve the fault tolerance and maintain the accuracy when some measurement mutant fault occurs.

Semi-Physical Experiment Results and Discussions
In this section, the semi-physical experiments are conducted to further validate the performance of the proposed algorithm due to the geographic restriction of the polar experiment. The semi-physical experiments are designed based on two principal factors which mainly affect the performance of the standard polar and ocean navigation strategies, i.e., the amplification of navigation errors caused by the meridian convergence and the navigation precision mainly influenced by the IMU measurement errors. Therefore, the semi-physical experiment, which can simulate the polar meridian convergence and simultaneously contain the actual IMU output errors, will be more realistic to verify the performance of the proposed navigation algorithm.
Moreover, compared to the simulation experiments, more standard navigation strategy and As shown by the blue curves in Figure 7, when the measurements fault occurs, the integrated navigation algorithm without the fault detection loses its accuracy. However, with the fault detection, the fault can be detected and located by the fault detection vector, and then the filter is reconstructed to isolate the fault measurement immediately. In this case, as shown by the red curves in Figure 7, the integrated navigation can maintain high accuracy because the reconstructed filter keeps updating uninterruptedly and accurately. On the other hand, the uninterruped filter ensures the uninterrupted fault detection. Therefore, when the measurements return to normal, the recovery is detected, and the isolated measurements are introduced into the integrated navigation filter again by another reconstruction without an obvious convergence time.
As a whole, the proposed vector fault detection method chooses the measurement as objects to locate the fault by employing the detection vector, so it also can detect and locate the fault measurement effectively when employed by the centralized filter. With the help of the vector fault detection method, the grid SINS/DVL/USBL integrated navigation algorithm can improve the fault tolerance and maintain the accuracy when some measurement mutant fault occurs.

Semi-Physical Experiment Results and Discussions
In this section, the semi-physical experiments are conducted to further validate the performance of the proposed algorithm due to the geographic restriction of the polar experiment. The semi-physical experiments are designed based on two principal factors which mainly affect the performance of the standard polar and ocean navigation strategies, i.e., the amplification of navigation errors caused by the meridian convergence and the navigation precision mainly influenced by the IMU measurement errors. Therefore, the semi-physical experiment, which can simulate the polar meridian convergence and simultaneously contain the actual IMU output errors, will be more realistic to verify the performance of the proposed navigation algorithm.
Moreover, compared to the simulation experiments, more standard navigation strategy and experiment condition are involved in the semi-physical experiment [27]. The GNSS signals are employed as measurements to give more comparative common navigation strategies and the vehicle trajectory is also designed to include both the surface environment where the GNSS can provide position coordinates and also the underwater environment where the GNSS signals are lost. Then the simultaneous faults of the DVL and USBL are involved to verify the performance of fault detection method.
The semi-physical experiment contains three parts, i.e., the extraction of sensor errors and generation of the semi-physical experiment data, the performance validation of the integrated algorithms, and the performance validation of the fault detection method, which will be discussed in the following subsections.

Extraction of Sensor Errors and Generation of the Semi-Physical Experiment Data
In this subsection, a set of polar semi-physical experiment data that contain the actual IMU output errors and ideal IMU outputs is generated and will be employed to verify the performance of the algorithm. In this experiment, the ideal IMU outputs in polar regions can be generated by the trajectory generator, and the key point to get the semi-physical experiment data is to get the actual IMU output errors.
The signal flow to get the actual IMU output errors, i.e., δω b ib and δf b ib , and the semi-physical experiment data, i.e., ω b ibs and f b ibs , is shown as Figure 8. experiment condition are involved in the semi-physical experiment [27]. The GNSS signals are employed as measurements to give more comparative common navigation strategies and the vehicle trajectory is also designed to include both the surface environment where the GNSS can provide position coordinates and also the underwater environment where the GNSS signals are lost. Then the simultaneous faults of the DVL and USBL are involved to verify the performance of fault detection method.
The semi-physical experiment contains three parts, i.e., the extraction of sensor errors and generation of the semi-physical experiment data, the performance validation of the integrated algorithms, and the performance validation of the fault detection method, which will be discussed in the following subsections.

Extraction of Sensor Errors and Generation of the Semi-Physical Experiment Data
In this subsection, a set of polar semi-physical experiment data that contain the actual IMU output errors and ideal IMU outputs is generated and will be employed to verify the performance of the algorithm. In this experiment, the ideal IMU outputs in polar regions can be generated by the trajectory generator, and the key point to get the semi-physical experiment data is to get the actual IMU output errors.
The signal flow to get the actual IMU output errors, i.e.,  In the turntable experiment, an IMU is installed on a high-precision three-axis turntable as shown in Figure 9. The main parameters of the three-axis turntable and IMU are shown in Tables 3  and 4.
During the turntable experiment, the IMU output, i.e., Then the actual IMU output errors are obtained as:  In the turntable experiment, an IMU is installed on a high-precision three-axis turntable as shown in Figure 9. The main parameters of the three-axis turntable and IMU are shown in Tables 3 and 4. During the turntable experiment, the IMU output, i.e., ω b ib and f b ib , and the turntable movement data are collected. Then, a trajectory generator is employed, which can generate the ideal reference of IMU output, i.e., ω b ib and f b ib , when the vehicle's movement is input or designed. As shown in Figure 8, the movement data of the turntable, i.e., the attitude, velocity, and position, are inputted into the trajectory generator, and the reference IMU output data, i.e., ω b ib and f b ib , are generated. Then the actual IMU output errors are obtained as: The δω b ib and δf b ib are the actual IMU output errors gained from the above turntable experiment. Meanwhile, the trajectory generator is designed to simulate the polar region geographic environment which leads to the meridian convergence. The ocean space vehicle trajectory in polar regions and the corresponding ideal IMU outputs, i.e.,    The standard navigation strategies for ocean vehicles include the SINS/DVL integration, SINS/DVL/GNSS integration and SINS/DVL/USBL integration, in which the DVL provides the velocity and the GNSS and USBL provide the absolute position. The grid SINS/DVL/USBL integrated navigation algorithm proposed in this paper employs the DVL to provide velocity and the USBL to provide the relative position as measurements.
There are four integrated navigation algorithms discussed in this section including the standard strategies and the proposed novel strategy:

Semi-Physical Experiments of the Integrated Navigation Algorithms
The standard navigation strategies for ocean vehicles include the SINS/DVL integration, SINS/DVL/GNSS integration and SINS/DVL/USBL integration, in which the DVL provides the velocity and the GNSS and USBL provide the absolute position. The grid SINS/DVL/USBL integrated navigation algorithm proposed in this paper employs the DVL to provide velocity and the USBL to provide the relative position as measurements.
There are four integrated navigation algorithms discussed in this section including the standard strategies and the proposed novel strategy: The corresponding semi-physical sensor output data obtained in Section 5.2.1 is employed in this experiment. Besides, all the integrated algorithms choose the MKF as the filter algorithm.
The navigation errors of different algorithms in the semi-physical experiment are depicted in Figure 11. Besides, the position errors of grid SINS/DVL/USBL algorithms with different USBL position measurements are shown in Table 5.  The corresponding semi-physical sensor output data obtained in Section 5.2.1 is employed in this experiment. Besides, all the integrated algorithms choose the MKF as the filter algorithm.
The navigation errors of different algorithms in the semi-physical experiment are depicted in Figure 11. Besides, the position errors of grid SINS/DVL/USBL algorithms with different USBL position measurements are shown in Table 5. The corresponding semi-physical sensor output data obtained in Section 5.2.1 is employed in this experiment. Besides, all the integrated algorithms choose the MKF as the filter algorithm.
The navigation errors of different algorithms in the semi-physical experiment are depicted in Figure 11. Besides, the position errors of grid SINS/DVL/USBL algorithms with different USBL position measurements are shown in Table 5.     Figure 11, the proposed navigation algorithm can conquer the error amplification caused by the meridian convergence because of the grid navigation frame. As shown by the black curves, the grid SINS output still contains the periodic oscillation error and accumulated error, and the integration technique can restrain the grid SINS errors effectively with the help of velocity or position measurements from the DVL, GNSS and USBL. Besides, firstly the grid SINS/DVL, grid SINS/DVL/GNSS and the grid SINS/DVL/USBL integrated algorithm have nearly the same accuracy of attitude and velocity, but the position errors of the grid SINS/DVL integrated algorithm still accumulate with time. Secondly, when the vehicle operates on the surface, the position errors of the grid SINS/DVL/GNSS and the grid SINS/DVL/USBL do not accumulate, but when the vehicle navigates under water, the GNSS signals are lost and position performance between two navigation strategies becomes larger. Figure 12 shows the position comparison between four integrated navigation strategies. According to Figures 11 and 12 and Table 5, the position accuracy of the proposed grid SINS/DVL/USBL 2 is higher than that of the traditional grid SINS/DVL/USBL 1 and compared to algorithm1, the latitude, longitude and height accuracies of algorithm2 are improved 87.38%, 63.84%, 91.73% (maximum error), and 87.01%, 65.26%, 89.36% (RMS), respectively. The maximum errors of latitude, longitude and height are reduced from 2.9663 m to 0.3742 m, 2.8152 m to 1.0179 m, and 2.5741 m to 0.2127 m, respectively, which can maintain the position accuracy of ocean space vehicles in polar regions. Figure 11. Description of the navigation errors in the semi-physical experiment. As shown in Figure 11, the proposed navigation algorithm can conquer the error amplification caused by the meridian convergence because of the grid navigation frame. As shown by the black curves, the grid SINS output still contains the periodic oscillation error and accumulated error, and the integration technique can restrain the grid SINS errors effectively with the help of velocity or position measurements from the DVL, GNSS and USBL. Besides, firstly the grid SINS/DVL, grid SINS/DVL/GNSS and the grid SINS/DVL/USBL integrated algorithm have nearly the same accuracy of attitude and velocity, but the position errors of the grid SINS/DVL integrated algorithm still accumulate with time. Secondly, when the vehicle operates on the surface, the position errors of the grid SINS/DVL/GNSS and the grid SINS/DVL/USBL do not accumulate, but when the vehicle navigates under water, the GNSS signals are lost and position performance between two navigation strategies becomes larger. Figure 12 shows the position comparison between four integrated navigation strategies. According to Figures 11 and 12 and Table 5, the position accuracy of the proposed grid SINS/DVL/USBL 2 is higher than that of the traditional grid SINS/DVL/USBL 1 and compared to algorithm1, the latitude,

Semi-Physical Experiments of the Vector Fault Detection Method
The proposed vector fault detection method and the fault-tolerant grid SINS/DVL/USBL integrated algorithm are discussed in this section. To validate the performance of the vector fault detection method, some mutant constant errors are added into the measurements. Compared to the simulation experiments, the ocean space vehicle trajectory and the corresponding semi-physical sensor output data obtained in Section 5.2.1 are employed in this experiment. Meanwhile, the possible condition, when part of USBL measurements and DVL measurements malfunction simultaneously, is involve in this semi-physical experiment. The starting and termination times of the measurement fault are shown in Table 6.  Figure 13, and the navigation errors of the fault-tolerant grid SINS/DVL/USBL integrated algorithm are shown in Figure 14.

Semi-Physical Experiments of the Vector Fault Detection Method
The proposed vector fault detection method and the fault-tolerant grid SINS/DVL/USBL integrated algorithm are discussed in this section. To validate the performance of the vector fault detection method, some mutant constant errors are added into the measurements. Compared to the simulation experiments, the ocean space vehicle trajectory and the corresponding semi-physical sensor output data obtained in Section 5.2.1 are employed in this experiment. Meanwhile, the possible condition, when part of USBL measurements and DVL measurements malfunction simultaneously, is involve in this semi-physical experiment. The starting and termination times of the measurement fault are shown in Table 6. The measurement errors and the fault detection flags are shown in Figure 13, and the navigation errors of the fault-tolerant grid SINS/DVL/USBL integrated algorithm are shown in Figure 14. As shown in Figure 13, different kinds of measurement fault, including the DVL fault, the USBL fault, the simultaneous DVL and USBL fault, can be detected and located by the proposed vector fault detection method.  As shown in Figure 13, different kinds of measurement fault, including the DVL fault, the USBL fault, the simultaneous DVL and USBL fault, can be detected and located by the proposed vector fault detection method. As shown in Figure 13, different kinds of measurement fault, including the DVL fault, the USBL fault, the simultaneous DVL and USBL fault, can be detected and located by the proposed vector fault detection method.   Owing to the detection vector, the fault measurements can be isolated by the reconstruction of filter, and as the red lines showing in Figure 14, the integrated performance has not been influenced by the measurement fault. But as the blue curves show in Figure 14, when the measurement fault occurs, the grid SINS/DVL/USBL integrated navigation algorithm without the fault detection method loses its accuracy.
Though the R measurements and angle measurements, i.e., α and β, which are both provided by the USBL, the fault of the two kinds of measurements are independent from each other. During the time 8261 to 8450 s, the integrated filter is challenged by both the velocity measurement fault from DVL and the R measurement fault from the USBL. Then the DVL and R measurements are located by the detection flag vector and isolated by the reconstruction. And meanwhile the filter is still operating with the α and β measurements to ensure the grid SINS errors were restrained uninterrupted during the short time. However, if the traditional scalar fault detection method is employed to isolate the simultaneous R and DVL fault, or the absolute position is employed as measurements, all the measurements would be isolated and the filter would be interrupted. As a result, the grid SINS errors would not be restrained by integration and the navigation accuracy would decrease sharply. Compared with the traditional scalar method, the vector method can improve the utilization of effective measurements and navigation performance.
Hence, the proposed vector fault detection method can be employed by the centralized filter to detect and locate the fault measurements and improve the grid SINS/DVL/USBL integrated algorithm's reliability and fault-tolerant to maintain the requirement of ocean space vehicles.

Conclusions
A fault-tolerant grid SINS/DVL/USBL integrated navigation algorithm based on the centralized filter and relative position information is proposed to improve the performance of ocean space navigation systems in polar regions. Firstly, the USBL and DVL output are employed as the measurements and the filter observation model of the USBL is proposed based on the relative position measurements to improve the USBL availability. Then, the grid SINS/DVL/USBL filter model is designed to improve the navigation accuracy. Moreover, considering of the sparse measurement from the USBL, the MKF is employed to improve the filter update frequency and estimate accuracy. Finally, the vector fault detection method, which takes the measurements as detection object instead of the traditional filter, is proposed to detect and isolate the measurement fault of centralized filter. Simulations and semi-physical experiment results show that the vector fault detection method can detect and isolate the measurement fault of centralized filter effectively, and together with the novel fault detection method, the grid SINS/DVL/USBL integrated algorithm can provide high-precision and high-reliable navigation information for ocean space vehicles to operate normally and safely in polar regions. Some important further developments are planned for the future. Firstly, if the conditions allow, a polar experiment is expected, and more kinds of external measurements will be employed to improve the integrated navigation accuracy and reliability.