# A Fault-Tolerant Level Damping Algorithm for Marine INS

Authors: Guohu Feng
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

When the measurement 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. The 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. The most obvious advantage of the algorithm is that the damping algorithm does not affect the inertial navigation solution. The 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. The effectiveness of the algorithm is verified by the actual navigation test data.


---

## Body

## 1. Introduction

The level channel of INS is undamped, and various error sources and initial condition errors will lead to continuous Schuler periodic oscillation. The 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]. This method can ensure the consistency of fusion results and is used in many applications [4–6]. The 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]. This damping scheme is based on optimal estimation and feedback correction. The 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. The 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–12]. The first method is based on qualitative analysis of the system closed-loop damping ratio which is related to the overshoot. The second method is based on the command rate before and after the switching. The 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.The 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. The 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. The navigation test verifies the effectiveness of the algorithm.The paper is organized as follows: the basic principle of the damping algorithm is presented in Section2. Section 3 analyses the calculation model of damping and the external reference velocity error variation criterion. The experimental verification is presented in Section 4. Section 5 gives the conclusions.


## 2. Basic Principle

The schematic diagram of level damping is shown in Figure1. A damping model is established outside the INS. As shown in Figure 1, damping velocity is calculated by INS velocity, DVL velocity, and attitude transfer matrix Cncnd, in which c denotes undamped INS and d denotes damping. The damping position increment is calculated by INS position and velocity difference between INS velocity and damping velocity. The attitude transfer matrix is calculated by INS position, attitude transfer matrix, and damping position. The damping effect does not affect the inertial navigation solution. The damping algorithm has two outputs at the same time, which are INS output and damping output. The 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.Figure 1
Block diagram of level damping.


## 3. Derivation of Formula

### 3.1. Level Damping

Velocity differential equation of undamped INS is as follows [16]:(1)v˜˙enc=C˜bncf˜b−2ω˜ienc+ω˜ennc×v˜enc+g˜nc,where v˜enc is the velocity estimate, C˜bnc is the attitude transfer matrix estimate, f˜b is the specific force estimate, ω˜ienc and ω˜ennc are the relative and transfer angular rate estimate, and g˜nc is the gravity acceleration estimate.Level velocity damping differential equation is as follows:(2)100010v˜˙ednd=100010C˜bndf˜b−1000102ω˜iend+ω˜ennd×v˜ednd+100010g˜nd−100010Cv˜ednd−v˜rnd,where the subscript c denotes undamped INS, d denotes damping, v˜rnd is the DVL velocity, and Cis the frequency function of Cs.According to equation (1),(3)C˜bncf˜b=v˜˙enc+2ω˜ienc+ω˜ennc×v˜enc−g˜nc.Substituting equation (3) into equation (2)(4)100010v˜˙ednd=100010C˜ncndv˜˙enc+2ω˜ienc+ω˜ennc×v˜enc−g˜nc−1000102ω˜iend+ω˜ennd×v˜ednd+100010g˜nd−100010Cv˜ednd−v˜rnd.By Laplace transform and simplifying, we get(5)100010v˜ednd−100010C˜ncndv˜enc=−100010Csv˜ednd−v˜rnd+100010Δs,where(6)Δ=C˜ncnd2ω˜ienc+ω˜ennc×v˜enc−g˜nc−2ω˜iend+ω˜ennd×v˜end−g˜nd.Level damping velocity is as follows:(7)100010v˜ednd=10001011+C/sC˜ncndv˜enc+Δs+100010C/s1+C/sv˜rnd.AssumingHs=1/1+C/s, then C/s/1+C/s=1−Hs.Calculation formula of level damping velocity is as follows:(8)100010v˜ednd=100010HsC˜ncndv˜enc+Δs+1000101−Hsv˜rnd.The design of Schuler periodic damping correction network generally requires the following conditions:(1)
Calibration network transfer functionHs 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:(9)s2+Hsωs2=0.(2)
lims⟶0Hs=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)
lims⟶∞Hs=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 networkHs=s2+aωss+ωs2/s2+bωss+ωs2 is selected in this paper.Because it satisfies conditionslims⟶0Hs=1 and lims⟶∞Hs=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 byZ-transform of equation (8).(10)100010v˜ednd=100010HzC˜ncndv˜enc+ZΔs+1000101−Hzv˜rnd.As the damping velocity is calculated attk， acceleration error C˜ncndtk2ω˜ienctk+ω˜ennctk×v˜enctk needs to be calculated according to the damping velocity at tk. It uses iterative methods to calculate. First, the acceleration error is calculated with the damping velocity at tk−1, and the damping velocity at tk is calculated according to the damping equation and then substituted into the acceleration error equation and recalculate C˜ncndtk2ω˜ienctk+ω˜ennctk×v˜enctk. Finally, the damping velocity at tk 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(11)L˜nctk+1−L˜nctk=∫tktk+1v˜nctRN+hdt,L˜ndtk+1−L˜ndtk=∫tktk+1v˜ndtRN+hdt,where L is the latitude, RN is the meridian circle radius, and h is the altitude. By subtracting the two formulas, we get(12)L˜nctk+1−L˜nctk−L˜ndtk+1−L˜ndtk=∫tktk+1δvntRN+hdt,where velocity error δvn is mainly caused by Schuler periodic oscillation, which can be closely approximated by the trapezoidal formula：(13)∫tktk+1δvntRN+hdt≈δvntk+δvntk+12RN+htk+1−tk.The damping position is expressed as(14)L˜ndtk+1=L˜ndtk+L˜nctk+1−L˜nctk−δvntk+δvntk+12RN+htk+1−tk.The damping attitude transfer matrix is as follows:(15)C˜ncnd=C˜endC˜nce,where C˜end is the calculated by damped position.


### 3.2. 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. The purpose of level damping is to suppress the Schuler periodic oscillation of INS. The key to fault-tolerant damping is to set the external velocity error variation criterion, which can automatically determine damping switch.The external velocity error criterion is set according to the velocity difference between the INS and DVLvnc−vr, where vnc=v+dvnc and vr=v+dvr, so vnc−vr=dvnc−dvr. The difference between different times is recorded as(16)Δv=dvnct1−dvnct2−dvrt1−dvrt2,where the first term of the equation is the difference value of velocity error calculated by INS from time t1 to time t2, and the second term is the difference value of velocity error measured by DVL from time t1 to time t2. The 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.


## 3.1. Level Damping

Velocity differential equation of undamped INS is as follows [16]:(1)v˜˙enc=C˜bncf˜b−2ω˜ienc+ω˜ennc×v˜enc+g˜nc,where v˜enc is the velocity estimate, C˜bnc is the attitude transfer matrix estimate, f˜b is the specific force estimate, ω˜ienc and ω˜ennc are the relative and transfer angular rate estimate, and g˜nc is the gravity acceleration estimate.Level velocity damping differential equation is as follows:(2)100010v˜˙ednd=100010C˜bndf˜b−1000102ω˜iend+ω˜ennd×v˜ednd+100010g˜nd−100010Cv˜ednd−v˜rnd,where the subscript c denotes undamped INS, d denotes damping, v˜rnd is the DVL velocity, and Cis the frequency function of Cs.According to equation (1),(3)C˜bncf˜b=v˜˙enc+2ω˜ienc+ω˜ennc×v˜enc−g˜nc.Substituting equation (3) into equation (2)(4)100010v˜˙ednd=100010C˜ncndv˜˙enc+2ω˜ienc+ω˜ennc×v˜enc−g˜nc−1000102ω˜iend+ω˜ennd×v˜ednd+100010g˜nd−100010Cv˜ednd−v˜rnd.By Laplace transform and simplifying, we get(5)100010v˜ednd−100010C˜ncndv˜enc=−100010Csv˜ednd−v˜rnd+100010Δs,where(6)Δ=C˜ncnd2ω˜ienc+ω˜ennc×v˜enc−g˜nc−2ω˜iend+ω˜ennd×v˜end−g˜nd.Level damping velocity is as follows:(7)100010v˜ednd=10001011+C/sC˜ncndv˜enc+Δs+100010C/s1+C/sv˜rnd.AssumingHs=1/1+C/s, then C/s/1+C/s=1−Hs.Calculation formula of level damping velocity is as follows:(8)100010v˜ednd=100010HsC˜ncndv˜enc+Δs+1000101−Hsv˜rnd.The design of Schuler periodic damping correction network generally requires the following conditions:(1)
Calibration network transfer functionHs 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:(9)s2+Hsωs2=0.(2)
lims⟶0Hs=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)
lims⟶∞Hs=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 networkHs=s2+aωss+ωs2/s2+bωss+ωs2 is selected in this paper.Because it satisfies conditionslims⟶0Hs=1 and lims⟶∞Hs=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 byZ-transform of equation (8).(10)100010v˜ednd=100010HzC˜ncndv˜enc+ZΔs+1000101−Hzv˜rnd.As the damping velocity is calculated attk， acceleration error C˜ncndtk2ω˜ienctk+ω˜ennctk×v˜enctk needs to be calculated according to the damping velocity at tk. It uses iterative methods to calculate. First, the acceleration error is calculated with the damping velocity at tk−1, and the damping velocity at tk is calculated according to the damping equation and then substituted into the acceleration error equation and recalculate C˜ncndtk2ω˜ienctk+ω˜ennctk×v˜enctk. Finally, the damping velocity at tk 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(11)L˜nctk+1−L˜nctk=∫tktk+1v˜nctRN+hdt,L˜ndtk+1−L˜ndtk=∫tktk+1v˜ndtRN+hdt,where L is the latitude, RN is the meridian circle radius, and h is the altitude. By subtracting the two formulas, we get(12)L˜nctk+1−L˜nctk−L˜ndtk+1−L˜ndtk=∫tktk+1δvntRN+hdt,where velocity error δvn is mainly caused by Schuler periodic oscillation, which can be closely approximated by the trapezoidal formula：(13)∫tktk+1δvntRN+hdt≈δvntk+δvntk+12RN+htk+1−tk.The damping position is expressed as(14)L˜ndtk+1=L˜ndtk+L˜nctk+1−L˜nctk−δvntk+δvntk+12RN+htk+1−tk.The damping attitude transfer matrix is as follows:(15)C˜ncnd=C˜endC˜nce,where C˜end is the calculated by damped position.


## 3.2. 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. The purpose of level damping is to suppress the Schuler periodic oscillation of INS. The key to fault-tolerant damping is to set the external velocity error variation criterion, which can automatically determine damping switch.The external velocity error criterion is set according to the velocity difference between the INS and DVLvnc−vr, where vnc=v+dvnc and vr=v+dvr, so vnc−vr=dvnc−dvr. The difference between different times is recorded as(16)Δv=dvnct1−dvnct2−dvrt1−dvrt2,where the first term of the equation is the difference value of velocity error calculated by INS from time t1 to time t2, and the second term is the difference value of velocity error measured by DVL from time t1 to time t2. The 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.


## 4. Experiment

The effectiveness of the fault-tolerant level damping algorithm is verified by the actual navigation test data. The experiment equipment includes high-precision laser gyro INS and DVL. The experiment equipment parameters are as follows. The bias stability of the gyro is better than 0.003/h, and the angular random walk is better than 0.001/h. The bias stability of the accelerometer is better than 20 μg, and the noise power spectral density is better than 20 μg/HZ. The velocity measurement accuracy of DVL is 0.5%. There are two experiments, one with external position reference and the other without.


### 4.1. Damping Verification

The experiment is 26 hours marine navigation with external position reference. The INS position and satellite reference position are shown in Figure2. The blue curve is INS position, and the red curve is GNSS position. From the curve comparison, we can see that the Schuler periodic oscillation is obvious.Figure 2
INS position and satellite reference position.
(a)(b)The 2-order damping correction networkHs=s2+0.02ωss+ωs2/s2+ωss+ωs2 is selected for both traditional damping and the proposed fault-tolerant damping. The traditional damping is based on closed-loop feedback, while the proposed fault-tolerant damping is based on open-loop output feedback. As shown in Figures 3 and 4, the damping algorithm proposed in the paper can suppress Schuler periodic oscillation of velocity.Figure 3
INS velocity and damping velocity.
(a)(b)Figure 4
Difference between INS velocity and damping velocity.
(a)(b)Figure5 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. The position error curves show that damping suppresses Schuler periodic oscillation of position.Figure 5
INS position and damping position.
(a)(b)Figure 6
INS position error and damping position error.
(a)(b)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.


### 4.2. Fault-Tolerant Damping Verification

The experiment is 60 hours marine navigation without external position reference. The INS velocity and DVL velocity are shown in Figure7. The 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.Figure 7
INS velocity and DVL velocity.
(a)(b)As shown in Figure8, the fault-tolerant damping algorithm proposed in the paper suppresses Shuler periodic oscillation of position and achieves the traditional damping algorithm effect.Figure 8
Traditional damping position and fault-tolerant damping position.
(a)(b)In order to show the superiority of the fault-tolerant damping algorithm proposed in the paper, dynamic noise is manually added based on DVL velocity. The noise-added DVL velocityvre1andvrn1 is as follows:(17)vre1=vre+5cos2πt360−20,vrn1=vrn+5sin2πt360+20,19∗3600≤t<20∗3600,vre1=vre+5cos2πt360+20,vrn1=vrn+5sin2πt360−2020∗3600≤t<21∗3600.The dynamic noise is composed of two parts: one part is fixed deviation, which amplitude is 20. The fixed deviation lasts for 1 hour each time, and the pulling direction is different. The other part is the sine curve with an amplitude of 5 and a period of 6 minutes. As shown in Figure9, the velocity difference between INS and noise-added DVL is obvious between 19 and 21 hours.Figure 9
INS velocity and noise-added DVL velocity.
(a)(b)As shown in Figure10, 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.Figure 10
Undamped position and traditional damping position.
(a)(b)The velocity comparison between INS and fault-tolerant damping is shown in Figure11. 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).Figure 11
INS velocity and fault-tolerant damping velocity.
(a)(b)As shown in Figure12, 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.Figure 12
Traditional damping position and fault-tolerant damping position with adding noise.
(a)(b)


## 4.1. Damping Verification

The experiment is 26 hours marine navigation with external position reference. The INS position and satellite reference position are shown in Figure2. The blue curve is INS position, and the red curve is GNSS position. From the curve comparison, we can see that the Schuler periodic oscillation is obvious.Figure 2
INS position and satellite reference position.
(a)(b)The 2-order damping correction networkHs=s2+0.02ωss+ωs2/s2+ωss+ωs2 is selected for both traditional damping and the proposed fault-tolerant damping. The traditional damping is based on closed-loop feedback, while the proposed fault-tolerant damping is based on open-loop output feedback. As shown in Figures 3 and 4, the damping algorithm proposed in the paper can suppress Schuler periodic oscillation of velocity.Figure 3
INS velocity and damping velocity.
(a)(b)Figure 4
Difference between INS velocity and damping velocity.
(a)(b)Figure5 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. The position error curves show that damping suppresses Schuler periodic oscillation of position.Figure 5
INS position and damping position.
(a)(b)Figure 6
INS position error and damping position error.
(a)(b)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.


## 4.2. Fault-Tolerant Damping Verification

The experiment is 60 hours marine navigation without external position reference. The INS velocity and DVL velocity are shown in Figure7. The 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.Figure 7
INS velocity and DVL velocity.
(a)(b)As shown in Figure8, the fault-tolerant damping algorithm proposed in the paper suppresses Shuler periodic oscillation of position and achieves the traditional damping algorithm effect.Figure 8
Traditional damping position and fault-tolerant damping position.
(a)(b)In order to show the superiority of the fault-tolerant damping algorithm proposed in the paper, dynamic noise is manually added based on DVL velocity. The noise-added DVL velocityvre1andvrn1 is as follows:(17)vre1=vre+5cos2πt360−20,vrn1=vrn+5sin2πt360+20,19∗3600≤t<20∗3600,vre1=vre+5cos2πt360+20,vrn1=vrn+5sin2πt360−2020∗3600≤t<21∗3600.The dynamic noise is composed of two parts: one part is fixed deviation, which amplitude is 20. The fixed deviation lasts for 1 hour each time, and the pulling direction is different. The other part is the sine curve with an amplitude of 5 and a period of 6 minutes. As shown in Figure9, the velocity difference between INS and noise-added DVL is obvious between 19 and 21 hours.Figure 9
INS velocity and noise-added DVL velocity.
(a)(b)As shown in Figure10, 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.Figure 10
Undamped position and traditional damping position.
(a)(b)The velocity comparison between INS and fault-tolerant damping is shown in Figure11. 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).Figure 11
INS velocity and fault-tolerant damping velocity.
(a)(b)As shown in Figure12, 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.Figure 12
Traditional damping position and fault-tolerant damping position with adding noise.
(a)(b)


## 5. Conclusion

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



---

*Source: 8847718-2020-11-04.xml*