Sculling Compensation Algorithm for SINS Based on Two-Time Scale Perturbation Model of Inertial Measurements

Abstract

In order to decrease the velocity sculling error under vibration environments, a new sculling error compensation algorithm for strapdown inertial navigation system (SINS) using angular rate and specific force measurements as inputs is proposed in this paper. First, the sculling error formula in incremental velocity update is analytically derived in terms of the angular rate and specific force. Next, two-time scale perturbation models of the angular rate and specific force are constructed. The new sculling correction term is derived and a gravitational search optimization method is used to determine the parameters in the two-time scale perturbation models. Finally, the performance of the proposed algorithm is evaluated in a stochastic real sculling environment, which is different from the conventional algorithms simulated in a pure sculling circumstance. A series of test results demonstrate that the new sculling compensation algorithm can achieve balanced real/pseudo sculling correction performance during velocity update with the advantage of less computation load compared with conventional algorithms.

1. Introduction

There are two key calculations in a typical strapdown inertial navigation system (SINS), namely, the attitude update and velocity update algorithms. The total acceleration in SINS is expressed in terms of the specific force, gravity and centrifugal acceleration. The velocity update algorithm of SINS is a process of integrating total acceleration in the navigation frame. During the velocity update process, the velocity differential equation can be obtained by transforming the specific force from the body frame to the navigation frame based on the attitude update result, and including other accelerations.

For the attitude update, many algorithms with high accuracy have been developed [1,2,3]. For the velocity update, many research works focus on multi-sensor information fusion based on various filtering techniques to achieve high accuracy velocity for alignment and navigation of SINS. In order to improve velocity estimation for alignment, an adaptive unscented Kalman filter with the aid of a star sensor is proposed in [4]. A 12-state Kalman filter is used to estimate the velocity error for the stationary self-alignment in [5]. In [6], a method using the Kalman filter based on integrating gravitational apparent motion to form apparent velocity is designed to reduce the random noise of the observation vectors for self-alignment. A tightly coupled SINS/GPS with the extended Kalman filter is utilized in [7] to improve the attitude and velocity update accuracy with the observability analysis of the whole system. Although the above methods achieve better performances, they all need other observable information with higher precision as measurement vectors. Recently, a dual Kalman filter has been developed in [8,9] to estimate the unknown input and states by using sparse noisy acceleration measurements. The outcome indicates that this method can achieve optimal performance in the presence of colored measurement and modelling errors, and it is effective for the state estimation of the metallic structures under low frequency vibrations due to unknown input forces. However, the aircraft considered in this paper works in a more complex environment than the metallic structure in that it undergoes not only low frequency vibrations but also wideband vibrations.

In a dynamic oscillating environment, the conventional velocity update usually generates the delta velocity quantity and causes the navigation system to degrade if there is no sculling compensation. Another method to enhance the velocity accuracy is to improve the numerical calculation of the velocity differential equation. In the past few decades, considerable researches have been focused on developing highly accurate and highly efficient velocity update algorithms. Savage [10,11] proposed a two-speed velocity update algorithm and a velocity translation vector. Sculling error compensation was computed with high speed by integrating the exact velocity translation vector differential equation, while the velocity was updated in a moderate-speed process. To further improve the accuracy of the two-speed velocity update algorithm, Wu et al. [12] developed a simpler dual quaternion (DQ) algorithm to determine the rotation and apparent velocity as a whole. Chelnokov [13] proved that the DQ solutions were stable in Lyapunov’s sense and the two-speed DQ algorithm achieved the third and fourth order of accuracy. In [14,15], geometry algebra and Lie group methods were used to construct the two-speed velocity update algorithms with comparable accuracy to the DQ algorithm. In addition, a simplified parallel velocity error compensation algorithm was proposed in [16,17] based on the two-speed velocity update concept and executed on a single-chip field programmable gate array (FPGA).

For the above velocity update algorithms, the sculling motion was assumed to oscillate with constant amplitude and frequency, and the signals from the accelerometers and gyroscopes were measured with white noise. However, in real dynamic environments, there exist random sculling motion and nonwhite stochastic noise in inertial sensor measurements. Previous works cannot mitigate the effect of these errors, thus limiting the velocity update accuracy in real applications. In order to overcome this limitation, Wang et al. [18] presented a nonlinear robust multiple integrator algorithm to estimate velocity from the measurements of accelerometers based on finite-time stability and singular perturbation technique. Furthermore, with the assumption that the derivative of acceleration satisfies a particular perturbation model, the presented algorithm proved that the velocity estimation was not only robust to the nonwhite noise but also finite-time stable. However, the particular perturbation model makes this method difficult to extend to the velocity update used in the SINS algorithm design. In [19], a new coning correction algorithm, based on the two-time scale perturbation model of the angular rate, was proposed for the attitude update algorithm with non-ideal angular rate information. It is proved valid for improving accuracy with reduced computations of the attitude update in SINS undergoing stochastic coning environments.

On the other hand, equivalency between the strapdown coning and the sculling correction algorithm has been proved in [20]. Additionally, the singular perturbation model can be used to analyze the vehicle dynamics equipped with an aerodynamic vectoring feature and has been applied in designing aerospace guidance and control systems [21,22]. In this paper, we extend the previous work to design the new velocity update algorithm based on singular perturbation.

In order to decrease the dynamic integration error of velocity update under vibration and other complicated environments, a new sculling compensation algorithm for the velocity integration based on a two-time scale perturbation model of inertial information is presented in this paper. An analytical expression for the sculling error from the specific force integration process is derived in Section 2. This expression can provide a solution to the dynamic sculling velocity error compensation. Then, we analyze the frequency spectrum of the specific force under a real vibration environment to provide a basis for the two-time scale perturbation model. Based on the analytical expression and two-time scale perturbation model of the specific force, a new sculling error compensation algorithm is derived in Section 3 combined with our previous work on attitude update [19]. A gravitational search optimization method is used to determine the perturbation parameters in the sculling error correction term. The performance of the new sculling compensation algorithm is evaluated and compared with the conventional sculling compensation algorithm under stochastic sculling motions in Section 4. The simulation results indicate that the proposed sculling compensation algorithm improves the velocity update accuracy and decreases computation load as well.

2. Sculling Error Formula in Velocity Update

The following relevant coordinate frames about navigation were used:

• The body coordinate frame (b-frame) is the strapdown inertial sensor frame. It is a right-handed frame with the origin at the center of vehicle gravity. Its X-axis, Y-axis and Z-axis are aligned with the roll, pitch and yaw directions of the vehicle, respectively.
• The navigation coordinate frame (n-frame) is a local geographic frame with its X-axis, Y-axis, and Z-axis aligned with the directions of north, east, and the local vertical (down), respectively.
• The earth coordinate frame (e-frame) has an Earth-fixed angular geometry relative to the earth with the origin at the center of earth. The X-axis passes through the Greenwich meridian and the Y-axis is perpendicular to X-axis in the equatorial plane. Z-axis points toward the north pole.
• The inertial coordinate frame (i-frame) is a non-rotating frame with its origin at the center of earth. Its X-axis is in the equatorial plane pointing towards the vernal equinox and the Z-axis points toward the north pole. The Y-axis completes the right-handed frame.

According to the mechanization process of SINS, the velocity update equation in n-frame is known as:

$${\dot{\mathit{v}}}^n={\mathit{C}}_b^n{\mathit{f}}^b-(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\times {\mathit{v}}^n+{\mathit{g}}^n$$

(1)

where ${\mathit{C}}_b^n$ is a direction cosine matrix that transforms a vector from b-frame to n-frame. ${\mathit{f}}^b$ is the specific force vector measured by the strapdown accelerometer in b-frame. ${\mathit{\omega }}_{ie}^n$ denotes the earth angular rate relative to i-frame expressed in n-frame, and ${\mathit{\omega }}_{en}^n$ is the angular rate of n-frame with respect to e-frame expressed in n-frame. ${\mathit{v}}^n$ is the vehicle velocity in n-frame and ${\dot{\mathit{v}}}^n$ is the velocity change over the velocity update interval. ${\mathit{g}}^n$ is the gravity vector expressed in n-frame.

It can be seen that the velocity update Equation (1) contains three terms: the transformed specific force term, the Coriolis acceleration, and the apparent gravity, respectively. The second and third terms vary slowly and smoothly during one navigation period and can be calculated using the linear interpolation method over a small time period [23]. However, under the condition of large angular motions, the attitude changes rapidly in the update interval, which can cause large errors in the specific force integration and reduce the velocity update accuracy. Therefore, the velocity update algorithm must account for the error caused by frame rotation to meet the requirement of precise navigation [24,25]. Let ${\mathit{f}}^n$ be the specific force vector with respect to n-frame and utilize the strapdown accelerometer output ${\mathit{f}}^b$, then

$${\mathit{f}}^n={\mathit{C}}_b^n{\mathit{f}}^b$$

(2)

Integrate the specific force in n-frame over each sculling update interval from $t_k$ to $t_{k+1}$ as follows

$${\mathit{u}}^n={\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{C}}_b^n{\mathit{f}}^b}\mathrm{d}t$$

(3)

where $t_k$ and $t_{k+1}$ represent the beginning time and ending time of one sculling update interval. ${\mathit{u}}^n$ denotes velocity increment in n-frame from $t_k$ to $t_{k+1}$.

Define ${\mathit{C}}_{n(k)}^{n(k+1)}$ as the direction cosine matrix from n-frame at $t_k$ to n-frame at $t_{k+1}$. The direction cosine matrix from b-frame at $t_k$ to n-frame at $t_k$ is denoted by ${\mathit{C}}_{b(k)}^{n(k)}$. Denote ${\mathit{C}}_{b(t)}^{b(k)}$ as the direction cosine matrix from b-frame at $t$ to b-frame at $t_k$. Then, the direction cosine matrix from b-frame at $t$ to n-frame at $t_{k+1}$ becomes

$${\mathit{C}}_b^n={\mathit{C}}_{n(k)}^{n(k+1)}{\mathit{C}}_{b(k)}^{n(k)}{\mathit{C}}_{b(t)}^{b(k)}$$

(4)

Letting $\mathit{C}={\mathit{C}}_{n(k)}^{n(k+1)}{\mathit{C}}_{b(k)}^{n(k)}$ and substituting Equation (4) into Equation (3) yields

$${\mathit{u}}^n=\mathit{C}{\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{C}}_{b(t)}^{b(k)}{\mathit{f}}^b}\mathrm{d}t$$

(5)

where ${\mathit{C}}_{b(t)}^{b(k)}=\mathit{I}+[\mathit{\varphi }\times ]+0.5{[\mathit{\varphi }\times ]}^2+\cdots$, $\mathit{\varphi }$ denotes the rotation vector defining the body attitude from $t_k$ to t [4,17]. $\mathit{\varphi }\times$ is the skew symmetric matrix of the rotation vector $\mathit{\varphi }$. Therefore, Equation (5) can be derived as

$${\mathit{u}}^n=\mathit{C}{\displaystyle {\int }_{t_k}^{t_{k+1}}\left(\mathit{I}+[\mathit{\varphi }\times ]+0.5{[\mathit{\varphi }\times ]}^2+\cdots \right){\mathit{f}}^b}\mathrm{d}t$$

(6)

For most SINS mechanization, the second cross-product term and other higher order terms can be assumed small enough to be negligible. Thus, ${\mathit{u}}^n$ can be approximated by

$${\mathit{u}}^n=\mathit{C}\left({\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{f}}^b}\mathrm{d}t+{\displaystyle {\int }_{t_k}^{t_{k+1}}\mathit{\varphi }\times {\mathit{f}}^b}\mathrm{d}t\right)$$

(7)

Over a small time interval, the rotation vector $\mathit{\varphi }$ can be approximated by

$$\mathit{\varphi }\approx \mathit{\alpha }(t)={\displaystyle {\int }_{t_k}^t\mathit{\omega }}(\tau )\mathrm{d}\tau$$

(8)

where $\mathit{\omega }(\tau )$ is the angular rate vector in b frame. Consider the following equation,

$$\mathit{\alpha }(t)\times \frac{\mathrm{d}}{\mathrm{d}t}\mathit{v}(t)=\frac{1}{2}\frac{\mathrm{d}}{\mathrm{d}t}\left(\mathit{\alpha }(t)\times \mathit{v}(t)\right)+\frac{1}{2}\left[\mathit{\alpha }(t)\times \frac{\mathrm{d}}{\mathrm{d}t}\mathit{v}(t)+\mathit{v}(t)\times \frac{\mathrm{d}}{\mathrm{d}t}\mathit{\alpha }(t)\right]$$

(9)

where $\mathit{v}(t)={\displaystyle {\int }_{t_k}^t{\mathit{f}}^b(\tau )}\mathrm{d}\tau$. Substituting Equations (8) and (9) into Equation (7) yields

$$\begin{array}{ll}{\mathit{u}}^n& =\mathit{C}\left({\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{f}}^b}\mathrm{d}t+{\displaystyle {\int }_{t_k}^{t_{k+1}}\mathit{\varphi }\times {\mathit{f}}^b}\mathrm{d}t\right)\\ & =\mathit{C}\left({\displaystyle {\int }_{t_k}^{t_{k+1}}{\mathit{f}}^b}\mathrm{d}t+{\displaystyle {\int }_{t_k}^{t_{k+1}}\mathit{\alpha }(t)\times {\mathit{f}}^b}\mathrm{d}t\right)\\ & =\mathit{C}(\mathit{v}(t_{k+1})+{\displaystyle {\int }_{t_k}^{t_{k+1}}\left\{\frac{1}{2}\frac{\mathrm{d}}{\mathrm{d}t}(\mathit{\alpha }(t)\times \mathit{v}(t))+\frac{1}{2}\left[\mathit{\alpha }(t)\times \frac{\mathrm{d}}{\mathrm{d}t}\mathit{v}(t)+\mathit{v}(t)\times \frac{\mathrm{d}}{\mathrm{d}t}\mathit{\alpha }(t)\right]\right\}}\mathrm{d}t)\\ & =\mathit{C}\left(\mathit{v}(t_{k+1})+\frac{1}{2}\left[\mathit{\alpha }(t)\times \mathit{v}(t)\right]{|}_{t_k}^{t_{k+1}}+\frac{1}{2}{\displaystyle {\int }_{t_k}^{t_{k+1}}\left[\mathit{\alpha }(t)\times {\mathit{f}}^b+\mathit{v}(t)\times \mathit{\omega }\right]\mathrm{d}t}\right)\end{array}$$

(10)

The second term $\frac{1}{2}\left[\mathit{\alpha }(t)\times \mathit{v}(t)\right]{|}_{t_k}^{t_{k+1}}$ in Equation (10) is defined as the rotation error compensation term, which represents a change in velocity due to rotational motion in the update cycle. This term leads to the low-frequency component during integration and can be compensated easily. The third integral term in Equation (10) is the sculling error compensation term, which is

$${\mathsf{\Delta }}_{scull}=\frac{1}{2}{\displaystyle {\int }_{t_k}^{t_{k+1}}\left[\mathit{\alpha }(t)\times {\mathit{f}}^b+\mathit{v}(t)\times \mathit{\omega }\right]\mathrm{d}t}$$

(11)

This term represents the integration contribution of the high-frequency component and results in rectified error during velocity update especially in the sculling motions. Thus, the key of velocity error compensation is to accurately estimate and compensate the sculling term.

Based on the analytical expression of the sculling error term, it is known that when high-frequency angular and linear vibrations occur in two or more axes simultaneously, the sculling error in velocity will be generated in the inertial sensor [10,12,17]. To improve the velocity accuracy, a series of high-rate algorithms for sculling integral compensation using angle and velocity increments as algorithm inputs have been developed [25,26]. However, they are not suitable for modern inertial sensors whose outputs are the angular rate and specific force. Some researchers designed sculling error correction algorithms based on the angular rate or linearly ramping polynomial specific force models, whose coefficients are estimated through Taylor or Picard series polynomial expansion [23,24,27]. However, it is ignored that the angular rate and specific force are changing randomly and non-linearly especially in high dynamics or complicated vibration environments. In addition, the conventional three-interval or four-interval sculling correction algorithms are usually simulated under pure sculling environments without considering vibration effect, and the results are not optimal in real vibration environments.

3. Sculling Error Compensation Algorithm Based on Two-Time Scale Perturbation Model

3.1. Performance Analysis of Specific Force under Vibrations

For aircraft, engine noise and air disturbance are usually the main vibration sources [28]. The engine noise mainly depends on aircraft type. When the engine starts to work, the engine vibration is transmitted to the vehicle and affects the normal operation of onboard instruments. Air disturbance is influenced by meteorological conditions with strong random characteristics. When the aircraft flies from one irregular vortex to another in turbulent areas, the unstable air flow causes vibration. Especially, the vibration strength will be intensified if the vibration natural frequency of aircraft is equal to the vortex vibration frequency. In addition, the rotor vibration is also the main vibration source for rotor aircraft. Most vibrations can be sensed directly by the inertial components mounted on the aircraft, which lead to coning and sculling errors in attitude and velocity updates, respectively. In order to compensate the sculling error, the specific force with wide-band nonwhite noise under aircraft dynamic environments is analyzed at first. Figure 1 shows the real raw data of the X-axis accelerometer fixed on the vehicle with 1.6 KHz sampling frequency. Some outliers can be observed in the specific force measurement except noises. Thus, it is necessary to analyze the frequency spectrum of the specific force and further preprocess the specific force to improve its signal-to-noise ratio under complicated circumstances.

Generally, a low-pass filter is often used in the preprocessing to filter high frequency noises, and enhances the signal-to-noise ratio. However, the remnant noises with high frequency still exist even after preprocessing. The result of utilizing the power spectral density (PSD) method to analyze the frequency response of filtered X-axis specific force in a random vibration experiment is shown in Figure 2a. The remnant noise with wide frequency bandwidth up to a thousand Hz is shown in Figure 2b.

It can be seen from Figure 2a that the frequency spectrum of the specific force can be partitioned into two regions under the real experimental conditions: a vehicle motion region characterized by higher dynamic energy and lower frequency bandwidth, and the wide-band noise region characterized by lower dynamic energy and higher frequency. The specific force over the sculling update interval has the characteristic with slow and fast time scale, which satisfies the requirement of the two-time scale model.

On the other hand, the Taylor series polynomial model of the specific force is usually adopted in conventional sculling algorithms to obtain the optimal coefficients of the sculling error correction term. For instance, the fourth-order fitting specific force model is often utilized in the four-interval algorithm compensating the sculling error. In fact, it is difficult for this deterministic model to describe the specific force with the slow and fast time scale characteristic under real vibration environments. Supposing the frequencies of velocity update and sculling update are 100 Hz and 400 Hz respectively, the inertial sensor’s sample frequency should be 1.6 KHz in the conventional four-interval sculling algorithm. This indicates that four sample measurements should be included in one sculling update interval. The estimates derived from the fourth-order fitting specific force model and the preprocessed specific force measurement over one sculling update interval are shown in Figure 3.

The estimates based on the fourth-order fitting specific force model cannot completely represent the real specific force characteristic over the sculling update interval. It can be seen that the errors between the estimates and real measurements can reach 0.0002 g at the second sample point and 0.0001 g at the third sample point. These errors will be accumulated after integration and cause large velocity error over time. Combing Figure 2 and Figure 3, a new model of specific force is constructed to better represent the frequency spectrum of specific force with less estimation error in Section 3.2, and the sculling error correction term based on the new model is derived in Section 3.3.

3.2. Two-Time Scale Perturbation Model of Specific Force

The frequency spectrum of specific force demonstrates the slow and fast characteristic time scales over the sculling update interval. In [29,30], it was found that a two-time scale model can be used to describe this experimental frequency spectrum shape. Moreover, the Markov process with weak interactions has been widely used to conveniently describe the noise in inertial sensor outputs and it can also be modeled as singularly perturbed systems [30]. Therefore, due to the simultaneous presence of slow and fast phenomena over the sculling update interval, the specific force can be characterized by a singularly perturbed process and modeled as a two-time scale perturbation model

$$\epsilon \ddot{\mathit{f}}(\tau )+\dot{\mathit{f}}(\tau )+\mathit{\beta }=0$$

(12)

With the boundary conditions $\mathit{f}(0)={\mathit{f}}_1\mathrm{and}\mathit{f}(T)={\mathit{f}}_2$, where $\epsilon$ is a small positive perturbation parameter; $\tau$ is the time relative to the beginning of a sculling update interval; $\mathit{\beta }$ is a variable vector; ${\mathit{f}}_1$ and ${\mathit{f}}_2$ are the specific force vectors sensed by the accelerometer at the beginning and ending of a sculling update interval, respectively; $T$ is the sculling update interval.

Although it is easy to determine the exact solution of Equation (12), a simplified composite solution to a two-time scale system is chosen because it can achieve nearly the same performance as the exact solution but demand less computation load [19]. From Equation (12), the singularly perturbed model of the specific force includes two subsystems: the inner subsystem and the outer subsystem. When solving the singularly perturbed model of the specific force with the boundary layer method, the simplified composite solution to Equation (12) can be obtained by solving the differential equation for each subsystem and combining their solutions.

The inner subsystem (boundary layer) is derived by stretching the time scale from $\tau$ to $\eta =\tau /\epsilon$, then $\dot{\mathit{f}}(\tau )=\frac{1}{\epsilon }\dot{\mathit{f}}(\eta )$ and $\ddot{\mathit{f}}(\tau )=\frac{1}{{\epsilon }^2}\ddot{\mathit{f}}(\eta )$. Substituting them into Equation (12) and letting $\epsilon =0$ yield

$${\ddot{\mathit{f}}}^{(i)}(\eta )+{\dot{\mathit{f}}}^{(i)}(\eta )=0\eta \in [0,\infty )$$

(13)

where the superscript ‘i’ stands for “inner”. In solving the perturbation equation, the transformed initial condition of the inner boundary layer should include the initial value of the outer subsystem, i.e., ${\mathit{f}}^{(i)}(0)={\mathit{f}}_1-{\mathit{f}}^{(o)}(0)$, where ${\mathit{f}}^{(i)}(0)$ is the initial value of the inner subsystem, and ${\mathit{f}}^{(o)}(0)$ is the initial value of the outer subsystem. The superscript ‘o’ stands for “outer”. The slower vector ${\mathit{f}}^{(o)}(\tau )$ is considered as a constant equal to its initial value in the boundary layer and the faster vector ${\mathit{f}}^{(i)}(\eta )$ satisfies $\underset{\eta \to \infty }{\lim }{\mathit{f}}^{(i)}(\eta )=0$.

By solving the second-order differential Equation (13), the solution to the inner boundary layer is

$${\mathit{f}}^{(i)}(\eta )=({\mathit{f}}_1-{\mathit{f}}^{(o)}(0)){\mathrm{e}}^{-\eta }$$

(14)

Suppose that the parameter $\epsilon$ is very small. The outer subsystem of Equation (12) can be described by

$${\dot{\mathit{f}}}^{(o)}(\tau )+\mathit{\beta }=0$$

(15)

with its terminal condition ${\mathit{f}}^{(o)}(T)={\mathit{f}}_2$. Integrating Equation (15) yields the outer subsystem solution

$${\mathit{f}}^{(o)}(\tau )=-\mathit{\beta }\tau +\mathit{\beta }T+{\mathit{f}}_2$$

(16)

with

$${\mathit{f}}^{(o)}(0)=\mathit{\beta }T+{\mathit{f}}_2$$

(17)

The approximate composite solution of Equation (12) is constructed as the sum of the inner solution and the outer solution based on Equations (14), (16) and (17), that is

$$\mathit{f}(\tau )={\mathit{f}}^{(i)}(\frac{\tau }{\epsilon })+{\mathit{f}}^{(o)}(\tau )=({\mathit{f}}_1-\mathit{\beta }T-{\mathit{f}}_2){\mathrm{e}}^{-\left(\frac{\tau }{\epsilon }\right)}-\mathit{\beta }\tau +\mathit{\beta }T+{\mathit{f}}_2$$

(18)

The two-time scale model of angular rate can be constructed by the same method based on a singularly perturbed system. The approximate composite solution of the angular rate perturbation model over the sculling update interval can be derived as:

$$\mathit{\omega }(\tau )=({\mathit{\omega }}_1-\mathit{\gamma }T-{\mathit{\omega }}_2){\mathrm{e}}^{-\left(\frac{\tau }{\epsilon }\right)}-\mathit{\gamma }\tau +\mathit{\gamma }T+{\mathit{\omega }}_2$$

(19)

where $\mathit{\gamma }$ is a vector in the angular rate singularly perturbed system; ${\mathit{\omega }}_1$ and ${\mathit{\omega }}_2$ are the angular rate vectors sensed by the gyroscopes on the vehicle at the beginning and ending time of the sculling update interval, respectively. Equations (18) and (19) are defined as the two-time scale perturbation models of inertial measurements.

In the traditional sculling compensation algorithms, two conflicting requirements usually exist. On one hand, the navigation system attempts to obtain accurate results by integrating high speed sensor data in very small step sizes, but on the other hand, this flood of data may cause great burden to the processor [31]. A good tradeoff is the key factor for sculling error compensation. It is known that the specific force model based on the polynomial in the N-interval traditional algorithm requires N new sample values over one sculling update interval, whereas the specific force based on the new two-time scale model only uses two sample values at the beginning and ending of the sculling update interval for integration. Thus, the frequency of sensor sampling based on the two-time scale model is the same as the frequency of sculling update.

3.3. Sculling Compensation Algorithm Based on Two-Time Scale Perturbation Models of Inertial Measurements

The two-time scale perturbation models of the specific force and angular rate are used to derive the sculling error correction term in this section. According to Equations (18) and (19), the inertial signals can be expressed in another form as follows:

$$\mathit{\omega }(t)=\mathit{a}{\mathrm{e}}^{-\left(\frac{t}{\epsilon }\right)}-\mathit{b}t+\mathit{d}\mathrm{and}\mathit{f}(t)=\mathit{A}{\mathrm{e}}^{-\left(\frac{t}{\epsilon }\right)}-\mathit{B}t+\mathit{D}$$

(20)

where the parameters in Equation (20) are

$$\begin{array}{l}\mathit{a}={\mathit{\omega }}_1-\mathit{\gamma }T-{\mathit{\omega }}_2,\mathit{b}=\mathit{\gamma },\mathit{d}=\mathit{\gamma }T+{\mathit{\omega }}_2\\ \mathit{A}={\mathit{f}}_1-\mathit{\beta }T-{\mathit{f}}_2,\mathit{B}=\mathit{\beta },\mathit{D}=\mathit{\beta }T+{\mathit{f}}_2\end{array}$$

(21)

The incremental angle vector $\mathit{\alpha }$ and incremental velocity vector $\mathit{v}$ from time 0 to t are

$$\mathit{\alpha }=-\epsilon \mathit{a}\left({\mathrm{e}}^{-\frac{t}{\epsilon }}-1\right)-\frac{1}{2}\mathit{b}{t}^2+\mathit{d}t,\mathit{v}=-\epsilon \mathit{A}\left({\mathrm{e}}^{-\frac{t}{\epsilon }}-1\right)-\frac{1}{2}\mathit{B}{t}^2+\mathit{D}t$$

(22)

Substituting Equations (21) and (22) into Equation (11), the sculling error correction term over the sculling update interval based on the proposed models is derived as

$$\begin{array}{ll}{\widehat{\mathsf{\Delta }}}_{scull}& =\frac{1}{2}{\displaystyle {\int }_{t_k}^{t_{k+1}}(\mathit{v}\times \mathit{\omega }+\mathit{\alpha }\times \mathit{f})}\mathrm{d}t\\ & =\frac{1}{2}(\mathit{A}\times \mathit{b}-\mathit{B}\times \mathit{a})(\frac{1}{2}\epsilon T^2+2{\epsilon }^3)+\frac{1}{2}(\mathit{D}\times \mathit{a}-\mathit{A}\times \mathit{d})(2{\epsilon }^2-\epsilon T)+\frac{1}{12}(\mathit{B}\times \mathit{d}-\mathit{D}\times \mathit{b})\end{array}$$

(23)

Substituting Equation (21) into Equation (23) yields

$$\begin{array}{l}\mathit{A}\times \mathit{b}-\mathit{B}\times \mathit{a}=({\mathit{f}}_1-{\mathit{f}}_2)\times \mathit{\gamma }+({\mathit{\omega }}_1-{\mathit{\omega }}_2)\times \mathit{\beta }\\ \mathit{D}\times \mathit{a}-\mathit{A}\times \mathit{d}={\mathit{f}}_2\times {\mathit{\omega }}_1-{\mathit{f}}_1\times {\mathit{\omega }}_2+\mathit{\gamma }T\times {\mathit{f}}_1+\mathit{\beta }T\times {\mathit{\omega }}_1\\ \mathit{B}\times \mathit{d}-\mathit{D}\times \mathit{b}=\mathit{\beta }\times {\mathit{\omega }}_2-{\mathit{f}}_2\times \mathit{\gamma }\end{array}$$

(24)

A numerical algorithm for computing the sculling integral term is developed based on Equations (23) and (24). In the conventional four-interval algorithm based on the fourth-order fitting, the sculling compensation term is [32]

$$\begin{array}{ll}{\widehat{\mathsf{\Delta }}}_{scull}=& K_{v1}\left[\mathsf{\Delta }{\theta }_1\times \mathsf{\Delta }{v}_2+\mathsf{\Delta }{\theta }_3\times \mathsf{\Delta }{v}_4+\mathsf{\Delta }{v}_1\times \mathsf{\Delta }{\theta }_2+\mathsf{\Delta }{v}_3\times \mathsf{\Delta }{\theta }_4\right]\\ & +K_{v2}\left[\mathsf{\Delta }{\theta }_1\times \mathsf{\Delta }{v}_3+\mathsf{\Delta }{\theta }_2\times \mathsf{\Delta }{v}_4+\mathsf{\Delta }{v}_1\times \mathsf{\Delta }{\theta }_3+\mathsf{\Delta }{v}_2\times \mathsf{\Delta }{\theta }_4\right]\\ & +K_{v3}\left[\mathsf{\Delta }{\theta }_1\times \mathsf{\Delta }{v}_4+\mathsf{\Delta }{v}_1\times \mathsf{\Delta }{\theta }_4\right]+K_{v4}\left[\mathsf{\Delta }{\theta }_2\times \mathsf{\Delta }{v}_3+\mathsf{\Delta }{v}_2\times \mathsf{\Delta }{\theta }_3\right]\end{array}$$

(25)

where $K_{v1}=\frac{736}{945}$, $K_{v2}=\frac{334}{945}$, $K_{v3}=\frac{526}{945}$, and $K_{v4}=\frac{654}{945}$ are the coefficients derived from the fourth-order fitting model of inertial information. In order to calculate the sculling compensation term in the conventional four-interval algorithm, four new inertial measurements about angular and linear motions should be supplied in one sculling interval $\mathsf{\Delta }{\theta }_1,\mathsf{\Delta }{\theta }_2,\mathsf{\Delta }{\theta }_3,\mathsf{\Delta }{\theta }_4$ are angular increments and $\mathsf{\Delta }{v}_1,\mathsf{\Delta }{v}_2,\mathsf{\Delta }{v}_3,\mathsf{\Delta }{v}_4$ are velocity increments at the four different sampling times during one sculling interval. It is convenient to obtain the sculling compensation term according to Equation (25) utilizing the inertial sensors whose outputs are the angular increment and velocity increment. However, the measurements of most inertial sensors nowadays are the angular rate and specific force. The angles and velocities at the sampling times can be calculated by integrating the measurements of the angular rate and specific force, and then the angular and velocity increments can be obtained by further processing of the calculated angles and velocities, which will introduce more calculation errors.

By comparing Equations (23) and (25), it can be seen that the conventional sculling algorithm utilizes angular and velocity increments to compute the sculling correction term, while the proposed sculling algorithm directly uses the angular rate and specific force as inputs and estimates the sculling correction term based on their two-time scale perturbation models. Therefore, the proposed approach is more appropriate for the modern accelerometer with the specific force output and gyroscope with the angular rate output. Additionally, the proposed approach can complete the sculling compensation with less sample measurements compared with the conventional four-interval algorithm that requires more sensor sampling data. In particular, in one sculling update interval, the proposal algorithm needs two sample measurements from each sensor (${\mathit{\omega }}_1$ and ${\mathit{\omega }}_2$ from gyroscope, and ${\mathit{f}}_1$ and ${\mathit{f}}_2$ from the accelerometer), whereas the conventional four-interval algorithm needs four sample measurements ($\mathsf{\Delta }{\theta }_1,\mathsf{\Delta }{\theta }_2,\mathsf{\Delta }{\theta }_3,\mathsf{\Delta }{\theta }_4$ for angular measurements and $\mathsf{\Delta }{v}_1,\mathsf{\Delta }{v}_2,\mathsf{\Delta }{v}_3,\mathsf{\Delta }{v}_4$ for velocity measurements). In addition, tuning the parameter $\epsilon$ and vectors $\mathit{\beta }$ and $\mathit{\gamma }$ can effectively improve the accuracy of the proposed sculling correction algorithm. The parameter selection rules will be given in the next section.

3.4. Selection of Perturbation Model Parameters Based on Gravitational Search Optimization Method

The parameter $\epsilon >0$ is a sufficiently small constant that represents the separation of time scales between the fast dynamics and the slow dynamics of inertial sensor outputs [19]. The vector parameters $\mathit{\beta }$ and $\mathit{\gamma }$ describe the vehicle specific force and angular rate motion in the sculling update interval.

Among many optimization algorithms, the gravitational search algorithm based on Newton’s law of universal gravitation can perform the global search in the solution space [33]. In this algorithm, each particle is regarded as a mass object in the solution space and it moves in the gravitational field of all other particles. The particles with small or large fitness will approach the optimal solution at a slow or fast speed, respectively. In order to determine the parameters $\epsilon$, $\mathit{\beta }$, and $\mathit{\gamma }$, the gravitational search optimization algorithm is used due to its excellent search speed and ability to tune and obtain the optimal solution.

The parameters $\epsilon$, $\mathit{\beta }$ and $\mathit{\gamma }$ in the two-time scale perturbation models of inertial measurements are considered as mass particles and optimized by the gravitational search algorithm. Define the fitness function as follows

$$F_{fit}=\frac{1}{N}{\displaystyle \sum _{i=1}^N\left[{\left(X_i-{X_i}^{\prime }\right)}^2+{\left(Y_i-{Y_i}^{\prime }\right)}^2\right]}$$

(26)

where N is the number of mass particles. $X_i$ and $Y_i$ are inertial measurements of the specific force or angular rate corresponding to the ith particle. ${X_i}^{\prime }$ and ${Y_i}^{\prime }$ are their estimates based on the two-time scale perturbation models. The flowchart of the parameter optimization algorithm is shown in Figure 4.

First, initialize the velocity and position of mass particles in the three dimensions randomly. Calculate the fitness values of all mass particles by Equation (26). According to Equation (27), the inertial mass of each mass particle is updated using the fitness value when the particle is moving:

$$m_i\left(t\right)=\frac{fi{t}_i\left(t\right)-worst\left(t\right)}{best\left(t\right)-worst\left(t\right)},M_i\left(t\right)=m_i\left(t\right)/{\displaystyle \sum _{j=1}^N{m}_j\left(t\right)}$$

(27)

where $M_i\left(t\right)$ denotes the inertial mass of each particle at the tth iteration. $fi{t}_i\left(t\right)$ is the fitness value of the ith particle at the tth iteration. $best\left(t\right)$ and $worst\left(t\right)$ are the minimum and maximum fitness values of all particles at the tth iteration, respectively.

Next, calculate the mutual force acting on the mass particle with the inertial mass based on Newton’s law of universal gravitation. The force of the jth particle on the ith particle is

$$F_{ij}=G\left(t\right)\frac{M_j\left(t\right)M_i\left(t\right)}{R_{ij}\left(t\right)}\left(x_j\left(t\right)-x_i\left(t\right)\right),G\left(t\right)=G_0{\mathrm{e}}^{\left(-\lambda /\xi \right)}$$

(28)

where $G\left(t\right)$ denotes the gravitational coefficient changing with current iteration number $\lambda$. $G_0$ is the initial value of the gravitational coefficient and $\xi$ is the maximum iteration number. $M_i\left(t\right)$ and $M_j\left(t\right)$ represent the inertial mass of the ith particle and the jth particle respectively at the tth iteration. $x_i\left(t\right)$ and $x_j\left(t\right)$ are the positions of $M_i\left(t\right)$ and $M_j\left(t\right)$ in the space at the tth iteration, and they also represent the values of the perturbation parameters at the tth iteration. $R_{ij}\left(t\right)$ is the Euclidean distance between $M_i\left(t\right)$ and $M_j\left(t\right)$. Therefore, the total force on the ith particle in the three-dimensional space is

$$F_i\left(t\right)={\displaystyle \sum _{j=1,j\ne i}^N{F}_{ij}\left(t\right)}$$

(29)

Then update the particle velocity $v_i\left(t+1\right)$ and position $x_i\left(t+1\right)$ based on the total force on the ith particle according to Equation (30):

$$v_i\left(t+1\right)=v_i\left(t\right)+\frac{F_i\left(t\right)}{M_i\left(t\right)},x_i\left(t+1\right)=x_i\left(t\right)+v_i\left(t+1\right)$$

(30)

Finally, the new fitness value is updated and used as the condition. The optimal parameter $\epsilon$, $\mathit{\beta }$ and $\mathit{\gamma }$ are obtained if the current iteration number reaches the preset maximum iteration number, or the precision of the updated fitness satisfies the preset requirement that makes the mutual force between mass particles the largest. If the condition is not met, the algorithm moves to the next iteration.

Due to the wide bandwidth of sensor measurement noise, the resulting optimal parameter $\epsilon$ is 0.00001 after the search. A series of $\mathit{\gamma }$ and $\mathit{\beta }$ are obtained as well and the fitting formula of $\mathit{\gamma }$ and $\mathit{\beta }$ are set to be

$$\mathit{\gamma }=({\mathit{\omega }}_2-{\mathit{\omega }}_1)\times 0.001\mathrm{and}\mathit{\beta }=({\mathit{f}}_2-{\mathit{f}}_1)\times 0.001$$

(31)

The formulas are based on the driving experimental data of the fiber IMU with 400 Hz sample frequency. However, they can change for different vehicles working in different environments.

4. Simulation under Wideband Vibrations and Analysis

Simulations were performed to illustrate the advantages of the proposed sculling algorithm compared with the conventional four-interval sculling algorithm under the complex vibration environment. In the simulations, the vehicle was assumed to undergo the sculling motion that is considered the worst dynamic environment for SINS since the sculling error is maximized under this motion. In traditional algorithms, the simulation condition of pure sculling motion does not take into account random noises. However, the advantage of the proposed algorithm is that it can sufficiently correct both the sculling and pseudo sculling velocity errors usually caused by vibration noise. The simulation focuses on the sculling error compensation affected by vibrations of different frequencies. According to the analysis of experimental data given in Figure 2a, harmonic angular and linear vibrations with the same frequency are constructed as the simulation condition. The vibrations with noises are acting along the vehicle orthogonal axes:

$$\mathit{\omega }(t)=\left[\begin{array}{c}{A}_{\alpha }\mathsf{\Omega }\cos (\mathsf{\Omega }t)+A_{\alpha n}{\mathsf{\Omega }}_n\cos ({\mathsf{\Omega }}_{n}t)\\ 0\\ 0\end{array}\right],\mathit{f}(t)=\left[\begin{array}{c}0\\ A_p\sin (\mathsf{\Omega }t)+A_{pn}\sin ({\mathsf{\Omega }}_{n}t)\\ 0\end{array}\right]$$

(32)

where $A_{\alpha }$ and $A_p$ denote the amplitude of the angular and linear vibrations, respectively. $\mathsf{\Omega }$ is the angular and linear vibration frequency. $A_{\alpha n}$ and $A_{pn}$ represent the amplitude of noise existing in the angular and linear vibrations, respectively. ${\mathsf{\Omega }}_n$ is the noise frequency.

The angular and velocity increment over one sculling update interval can be determined by integrating Equation (32),

$$\begin{array}{l}\alpha (t)={\displaystyle {\int }_{t_k}^{t_{k+1}}\omega \mathrm{d}t}=\left[\begin{array}{c}{A}_{\alpha }\left(\sin \mathsf{\Omega }{t}_{k+1}-\sin \mathsf{\Omega }{t}_k\right)+A_{\alpha n}\left(\sin {\mathsf{\Omega }}_n{t}_{k+1}-\sin {\mathsf{\Omega }}_n{t}_k\right)\\ 0\\ 0\end{array}\right]\\ \mathit{v}(t)={\displaystyle {\int }_{t_k}^{t_{k+1}}f\mathrm{d}t}=\left[\begin{array}{c}0\\ \frac{A_p}{\mathsf{\Omega }}\left(\cos \mathsf{\Omega }{t}_k-\cos \mathsf{\Omega }{t}_{k+1}\right)+\frac{A_{pn}}{\mathsf{\Omega }}\left(\cos {\mathsf{\Omega }}_n{t}_k-\cos {\mathsf{\Omega }}_n{t}_{k+1}\right)\\ 0\end{array}\right]\end{array}$$

(33)

Substituting Equation (33) into the sculling term leads to the theoretic sculling compensation term ${\mathsf{\Delta }}_{scull}$. In simulation, the angular and linear vibration amplitudes of the vehicle sculling motion are assumed $A_{\alpha }={0.1}^{\circ }$ and $A_p=0.0001\mathrm{g}$, respectively. The angular and linear vibration frequency $\mathsf{\Omega }$ is 30 Hz. The noise frequency ${\mathsf{\Omega }}_n$ varies in a wide range from 0 Hz to 1000 Hz. The angular noise amplitude is $A_{\alpha n}={0.01}^{\circ }$ and the linear velocity noise amplitude is $A_{pn}=0.00001\mathrm{g}$. The parameters $\mathit{\gamma }$ and $\mathit{\beta }$ are determined according to Equation (31). The outputs of strapdown inertial sensors are the angular rate and specific force with the sample frequency 400 Hz. The total duration of simulation is 10 s. The results of the velocity error along the Z-axis in the navigation frame are illustrated in Figure 5 and Figure 6. Note that the sculling update frequency of the improved sculling algorithm is 400 Hz as well, while that of the conventional four-interval algorithm is 100 Hz when the sensor sample frequency is 400 Hz.

Figure 5 reveals that there is a pseudo sculling phenomenon caused by the overlap of frequency bands. It can be seen that the velocity error is actually amplified around 400 Hz and 800 Hz when the multiples of the sample frequency are mixed with the noise frequency. It is evident that the improved algorithm performs much better for the velocity update than the conventional four-interval approach under the sculling motion, because the improved algorithm can reduce the pseudo sculling error.

Figure 6 is presented to more clearly compare the performances of both algorithms at around the low noise frequency from 0 Hz to 250 Hz and the high noise frequency from 350 Hz to 450 Hz. It can be seen that the average velocity error of the improved sculling algorithm is 0.015 m/s at the low frequency band while the average velocity error of the conventional four-interval algorithm reaches 0.04 m/s. The maximum velocity error of the conventional four-interval algorithm in the vicinity of 400 Hz is 0.1795 m/s while that of the improved algorithm reduces to 0.0685 m/s under the same simulated condition. The improved sculling algorithm is more effective in attenuating the pseudo sculling error than the conventional algorithm as the frequency increases.

If the error drift of an algorithm is minimized in the sculling motion, it will perform satisfactorily in most other environments. In the process of sculling error compensation, the error drift is used as the criterion for accuracy evaluation, and its definition is given in Equation (34) [34],

$${\rho }_{drift}={\widehat{\mathsf{\Delta }}}_{scull}-{\mathsf{\Delta }}_{scull}$$

(34)

The results of error drift simulations under the sculling motion are shown in Figure 7.

Figure 7 illustrates that the error drifts of the conventional and improved algorithms change with the sculling motion noise frequency. The error drift of the improved algorithm based on the two-time scale perturbation model of inertial measurement is much smaller than the conventional one. Especially, when the noise frequency and sensor sample frequency fall into the overlap bands around 400 Hz and 800 Hz, the improved sculling algorithm can effectively attenuate the pseudo sculling error arising from the inertial sensor errors and vibration.

The average velocity error during the simulation duration is used to evaluate the accuracy performance of the two algorithms. The computational time is the total time that the algorithms spent on calculating the sculling error term in one velocity update interval. The performance statistic result is shown in

Table 1. Performance comparison.

Performance Index	Four-Interval Algorithm	Improved Algorithm
Accuracy (m/s)	−0.0399	−0.0152
Computational time (s)	0.01432	0.00414
Correction frequency (Hz)	100	400

.

It is evident that the improved algorithm is more accurate than the four-interval algorithm. Besides favorable accuracy, the improved algorithm shows its excellent work efficiency compared with the conventional algorithm at the same sample frequency. It can be seen that the computational time of the improved algorithm is much less than the conventional one. Furthermore, when the vehicle has very fast motion, especially a vibration-like oscillating motion, the sculling error can creep into velocity if the integration is not fast enough. Then it is necessary to design a fast integration algorithm to compensate the sculling error. If fast integration is desired to avoid the velocity sculling error, inertial data at a very high sample rate should be applied, yet it will impose a heavy burden on the processor. From Table 1, it can be seen that the update frequency of the new sculling compensation based on the two-time scale perturbation model is equal to the inertial sensor sample frequency 400 Hz, and the accuracy of the improved algorithm is higher. The update frequency of the conventional four-interval algorithm is 100 Hz because four sample data is needed to calculate the polynomial model coefficients in one sculling update interval. The statistical results indicate that the improved sculling correction algorithm based on the two-time scale perturbation model can effectively reduce the sculling error with less computation load.

5. Conclusions

In this paper, a novel velocity update algorithm for sculling error compensation was proposed for SINS based on the inertial sensor outputs of the angular rate and specific force. The new algorithm utilizes the inertial information’s two-time scale singular perturbation models instead of polynomial models to compute the increments of the velocity and angle to compensate the sculling error in the velocity update. Experimental results have shown that the new sculling compensation algorithm based on the singular perturbation can reduce the algorithm design complexity and achieve more accurate performance with less computation load compared with the traditional algorithm. In addition, the proposed algorithm can decrease both the sculling and pseudo sculling velocity errors that are usually caused by stochastic vibrations. Owing to these advantages, the improved algorithm is more suitable for SINS.