A Fault-Tolerant Level Damping Algorithm for Marine INS

When themeasurement error of the external reference velocity changes dramatically, the traditional level damping for marine INS needs to cut off the damping to maintain the navigation accuracy. *e level channel has a large overshoot oscillation during the variable damping instantaneous, which results in obvious position deviation. In order to solve this practical problem, a damping model is established outside the INS. *e most obvious advantage of the algorithm is that the damping algorithm does not affect the inertial navigation solution. *e fault-tolerant algorithm realizes the automatic damping switch according to the external reference velocity error variation criterion, which avoids the velocity oscillation and position deviation. Compared with traditional methods, the algorithm presented in this paper has higher reliability and better environmental adaptability. *e effectiveness of the algorithm is verified by the actual navigation test data.


Introduction
e level channel of INS is undamped, and various error sources and initial condition errors will lead to continuous Schuler periodic oscillation. e Schuler periodic oscillation can be suppressed by the damping algorithm based on the external velocity reference. For marine inertial navigation, the velocity reference usually comes from the DVL. Traditional level damping is realized based on closed-loop feedback, and external reference velocity error is introduced into inertial navigation solution. In order to solve the problem introduced by external reference velocity error, variable damping technique is usually adopted. Due to the complexity of ocean navigation, variable damping technology can only play an appropriate role under certain conditions [1,2]. If the external reference values are not ideal, the predicted values and measured values can be fused by inverse covariance intersection [3]. is method can ensure the consistency of fusion results and is used in many applications [4][5][6].
e marine INS such as AN/ WSN-7B and MK49 in the United States adopt the damping scheme of Kalman filter and can realize the manual switching between traditional damping and Kalman filter [7,8]. is damping scheme is based on optimal estimation and feedback correction. e error state of the system is estimated by Kalman filter, and the error is fed back to the INS to correct the system error, thus achieving the effect of damping error oscillation. e shortcomings of the traditional method are that in the process of damping switching based on feedback correction, especially in the process of switching from undamped to damped, the velocity overshoot oscillation and its position derivation are easy to occur.
In order to improve this situation, two main methods are used [9][10][11][12]. e first method is based on qualitative analysis of the system closed-loop damping ratio which is related to the overshoot. e second method is based on the command rate before and after the switching. e method needs accurate position provided by GNSS, and this can limit its application fields.
A switched system is a typical hybrid system which consists of several subsystems and a switching law coordinating the operation of each subsystem. Many researchers have investigated switched systems such as for robot control [13], electrical power systems [14], and spacecraft control [15]. INS also have a problem of switching between two operating modes, but published work into similar research from the perspective of switched systems is difficult to find. e fault-tolerant level damping algorithm proposed in this paper is realized outside the INS, and the damping is switched automatically according to the change of external reference velocity error. e most obvious advantage of the algorithm is that the damping algorithm does not affect the inertial navigation solution, the damping switching is convenient, and the large-scale velocity overshoot oscillation and position derivation can be avoided. Compared with traditional methods, this method has high reliability and great practical value. e navigation test verifies the effectiveness of the algorithm. e paper is organized as follows: the basic principle of the damping algorithm is presented in Section 2. Section 3 analyses the calculation model of damping and the external reference velocity error variation criterion. e experimental verification is presented in Section 4. Section 5 gives the conclusions. velocity. e attitude transfer matrix is calculated by INS position, attitude transfer matrix, and damping position. e damping effect does not affect the inertial navigation solution. e damping algorithm has two outputs at the same time, which are INS output and damping output. e algorithm realizes the automatic damping switch according to the external reference velocity error variation criterion, avoiding the velocity oscillation and position deviation caused by the traditional damping switch. When the DVL velocity error varied drastically, the INS solution is adopted instead of damping output. When the DVL velocity error varied gently, damping output is used. Compared with traditional methods, the method presented in this paper has higher reliability and better environmental adaptability.

Derivation of Formula
3.1. Level Damping. Velocity differential equation of undamped INS is as follows [16]: where v n c e is the velocity estimate, C n c b is the attitude transfer matrix estimate, f b is the specific force estimate, ω n c ie and ω n c en are the relative and transfer angular rate estimate, and g n c is the gravity acceleration estimate. Level velocity damping differential equation is as follows: where the subscript c denotes undamped INS, d denotes damping, v n d r is the DVL velocity, and Cis the frequency function of C(s).
According to equation (1), Substituting equation (3) into equation (2) 1 0 0 By Laplace transform and simplifying, we get 1 0 0 where Level damping velocity is as follows: Assuming H(s) Calculation formula of level damping velocity is as follows: e design of Schuler periodic damping correction network generally requires the following conditions: (1) Calibration network transfer function H(s) should provide the necessary leading phase in the intermediate frequency segment to ensure that the root of the closed-loop characteristic equation of the Schuler damping loop is all in the left half plane of the s plane so that the system is asymptotically stable: (2) lim s⟶0 H(s) � 1 makes the external reference velocity have no effect on the damping velocity, while the influence of the low frequency components of the acceleration error and the inertial navigation velocity remains unchanged from the undamped state.
(3) lim s⟶∞ H(s) � 1 makes the Schuler damping loop have the high frequency attenuation characteristic above the second order and can effectively filter the high frequency interference signal from the external reference speed and the inertial instrument output. Hence, the 2-order damping correction network H(s) � s 2 + aω s s + ω 2 s /s 2 + bω s s + ω 2 s is selected in this paper. Because it satisfies conditions lim s⟶0 H(s) � 1 and lim s⟶∞ H(s) � 1 and it has excellent low frequency and high frequency characteristics, the coefficients a and b are selected according to condition (1) to satisfy the system asymptotic stability condition.
Discrete time equation of damping velocity is put forward by Z-transform of equation (8).
As the damping velocity is calculated at t k ， acceleration error C n d n c (t k )(2ω n c ie (t k ) + ω n c en (t k )) × v n c e (t k ) needs to be calculated according to the damping velocity at t k . It uses iterative methods to calculate. First, the acceleration error is calculated with the damping velocity at t k−1 , and the damping velocity at t k is calculated according to the damping equation and then substituted into the acceleration error equation and recalculate C n d n c (t k )(2ω n c ie (t k ) + ω n c en (t k )) × v n c e (t k ). Finally, the damping velocity at t k is calculated according to the damping equation.
On the basis of inertial navigation position increment, the damped position increment is obtained by the integral of velocity difference between the damping and the INS. Taking the latitude, for example, the position increment equation could be expressed as where L is the latitude, R N is the meridian circle radius, and h is the altitude. By subtracting the two formulas, we get where velocity error δv n is mainly caused by Schuler periodic oscillation, which can be closely approximated by the trapezoidal formula： e damping position is expressed as e damping attitude transfer matrix is as follows: where C n d e is the calculated by damped position.

Fault Tolerant.
It is very complicated to establish an accurate external velocity error model. On the one hand, the DVL velocity is polluted by the distorted current around the carrier when the ship is maneuvering; on the other hand, in some sea areas (such as bays and trenches), the change of ocean current size and direction will also cause the change of external velocity error. e purpose of level damping is to suppress the Schuler periodic oscillation of INS. e key to fault-tolerant damping is to set the external velocity error variation criterion, which can automatically determine damping switch. e external velocity error criterion is set according to the velocity difference between the INS and DVL

e difference between different times is recorded as
Δv � dv n c t 1 − dv n c t 2 − dv r t 1 − dv r t 2 , (16) where the first term of the equation is the difference value of velocity error calculated by INS from time t 1 to time t 2 , and the second term is the difference value of velocity error measured by DVL from time t 1 to time t 2 . e velocity error of INS has little change in a short time. If the value of equation (16) is obvious, it can be considered as the result of external velocity error, and the damping switch is carried out automatically.

Experiment
e effectiveness of the fault-tolerant level damping algorithm is verified by the actual navigation test data. e experiment equipment includes high-precision laser gyro INS and DVL.
e experiment equipment parameters are as follows. e bias stability of the gyro is better than 0.003/h, and the angular random walk is better than 0.001/ . e bias stability of the accelerometer is better than 20 μg, and the noise power spectral density is better than 20 μg/ ��� HZ

√
. e velocity measurement accuracy of DVL is 0.5%. ere are two experiments, one with external position reference and the other without.   Figures 3 and 4, the damping algorithm proposed in the paper can suppress Schuler periodic oscillation of velocity. Figure 5 shows the damping position. Compared to INS position, damping position has a smaller range. Taking satellite position as the position reference, position error of INS and damping is obtained; the curves are shown in Figure 6. e position error curves show that damping suppresses Schuler periodic oscillation of position.
As a conclusion, 26 hours marine navigation tests show that the proposed level damping algorithm can suppress Schuler periodic oscillation of velocity and position and achieve the effect as traditional damping algorithm.

Fault-Tolerant Damping Verification.
e experiment is 60 hours marine navigation without external position reference. e INS velocity and DVL velocity are shown in Figure 7. e blue curve is INS velocity, and the red curve is DVL velocity. From the curve comparison, we can see that the Schuler periodic oscillation is obvious.
As shown in Figure 8, the fault-tolerant damping algorithm proposed in the paper suppresses Shuler periodic oscillation of position and achieves the traditional damping algorithm effect.
e dynamic noise is composed of two parts: one part is fixed deviation, which amplitude is 20. e fixed deviation lasts for 1 hour each time, and the pulling direction is different. e other part is the sine curve with an amplitude of 5 and a period of 6 minutes. As shown in Figure 9, the velocity difference between INS and noise-added DVL is obvious between 19 and 21 hours.
As shown in Figure 10, the traditional damping position is obviously deviated when the external velocity has a large error. Compared with the damping position without adding noise (Figure 8), the damping latitude and longitude have deviations of 10 n mile and 15 n mile, respectively.      e velocity comparison between INS and fault-tolerant damping is shown in Figure 11. When the DVL velocity is abnormal, the fault-tolerant damping is automatically switched to undamped. After the DVL velocity returns to normal, the fault-tolerant damping is automatically switched to damping. During overshoot oscillation of the damping velocity, the damping position increment is calculated by INS position increment. After the damping velocity is stabilized, the damping position increment is calculated by equation (14).
As shown in Figure 12, the fault-tolerant damping algorithm proposed in the paper suppresses Shuler periodic oscillation of position, and there is no obvious position deviation with adding noise. Compared with the traditional damping algorithm, the damping position is less affected by the external velocity error.

Conclusion
e navigation test shows that the fault-tolerant level damping algorithm proposed in this paper can suppress Schuler periodic oscillation of INS and achieve the traditional level damping effect. e fault-tolerant damping can automatically switch the damping when the DVL velocity error changes drastically, and there is no obvious position offset. Compared with the traditional damping algorithm, the fault-tolerant damping algorithm has better reliability and strong environmental adaptability and is suitable for long-term marine.
It is difficult to obtain the optimal convergence rate and overshoot simultaneously. Under the condition of quick convergence of oscillation error, how to compensate the overshoot is the next step of the paper.
Data Availability e long-term navigation data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that they have no conflicts of interest.