FOG Random Drift Signal Denoising Based on the Improved AR Model and Modified Sage-Husa Adaptive Kalman Filter

Abstract

In order to reduce the influence of fiber optic gyroscope (FOG) random drift error on inertial navigation systems, an improved auto regressive (AR) model is put forward in this paper. First, based on real-time observations at each restart of the gyroscope, the model of FOG random drift can be established online. In the improved AR model, the FOG measured signal is employed instead of the zero mean signals. Then, the modified Sage-Husa adaptive Kalman filter (SHAKF) is introduced, which can directly carry out real-time filtering on the FOG signals. Finally, static and dynamic experiments are done to verify the effectiveness. The filtering results are analyzed with Allan variance. The analysis results show that the improved AR model has high fitting accuracy and strong adaptability, and the minimum fitting accuracy of single noise is 93.2%. Based on the improved AR(3) model, the denoising method of SHAKF is more effective than traditional methods, and its effect is better than 30%. The random drift error of FOG is reduced effectively, and the precision of the FOG is improved.

1. Introduction

With the development of fiber optical technology, the fiber optic gyroscope (FOG) is gradually replacing other types of gyroscopes with its unique advantages, and it has become mainstream in inertial navigation system applications [1]. At present, much research work in modeling FOG random drift is being carried out at home and abroad. When the initial alignment of a strapdown inertial navigation system (SINS), the core of which is a FOG, is carried out, FOG random drift is an important factor that influences the alignment precision [2]. Through modeling and filtering, the effect of FOG random drift can be effectively restrained. The traditional modeling of FOG random drift is limited to an offline form, which uses a random drift model based on the output data of a single gyroscope obtained in the laboratory. Additionally, the estimated model is of certain significance in analyzing the characteristic of random drift due to changeable measurement conditions under the circumstances of the moment, such as temperature, humidity, electromagnetic field and gravity, as well as the impact caused by restarting the gyroscope; the reliability of a model established offline will decrease, and its universality is not strong [3,4]. To solve the above problems, we need to develop a real-time filtering method of online modeling of FOG random drift, based on real-time observations at each restart of the gyroscope, so an online model of FOG random drift can be established with a software method, and the model then is used to realize real-time filtering on the FOG.

In [1,2,3,4,5], auto regressive moving average (ARMA) models are used to establish the random drift error model of FOG and a ring laser gyroscope (RLG); the Kalman filter is adopted to filter the zero the drift of FOG and RLG, and the Allan variance analysis method is used to analyze various data noise sources before and after modeling and filtering. The ARMA(n, m) models require a stable signal, with a normal distribution and zero mean time series, which is also necessary for modeling gyroscope signals. Therefore, the modeling and filtering of gyroscope signals is performed only with post-processing. The estimated noise source error coefficient or performance parameters of the FOG are not suitable for inertial navigation systems used in practical applications.

In [6], an improved AR(2) model is proposed to establish the random drift error model of FOG online, and then, the Kalman filtering algorithm is adopted to filter the random drift error of FOG. Jin [7] proposed an online modeling and filtering method. In this method, the traditional offline model was improved based on a large number of measured data. In addition, a method of the random drift model of FOG based on the AR model is studied, then an $H_{\infty }$ filter is designed for filtering the signal online. An improved AR model of FOG random drift error and a forward linear prediction (FLP) filter were designed by Wang [8]. In [9], adaptive moving average (AMA) and random weighting estimation (RWE) based on double-factor adaptive KF algorithm, named AMA-RWE-DFAKF was proposed to denoise FOG drift signals under both static and dynamic conditions. Han [10] undertook research on the wavelet filtering method of FOG output signals. It is based on the combination of a Mallat pyramid algorithm and the characteristics of a finite impulse response (FIR) filter. Additionally, an equivalent FIR filtering algorithm was deduced based on wavelets. On the basis of the wavelet threshold filtering, a real-time wavelet filtering method for FOG output signals was given. In [11], FOG state estimation was combined with an autoregressive integrated moving average (ARIMA) model for non-linear parameter estimation. A Gaussian particle filter (GPF) was used to achieve ARIMA model identification and state estimation of a FOG.

In this paper, an improved AR(3) model is suggested, where the FOG random drift model is established online using measured FOG signal instead of a signal with zero mean. After modeling of real-time data at each restart of a gyroscope with the improved AR(3) model, direct filtering of the FOG signal is conducted using SHAKF. Filtering results are analyzed with Allan variance.

The rest of this paper is organized as follows: the online model of FOG random drift based on the improved AR model is introduced in Section 2. The modified SHAKF algorithm is described in Section 3. In Section 4, the practical implementation of the proposed method is introduced. The results are given for static and dynamic experiments to verify the feasibility of the improved AR model and modified SHAKF. Finally, conclusions are drawn in Section 5.

2. Online Modeling of FOG Random Drift

Time series analysis methods are commonly used to model the random drift error of FOG. ARMA modeling is a time series analysis method for analyzing observed random data. ARMA modeling involves fitting the suitable ARMA(n, m) model to the observed time series $\{x_t,t=1,2,\dots ,M\}$. The estimation process of ARMA model parameters is a nonlinear regression process, and its calculation is very complex. Enough high order AR models can be used to replace ARMA models to avoid complexity in the parameter estimation of the ARMA model. The AR model is a class of ARMA model, because its parameter estimation is a linear estimation, and the calculation is simple and fast. As such, the model has great advantages in engineering applications.

By adopting the method proposed in [1] for offline analysis on a certain type of FOG static data under different conditions, it is shown that the AR model can better fit FOG random drift, so in this paper, an improved AR model is used to realize the modeling of gyroscope random drift time series, estimate model parameters in real time and achieve online modeling.

Gyroscope output needs to meet the three conditions of being stable with a normal distribution and zero mean when the AR model is used to model gyroscope drift time series [2]. The analysis results of a certain type of FOG static output data under measurement conditions show that, gyroscope static data always satisfy the conditions of stationarity and normality; but, they do not meet the condition of zero mean, and the detected means are different after each restarting. The cause of this phenomenon is as follows: gyroscopes can measure the Earth’s rotation rate of a geographical location. Due to the impact of current surges, electromagnetic interference and other factors, the mean of the gyroscope will drift in a small range after each restarting. The value is steady (constant) after the stabilization of a gyroscope. The constant after stabilization contains the Earth’s rotation rate and (smaller) constant drift measured by the gyroscope. Therefore, the zero mean problem should be solved when adopting an AR model for online modeling. In continuation, an improved AR model is proposed as a solution of the described problem.

2.1. The Principle of Online Modeling

The framework of the AR(n) model is:

$$x_k={\mathsf{\phi }}_1{x}_{k-1}+{\mathsf{\phi }}_2{x}_{k-2}+\dots +{\mathsf{\phi }}_n{x}_{k-n}+a_k$$

(1)

where k is the sequence number, and its range is $[1,+\infty ]$; n is the order of AR, and its range is $n\ge 1$, and k, n are integers; $x_k$ is the observed time series; ${\mathsf{\phi }}_1~{\mathsf{\phi }}_n$ are parameters to be estimated; $a_k$ is white noise. On the assumption that the collected gyroscope data $z_1,z_2\dots z_k,z_{k+1}\dots$ are stationary and a normal sequence, the AR model can be established by the traditional method with a zero mean value. The observed time series can be written using the mean value of the sequence $\overline{z}$ as follows:

$$\{\begin{array}{c}{x}_k=z_k-\overline{z}\\ x_{k-1}=z_{k-1}-\overline{z}\\ x_{k-2}=z_{k-2}-\overline{z}\\ \cdots \\ x_{k-n}=z_{k-n}-\overline{z}\end{array}$$

(2)

By writing Equation (2) into Equation (1), the following expression is obtained:

$$z_k={\mathsf{\phi }}_1{z}_{k-1}+{\mathsf{\phi }}_2{z}_{k-2}+\dots +{\mathsf{\phi }}_n{z}_{k-n}+(1-{\mathsf{\phi }}_1-{\mathsf{\phi }}_2\dots -{\mathsf{\phi }}_n)\cdot \overline{z}+a_k$$

(3)

From the above theoretical analysis, we can know that the average value $\overline{z}$ of the FOG static output data should be constant after gyroscope stabilization after restarting. After the model is established, ${\mathsf{\phi }}_1~{\mathsf{\phi }}_n$ are also constant, so we denote $c=(1-{\mathsf{\phi }}_1-{\mathsf{\phi }}_2-\dots -{\mathsf{\phi }}_n)\cdot \overline{z}$. Equation (3) is now rewritten as:

$$z_k={\mathsf{\phi }}_1{z}_{k-1}+{\mathsf{\phi }}_2{z}_{k-2}+\dots +{\mathsf{\phi }}_n{z}_{k-n}+c+a_k$$

(4)

and describes the dynamic AR(n) model. The recursive least squares (RLS) method is used to estimate unknown parameters in real time.

2.2. Estimation of Model Parameters

The estimation methods of AR model parameters can be divided into two categories: direct estimation methods and recursive estimation methods. The former use observed data or the statistical properties directly to estimate the model parameters, among which the least squares complex exponential (LSCE) method is a most popular method. Due to how much data are required to estimate the parameters accurately being unknown, these kinds of methods select the length of the time window empirically. When the length is too small, this will make the parameters inaccurate; however, when an excessive length is applied, the time demanded makes these methods only be able to be used in offline modeling. The recursive estimation methods can estimate the model parameters in real time, and the RLS method is selected to estimate the parameters of the FOG random drift model in this paper.

Substituting the collected output $\{z_k,k=1,2,\dots ,N\}$ at the initial time into Equation (4), then the following linear equations can be obtained [12,13]:

$$\{\begin{array}{c}{z}_{n+1}={\mathsf{\phi }}_1{z}_n+{\mathsf{\phi }}_2{z}_{n-1}+\dots +{\mathsf{\phi }}_n{z}_1+c+a_{n+1}\\ z_{n+2}={\mathsf{\phi }}_1{z}_{n+1}+{\mathsf{\phi }}_2{z}_n+\dots +{\mathsf{\phi }}_n{z}_2+c+a_{n+2}\\ \cdots \\ z_N={\mathsf{\phi }}_1{z}_{N-1}+{\mathsf{\phi }}_2{z}_{N-2}+\dots +{\mathsf{\phi }}_n{z}_{N-n}+c+a_N\end{array}$$

(5)

where n is the order of AR.

The above equation can be written in the form:

$$Y_N=Z_N{\theta }_N+a_N$$

(6)

where $Y_N={[\begin{array}{cccc}{z}_{n+1}& z_{n+2}& \cdots & z_N\end{array}]}_{1\times (N-n)}^T$, ${\theta }_N={[\begin{array}{ccccc}{\mathsf{\phi }}_1& {\mathsf{\phi }}_2& \cdots & {\mathsf{\phi }}_n& c\end{array}]}_{1\times (n+1)}^T$, $a_N={[\begin{array}{cccc}{a}_{n+1}& a_{n+2}& \cdots & a_N\end{array}]}_{1\times (N-n)}^T$, $Z_N={\left[\begin{array}{ccccc}{z}_n& z_{n-1}& \cdots & z_1& 1\\ z_{n+1}& z_n& \cdots & z_2& 1\\ \cdots \\ z_{N-1}& z_{N-2}& \cdots & z_{N-n}& 1\end{array}\right]}_{(N-n)\times (n+1)}$.

According to the theory of multiple regression, the least square estimation of the parameter matrix θ_N is:

$${\theta }_N={(Z_N^T{Z}_N)}^{-1}{Z}_N^T{Y}_N$$

(7)

Assuming that the model parameter of the observed sequence at the present stage is ${\theta }_N$ and with the arrival of new data $z_{N+1}$, an updated estimation of ${\theta }_N$ can be obtained based on sequence $\{z_t,t=1,2,\dots ,N,N+1\}$. According to the form of the above matrix, the least square estimation based on N + 1 data is as follows:

$${\theta }_{N+1}=p_{N+1}{Z}_{N+1}^T{Y}_{N+1}, p_{N+1}={(Z_{N+1}^T{Z}_{N+1})}^{-1}$$

(8)

where h is sequence number, and its range is $[1,+\infty ]$; h is an integer. $Z_{N+1}={\left[\begin{array}{c}{Z}_N\\ Z_{(h+1)}\end{array}\right]}_{(N-n+1)\times (n+1)}$, $Y_{N+1}={\left[\begin{array}{c}{Y}_N\\ z_{N+1}\end{array}\right]}_{(N-n+1)\times 1}$, $Z_{(h+1)}={\left[\begin{array}{ccccc}{z}_N& z_{N-1}& \cdots & z_{N-n+1}& 1\end{array}\right]}_{1\times (n+1)}$.

According to the multiplication of the block matrix, the following expression can be obtained:

$$\{\begin{array}{c}{Z}_{N+1}^T{Y}_{N+1}=Z_N^T{Y}_N+Z_{(h+1)}^T{z}_{N+1}\\ p_{N+1}={(p_N^{-1}+Z_{(h+1)}^T{Z}_{(h+1)})}^{-1}\end{array}$$

(9)

By the matrix inversion formula, we can get the following:

$$p_{N+1}=(I+p_N\frac{Z_{(h+1)}^T{Z}_{(h+1)}}{1+Z_{(h+1)}{p}_N{Z}_{(h+1)}^T})p_N$$

(10)

where $I_{(n+1)\times (n+1)}$ is an identity matrix.

Substituting Equations (9) and (10) into Equation (8), the recursive estimation formula of the parameters is obtained:

$${\theta }_{N+1}={\theta }_N+K_{N+1}(z_{N+1}-Z_{(h+1)}{\theta }_N)$$

(11)

where $K_{N+1}=\frac{1}{1+Z_{(h+1)}{p}_N{Z}_{(h+1)}^T}{p}_N{Z}_{(h+1)}^T$.

The above formula shows that the new estimation θ_N+1 of the AR model parameters involves amending the original estimation θ_N, and the correction term is $K_{N+1}(z_{N+1}-Z_{(h+1)}{\theta }_N)$.

3. Real-Time Filtering

3.1. Selection of the Filter

After the AR model of FOG random drift is obtained, the traditional approach is to filter it through a Kalman filter [14,15,16,17,18,19]. The state space that sets the gyroscope as the research object is described as follows:

$$\{\begin{array}{c}{X}_t={\Phi }_{t,t-1}{X}_{t-1}+e_t\\ Z_t=H_t{X}_t+{\epsilon }_t\end{array}$$

(12)

where $X_t$ is the state vector at time t; ${\Phi }_{t,t-1}$ is the state transition matrix; $e_t$ is the system process noise sequence; $Z_t$ is the observation vector at time t, and here, it is the gyroscope output; $H_t$ is the observation matrix; ${\epsilon }_t$ is the measurement noise sequence; here, it is the fitting residual sequence of model. The state equation is the improved AR model of the gyroscope. Using a Kalman filter to filter the output signal of the gyroscope, the residual sequence ${\epsilon }_t$ is generally regarded as white noise, but this approach is not reasonable. An improved AR(3) model is established by using the static output data of a certain type of FOG. The residual error sequence fitted by the model is shown in Figure 1, and the corresponding power spectral density (PSD) is shown in Figure 2.

According to [20], the PSD of white noise should be constant, and the power spectrum is equal to the variance intensity. As can be seen from Figure 2, the PSD of the residual series ${\epsilon }_t$ is not constant, so it is not reasonable to simply treat the fitting residual part as white noise.

3.2. Sage-Husa Adaptive Kalman Filter

SHAKF, proposed by Sage and Husa [21], is an adaptive filtering algorithm for the uncertainty of noise statistical characteristics. A time varying noise estimator is added into the KF framework, which can estimate the statistical characteristics of noise in real time and mitigate the filter divergence.

3.2.1. Design of SHAKF

Consider the stochastic linear discrete system [22,23]:

$$\{\begin{array}{l}{\mathit{X}}_k={\mathit{\Phi }}_{k,k-1}{\mathit{X}}_{k-1}+{\mathit{W}}_{k-1}^{\prime }\\ {\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_k+{\mathit{V}}_k^{\prime }\end{array}$$

(13)

where $X_k$ is the state vector at epoch k; ${\Phi }_{k,k-1}$ is the state transition matrix; $W_{k-1}^{\prime }$ is the system process noise sequence; $Z_k$ is the observation vector at epoch k; $H_k$ is the observation matrix; $V_k^{\prime }$ is the measurement noise sequence; both are Gauss white noise, which are independent of each other with time varying mean and covariance matrix; they meet the following conditions:

$$\{\begin{array}{l}E\left[{\mathit{W}}_k^{\prime }\right]={\mathit{q}}_k\begin{array}{ccc}\begin{array}{}\end{array}& \begin{array}{}\end{array}& E\left[\left({\mathit{W}}_k^{\prime }-{\mathit{q}}_k\right)\left({\mathit{W}}_k^{\prime T}-{\mathit{q}}_j\right)\right]={\mathit{Q}}_k{\mathsf{\delta }}_{kj}\end{array}\\ E\left[{\mathit{V}}_k^{\prime }\right]={\mathit{r}}_k\begin{array}{ccc}\begin{array}{}\end{array}& \begin{array}{}\end{array}& \begin{array}{c}E\left[\left({\mathit{V}}_k^{\prime }-{\mathit{r}}_k\right)\left({\mathit{V}}_k^{\prime T}-{\mathit{r}}_k\right)\right]={\mathit{R}}_k{\mathsf{\delta }}_{kj}\end{array}\end{array}\\ E\left[\left({\mathit{W}}_k^{\prime }-{\mathit{q}}_k\right)\left({\mathit{V}}_k^{\prime T}-{\mathit{r}}_k\right)\right]=0\end{array}$$

(14)

where ${\mathsf{\delta }}_{kj}$ represents the Dirac delta function, ${\mathsf{\delta }}_{kj}=\{\begin{array}{cc}1,& ifk=l\\ 0,& otherwise\end{array}$; $Q_k$ is the process noise covariance matrix; and $R_k$ is the measurement noise covariance matrix.

Because the process noise and measurement noise have a non-zero mean value, the system cannot directly use the Kalman filter. Now, we will derive the SHAKF algorithm briefly [22].

Equation (13) is rewritten as:

$$\{\begin{array}{l}{\mathit{X}}_k={\mathit{\Phi }}_{k,k-1}{\mathit{X}}_{k-1}+{\mathit{q}}_{k-1}+{\mathit{W}}_{k-1}\\ {\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_k+{\mathit{r}}_k+{\mathit{V}}_k\end{array}$$

(15)

where ${\mathit{W}}_{k-1}$ and ${\mathit{V}}_k$ are both white noise with zero mean.

According to Equation (15), the following equation can be obtained:

$$\{\begin{array}{l}{\widehat{\mathit{X}}}_{k,k-1}={\mathit{\Phi }}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}+{\mathit{q}}_{k-1}\\ {\mathit{\nu }}_k={\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_{k,k-1}-{\mathit{r}}_k\\ {\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k,k-1}+{\mathit{K}}_k{\mathit{\nu }}_k\end{array}$$

(16)

where ${\widehat{X}}_{k,k-1}$ is the predicted state vector; ${\widehat{X}}_k$ is the state estimation vector; ${\nu }_k$ is the residual error series vector; $K_k$ is the filtering gain.

Subtracting Equations (15) and (16), then we can get:

$${\mathit{X}}_k-{\widehat{\mathit{X}}}_k={\mathit{\Phi }}_{k,k-1}\left({\mathit{X}}_{k-1}-{\widehat{\mathit{X}}}_{k-1}\right)-{\mathit{K}}_k{\mathit{\nu }}_k+{\mathit{W}}_{k-1}$$

(17)

$${\mathit{\nu }}_k={\mathit{H}}_k\left({\mathit{X}}_k-{\widehat{\mathit{X}}}_{k,k-1}\right)+{\mathit{V}}_k$$

(18)

Firstly, we will derive the estimation formula of the statistical characteristics of the measurement noise. Transposing both sides of Equation (18), the following can be obtained:

$${\mathit{\nu }}_k^T={\left({\mathit{X}}_k-{\widehat{\mathit{X}}}_{k,k-1}\right)}^T{\mathit{H}}_k^T+{\mathit{V}}_k^T$$

(19)

Furthermore, considering that the measurement noise and the estimation error are not related, we can get:

$$E\left[{\mathit{\nu }}_k{\mathit{\nu }}_k^T\right]=E\left[{\mathit{H}}_k\left({\mathit{X}}_k-{\widehat{\mathit{X}}}_{k,k-1}\right){\left({\mathit{X}}_k-{\widehat{\mathit{X}}}_{k,k-1}\right)}^T{\mathit{H}}_k^T\right]+E\left[{\mathit{V}}_k{\mathit{V}}_k^T\right]$$

(20)

The above formula can be simplified as:

$$E\left[{\mathit{\nu }}_k{\mathit{\nu }}_k^T\right]={\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k^T+{\mathit{R}}_{\mathrm{k}}$$

(21)

where $P_{k,k-1}$ is the covariance matrix of the predicted state vector.

The formula is written in the form of a recursive estimation formula as:

$${\widehat{\mathit{R}}}_k=(1-d_k){\widehat{\mathit{R}}}_{k-1}+d_k\left[{\mathit{\nu }}_k{\mathit{\nu }}_k^T-{\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k^T\right]$$

(22)

where $d_k$ is a correction factor with $d_k=\frac{1-b}{1-b^{k+1}}$ and b is the forgetting factor, $0<b<1$. Equation (22) is namely the recursive estimation formula of measurement noise ${\widehat{\mathit{R}}}_k$ of the SHAKF algorithm.

Then, we derive the estimation formula of the statistical properties of process noise. Transposing both sides of Equation (17), the following can be obtained:

$${\left({\mathit{X}}_k-{\widehat{\mathit{X}}}_k\right)}^T+{{\mathit{\nu }}_k}^T{{\mathit{K}}_k}^T={\left({\mathit{X}}_{k-1}-{\widehat{\mathit{X}}}_{k-1}\right)}^T{{\mathit{\Phi }}_{k,k-1}}^T+{{\mathit{W}}_{k-1}}^T$$

(23)

Both sides of Equation (17) are respectively multiplied by both sides of Equation (23), and then, the mathematical expectation is taken. Furthermore, considering that the process noise and the estimation error are not related, the estimated residual error is uncorrelated with the estimation error, and the process noise and the estimated residual error have zero expectation, so we can get:

$$\begin{array}{l}E\left[{\mathit{W}}_{k-1}{{\mathit{W}}_{k-1}}^T\right]+{\mathit{\Phi }}_{k,k-1}E\left[\left({\mathit{X}}_{k-1}-{\widehat{\mathit{X}}}_{k-1}\right){\left({\mathit{X}}_{k-1}-{\widehat{\mathit{X}}}_{k-1}\right)}^T\right]{{\mathit{\Phi }}_{k,k-1}}^T=\\ E\left[{\mathit{K}}_k{\mathit{\nu }}_k{{\mathit{\nu }}_k}^T{{\mathit{K}}_k}^T\right]+E\left[\left({\mathit{X}}_k-{\widehat{\mathit{X}}}_k\right){\left({\mathit{X}}_k-{\widehat{\mathit{X}}}_k\right)}^T\right]\end{array}$$

(24)

The above formula can be simplified as:

$${\widehat{\mathit{Q}}}_k={\mathit{K}}_k{\mathit{\nu }}_k{\mathit{\nu }}_k^T{\mathit{K}}_k^T+{\mathit{P}}_k-{\mathit{\Phi }}_{k,k-1}{\mathit{P}}_{k-1}{\mathit{\Phi }}_{k,k-1}^T$$

(25)

where $P_k$ is the predicted state covariance matrix at time k.

The formula is written in the form of a recursive estimation formula as:

$${\widehat{\mathit{Q}}}_k=(1-d_k){\widehat{\mathit{Q}}}_{k-1}+d_k\left({\mathit{K}}_k{\mathit{\nu }}_k{\mathit{\nu }}_k^T{\mathit{K}}_k^T+{\mathit{P}}_k-{\mathit{\Phi }}_{k,k-1}{\mathit{P}}_{k-1}{\mathit{\Phi }}_{k,k-1}^T\right)$$

(26)

Equation (26) is the recursive estimation formula of noise ${\widehat{\mathit{Q}}}_k$ of the SHAKF algorithm.

The flow chart of the SHAKF algorithm is shown in Figure 3.

As can be seen from Figure 3, the SHAKF algorithm consists of a noise estimator and a conventional Kalman filtering algorithm. The noise estimator can be used to estimate the real-time noise statistics, and the Kalman filtering algorithm can be adopted to complete the filtering state estimation.

3.2.2. Analysis of the SHAKF Algorithm

The SHAKF algorithm is analyzed as follows:

• The noise estimator cannot estimate the statistical properties of the process noise and measurement noise at the same time. Notice that the estimations of system noise and measurement noise both depend on the innovation, more specifically in the formula of ${\mathit{\nu }}_k{\mathit{\nu }}_k^T$. This is because ${\mathit{\nu }}_k{\mathit{\nu }}_k^T$ reflects the changes in the statistical characteristics of two kinds of noise at the same time, but in fact, ${\mathit{\nu }}_k{\mathit{\nu }}_k^T$ does not accurately reflect which kind of noise has changed. Assume that the measurement noise changes and process noise remain constant from a certain moment, then the covariance matrix of the measurement noise can be correctly estimated by means of Equation (22); and the estimated covariance matrix of the process noise obtained through Equation (26) is obviously inaccurate.
• Suppose the expected value of measurement noise is ${\widehat{\mathit{r}}}_k$, then the observation equation can be expressed as:

  $${\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_k+{\widehat{\mathit{r}}}_k+{\mathit{V}}_k$$

  (27)

  and ${\widehat{\mathit{r}}}_k$ can be obtained as follows:

  $${\widehat{\mathit{r}}}_k={\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_k-{\mathit{V}}_k$$

  (28)

It can be seen by comparison with Figure 3 that during the estimation of measurement noise expectation, the final estimated value ${\widehat{\mathit{X}}}_k$ is substituted by a one-step prediction ${\widehat{\mathit{X}}}_{k,k-1}$, so it is a kind of suboptimal algorithm.

Due to the fact the estimation of system process noise expectation and measurement noise expectation are suboptimal estimations, it may lead the expected estimation error to gradually accumulate and then produce large deviations during the recursive process and even interfere with the estimation of measurement noise or process noise variance.

3.

The recursive formula of measurement noise covariance matrix ${\mathit{R}}_k$ is rewritten as follows:

$$\begin{array}{cc}\hfill {\widehat{\mathit{R}}}_k=& \frac{b^k(1-b)}{1-b^{k+1}}{\widehat{\mathit{R}}}_0+\frac{b^{k-1}(1-b)}{1-b^{k+1}}({\mathit{\nu }}_1{\mathit{\nu }}_1^T-{\mathit{H}}_1{\mathit{P}}_{1,0}{\mathit{H}}_1^T)+\frac{b^{k-2}(1-b)}{1-b^{k+1}}({\mathit{\nu }}_2{\mathit{\nu }}_2^T-{\mathit{H}}_2{\mathit{P}}_{2,1}{\mathit{H}}_2^T)+\hfill \\ & \mathrm{...}+\frac{1-b}{1-b^{k+1}}({\mathit{\nu }}_k{\mathit{\nu }}_k^T-{\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k^T)\hfill \end{array}$$

(29)

Notice that, in the estimation of ${\mathit{R}}_k$, the weight of the correction at the current time is maximum. Additionally, the weight gradually tends to a constant value of $1-b$ with the increase of time. Similarly, with the increase of $k$, the distribution weight, for which the initial value ${\mathit{R}}_0$ distributes on ${\mathit{R}}_k$, gradually decays and tends to zero. This shows that the adaptive degree of the estimator decreases gradually with the progress of filtering.

4.

The recursive formula of ${\mathit{R}}_k$ can also be rewritten as follows:

$${\widehat{\mathit{R}}}_k={\widehat{\mathit{R}}}_{k-1}+\frac{1-b}{1-b^{k+1}}\left[{\mathit{\nu }}_k{\mathit{\nu }}_k^T-({\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k^T+{\widehat{\mathit{R}}}_{k-1})\right]$$

(30)

It is known that:

$$E[{\mathit{\nu }}_k{\mathit{\nu }}_k^T]={\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k^T+{\widehat{\mathit{R}}}_k$$

(31)

On the one hand, it can be seen that during the recursive estimation of measurement noise, the autocovariance of the current measurement value is used. Actually, in the calculation of the autocovariance, ${\widehat{\mathit{R}}}_k$ is substituted by ${\widehat{\mathit{R}}}_{k-1}$, which confirms that the measurement noise estimator is a suboptimal noise estimator. On the other hand, the recursive formula requires the filter to tend to become stable, with the result that the noise estimation after calculating the difference makes the new estimation of the ${\mathit{R}}_k$ matrix lose the positive definite value. This is not because the estimation error variance matrix $\mathit{P}$ is larger. In fact, the matrix $\mathit{P}$ is generally chosen to be larger at the initial time of filtering, and a larger deviation can appear during the filtering process due to carrier maneuvers or innovation appearing, which causes coarse errors. When the estimated ${\mathit{R}}_k$ is negative definite because the matrix $\mathit{P}$ is larger, it is likely to lead the filtering to diverge.

3.2.3. Improvement of the SHAKF Algorithm

Aiming at the weight problem of the filtering algorithm and the divergence problem of the filter, we can think about treating the SHAKF algorithm as follows [24,25]:

1.

A criterion of filtering convergence is introduced for the estimator, which is used to judge whether there is a large change in the measurement noise. The criterion is formulated as:

$${\mathit{\nu }}_k^T{\mathit{\nu }}_k\le {\gamma }^{2}Tr\left\{E\left[{\mathit{\nu }}_k{\mathit{\nu }}_k^T\right]\right\}$$

(32)

where $Tr(A)$ denotes the trace of the matrix $A$; $\gamma$ is used to control the strictness degree of criterion, and its range is $\gamma \ge 1$. The specific method of use is: update the above criterion with new innovation; if the criterion is established, carry out the filtering; if not, indicating that the measurement noise variance changes greatly, then calculate the weight value $d_k$ from the initial value, i.e., the value of k is zero.

In the estimation of ${\mathit{R}}_k$, the degree of the utilization of the4 current innovation is maximum at the beginning of filtering. With the increase of time, $d_k$ is decreased; then, ${\mathit{R}}_k$ depends on ${\mathit{R}}_{k-1}$ much more than innovation. The tracking ability of measurement noise is improved by the above method.

2.

The estimator of ${\mathit{R}}_k$ is rewritten in the following form:

$${\widehat{\mathit{R}}}_k=(1-d_k){\widehat{\mathit{R}}}_{k-1}+d_k{\mathit{\nu }}_k{\mathit{\nu }}_k^T$$

(33)

where the related items of the one-step prediction error covariance matrix are cast out on the basis of Equation (22). Although the change is at the expense of certain filtering accuracy, the stability of the filter is improved.

4. Experiment and Analysis of Adaptive Filtering Based on the AR Model

4.1. The Filtering Equation

The offline analysis of static data in different environments from a certain type of FOG was developed by Casic33s by using the improved AR model and looks for the optimal fitting model. The results show that the improved AR(3) model can fit the FOG random drift well, so the improved AR(3) model is used to model online. The gyroscope precision can be described as follows: the constant error is 0.01°/h; the random drift error is ${0.006}^{°}/\sqrt{h}$.

Under static conditions, we record the real-time data of the FOG single axis (x-axis). According to Equation (4), the improved AR(3) model can be established online. Then, we adopt RLS to estimate the model parameters in real time. The real-time estimation of parameter curves is shown in Figure 4 and Figure 5. Finally, we carry out adaptive filtering on the FOG signal directly. As a result of using the FOG measured signal instead of the zero mean signal, the improved AR model has a constant c. In this paper, c is also considered as a state variable. Therefore, the system state equation can be expressed as follows:

$$X_k=A{X}_{k-1}+B{W}_k$$

(34)

where state vector $X_k={[\begin{array}{cccc}{z}_k& z_{k-1}& z_{k-2}& c\end{array}]}_{1\times 4}^T$, process noise $W_k={[\begin{array}{cccc}{a}_k& 0& 0& 0\end{array}]}_{1\times 4}^T$, $A={\left[\begin{array}{cccc}{\phi }_1& {\phi }_2& {\phi }_3& 1\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]}_{4\times 4}$, $B={\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]}_{4\times 4}$.

Suppose the FOG output is $Z_k$, then the measurement equation of the system is as follows:

$$Z_k=H{X}_k+V_k$$

(35)

where $H={[\begin{array}{cccc}1& 0& 0& 0\end{array}]}_{1\times 4}$, $V_k$ is measurement noise.

As can be seen from the figures, in the absence of disturbance, the estimated parameters tend to become stable in about 30 s, and the estimated values are as follows:

$${\mathsf{\phi }}_1=0.4885, {\mathsf{\phi }}_2=-0.6660, {\mathsf{\phi }}_3=-0.1705, \mathrm{c}=0.0048$$

(36)

Therefore, the improved AR(3) model is:

$$z_k=0.4885z_{k-1}-0.6660z_{k-2}-0.1705z_{k-3}+0.0048+a_k$$

(37)

Then, the filtering equation is:

$$\{\begin{array}{l}\left[\begin{array}{c}{z}_k\\ z_{k-1}\\ z_{k-2}\\ c\end{array}\right]=\left[\begin{array}{cccc}0.4885& -0.6660& -0.1705& 1\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{z}_{k-1}\\ z_{k-2}\\ z_{k-3}\\ c\end{array}\right]+\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{a}_k\\ 0\\ 0\\ 0\end{array}\right]\\ z_k=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{z}_{k-1}\\ z_{k-2}\\ z_{k-3}\\ c\end{array}\right]+V_k\end{array}$$

(38)

According to the above filtering equation, the random drift error of FOG can be filtered using SHAKF.

4.2. Static Experiment Results and Analysis

Under static conditions, we record the real-time data of a single axis (x-axis) FOG for 2 h. The sampling period is 5 ms, and the experimental picture is shown in Figure 6.

The original output of gyroscope (x-axis) is shown in Figure 7, and the results with KF and SHAKF processing are shown in Figure 8 and Figure 9.

As can be seen from the figures, after filtering by the SHAKF, the noise is significantly reduced, while the average value remains unchanged. The filtering effect of SHAKF is better than that of KF. According to the filtering results, on the one hand, it can be seen that using the improved AR model of FOG to model can effectively reduce the gyroscope random drift error; the validity of the denoising method is verified. On the other hand, the effect of using the KF is not as good as the SHAKF, which indicates that when dealing with the filtering problem of an uncertain model, the adaptive filtering algorithm has better robustness.

Allan Variance Analysis

Now, the sampling data, the data of the random drift model and the data after filtering by the KF and SHAKF are compared by adopting the Allan variance analysis method. The Allan variance method is a time domain analysis method, which is recognized as a standard method for the analysis of FOG parameters by IEEE [26]. It can characterize and identify all kinds of error sources and their contribution to the statistical properties of the whole noise very easily and meticulously. Therefore, the Allan variance analysis can be used to analyze the variation of error coefficients before and after filtering FOG signals quantitatively. If the noise sources are independent of each other, then the Allan variance is the sum of squares of each type of error. The Allan variance of FOG can be expressed as [27,28,29,30,31]:

$${\sigma }_{total}^2(\tau )={\sigma }_{ARW}^2(\tau )+{\sigma }_{BI}^2(\tau )+{\sigma }_{RRW}^2(\tau )+{\sigma }_{RR}^2(\tau )+{\sigma }_{QN}^2(\tau )$$

(39)

Namely,

$${\sigma }_{\Omega }^2(\tau )=\frac{R^2}{2}{\tau }^2+\frac{K^2}{3}\tau +\frac{2B^2\ln 2}{\pi }+N^2{\tau }^{-1}+3Q^2{\tau }^{-2}$$

(40)

where ARW is the angle random walk, and its error coefficient is N; BI is the bias instability, and its error coefficient is B; RRW is the rate random walk, and its error coefficient is K; RR is the rate ramp, and its error coefficient is R; QN is the quantization noise, and its error coefficient is Q.

According to Equation (35), if we use the least square method to fit the data, we can get the error coefficients of five noise sources before and after filtering the FOG signal. They are shown in

Table 1. Comparison of the noise source error coefficients before and after filtering.

Error Coefficients (Unit)	Original Signal	Fitting Sequence	Kalman Filtering	SHAKF
$N(°/h^{1/2})$	4.2026 × 10−6	3.9168 × 10−6	3.2547 × 10−6	2.0632 × 10−6
$B(°/h)$	7.1221 × 10−4	7.0438 × 10−4	6.1221 × 10−4	3.5235 × 10−4
$K(°/h^{3/2})$	0.0335	0.0334	0.0246	0.0162
$R(°/h^2)$	0.5401	0.5390	0.4286	0.2636
$Q(°)$	1.1565 × 10−5	1.1195 × 10−5	9.4795 × 10−6	5.4532 × 10−6

.

It can be seen from Table 1 that the sampling data are basically consistent with the noise characteristics of the noise sequence fitted by the improved AR model. The fitting accuracy of the rate ramp and rate random walk error are both more than 99%. Due to the fact that the magnitudes of the angle random walk, bias instability and quantization noise are small, fitting is more difficult, but the fitting accuracy reaches above 93%. After the data are processed by two filtering methods, the five noise source error coefficients of the FOG output signal are obviously reduced. After adopting the modified SHAKF, the noise source error coefficients are all less than half of their values before filtering; for example, the FOG bias stability reduces from the original 0.0007122 (°/h) to 0.0003523 (°/h). When analyzing the filtering effect of a single noise, the modified SHAKF can improve the performance by up to 42.5% (quantization noise) compared to KF, and the filtering effects of the random walk and rate ramp error are increased by 34.1% and 38.5%, respectively. It can be seen that the effect of the modified SHAKF is better than KF. Therefore, the random drift error of FOG is effectively reduced, and the accuracy of FOG is improved.

4.3. Dynamic Experiment Results and Analysis

In order to verify the applicability of the modified model under moving base conditions, the gyroscope axis (x-axis) is rotated by 5°/s, 15°/s, 25°/s, 35°/s and 50°/s, respectively. In this paper, the dynamic data of FOG (x-axis) is recorded for 2 h at room temperature with a sampling period of 5 ms. The improved AR(3) model is established as the system state equation using 1,440,000 differentiated samples. Then, the SHAKF algorithm is applied to denoise the dynamic signal of the FOG with the same initial parameters chosen as under static conditions. The results are shown in the following figures, where the blue curve represents the FOG noisy signal, the red curve indicates the SHAKF denoising signal and the green curve denotes the KF denoising signal.

It can be seen from Figure 10, Figure 11, Figure 12, Figure 13 and Figure 14 that the denoising effect of the KF becomes worse and worse with the increasing of rotational velocity, even becoming invalid. Relative to the KF, the denoising effect of the modified SHAKF is very obvious, and any increase in rotational velocity has little influence on SHAKF.

Mean square error (MSE), root mean square error (RMSE) or the signal-to-noise power ratio (SNR) [9] are generally employed to compare the performance of denoising methods before and after denoising the FOG dynamic drift signal. The MSE is defined as follows:

$$MSE=\sqrt{\frac{1}{N}{{\displaystyle \sum _{\mathrm{t}=1}^N(x(t)-\overline{x}(t))}}^2}$$

(41)

where $\overline{x}(t)$ is the mean value of the signal, $x(t)$ is the actual signal and N is the number of signals. The MSE results calculated before and after denoising are in

Table 2. MSE results of the FOG signal with different rotational velocities.

Rotation (°/s)	FOG Signal (°/s)	KF Denoised Signal (°/s)	SHAKF Denoised Signal (°/s)
5	0.1068	0.0362	0.0184
15	0.2461	0.0932	0.0396
25	0.1629	0.1086	0.0280
35	0.0844	0.0637	0.0145
50	0.0538	0.0445	0.0093

.

Through comparing the statistical characteristics of the FOG output signal before and after denoising, it can be seen that the MSE of random drift error before and after filtering clearly changes. Additionally, the MSE of SHAKF denoised signal is much less than that of KF. Therefore, it can be concluded through the above analysis that the modified SHAKF is a suitable denoising algorithm for reducing the FOG random drift error.

5. Conclusions

In this paper, the improved AR(3) model is proposed. It is shown that a model of the FOG output signal can be established online by directly using the output static data of a FOG. Additionally, according to the model, the modified SHAKF is adopted to filter the FOG random drift errors in real time, which effectively reduces FOG errors and improves the accuracy of FOG. Static experiments and dynamic experiments were done to verify the effectiveness of this method. Under static conditions, the filtering performance of the modified SHAKF is compared to KF, and it is proven to be superior to KF. Based on Allan variance analysis, the random errors, like the angle random walk and bias instability, are reduced two-fold. The fitting precision of the FOG random drift model established online by the improved AR model is higher. The real-time performance is strong, and the minimum fitting accuracy of single noise is 93.2%. Under dynamic conditions, the minimum MSE obtained by the modified SHAKF shows that the improved AR model established under static conditions is also perfect. The effectiveness of this method is validated in denoising the single-axis FOG signal under both static and dynamic conditions. The research presented in this paper is of great significance in engineering applications.