AMA- and RWE- Based Adaptive Kalman Filter for Denoising Fiber Optic Gyroscope Drift Signal

An improved double-factor adaptive Kalman filter called AMA-RWE-DFAKF is proposed to denoise fiber optic gyroscope (FOG) drift signal in both static and dynamic conditions. The first factor is Kalman gain updated by random weighting estimation (RWE) of the covariance matrix of innovation sequence at any time to ensure the lowest noise level of output, but the inertia of KF response increases in dynamic condition. To decrease the inertia, the second factor is the covariance matrix of predicted state vector adjusted by RWE only when discontinuities are detected by adaptive moving average (AMA).The AMA-RWE-DFAKF is applied for denoising FOG static and dynamic signals, its performance is compared with conventional KF (CKF), RWE-based adaptive KF with gain correction (RWE-AKFG), AMA- and RWE- based dual mode adaptive KF (AMA-RWE-DMAKF). Results of Allan variance on static signal and root mean square error (RMSE) on dynamic signal show that this proposed algorithm outperforms all the considered methods in denoising FOG signal.


Introduction
As a kind of inertial sensor based on optical Sagnac effect, fiber optic gyroscope (FOG) has been widely used in inertial navigation system (INS) because of its significant advantages such as small size, low cost, no moving parts, long lifespan, and large dynamic range [1,2]. However, the accuracy of the FOG sensor is limited by drift errors due to internal device operations and external environment disturbances [3]. The FOG drift signal has two types of errors namely, deterministic errors and stochastic errors. Deterministic errors are bias, scale factor, and misalignment, which are relatively easier to be compensated by suitable calibration methods in laboratory environment. Stochastic errors, which are induced from the environmental temperature changes, electronic noises, and other electronic equipment interfaced with it [4,5], are difficult to be directly eliminated. As an alternative, stochastic models and denoising methods are two main techniques to restrain the FOG random errors reported in the literature.
Signal processing methods like low pass filter [6], wavelet transforms [7,8], and empirical mode decomposition (EMD) [9][10][11] are used to remove random errors for improving the performance of FOG measurement. These methods are successfully applied for denoising FOG static signal but fail to denoise FOG signal in highly dynamic conditions due to delay or complexity. Recently, time series analysis as a powerful tool is used to model the FOG random drift signal. Autoregressive (AR), moving average (MA), and autoregressive moving average (ARMA) have been developed in modeling stochastic models for FOG random errors in [12][13][14]. Combining these stochastic models, a conventional Kalman filter (CKF) is usually employed to remove the FOG random drift [13][14][15][16], where the process and measurement noises are pre-calculated by sampling lots of drift data. However, fixed noise variances are unsuitable in real applications which may lead to divergent problems. To improve the practicability and to avoid divergent effects, adaptive KF (AKF) methods have been investigated which are based on innovation-based adaptive estimation (IAE) or residual-based adaptive estimation (RAE) [17][18][19]. An AKF with double transitive factors is developed in [17,18], where the covariance matrix of predicted state vector is modified by an adaptive factor in stage one and the covariance matrix of measurement noise is modified by another adaptive factor in stage two. However, this method requires that innovation or residual vectors at each time point be in the identical type, spatial dimension and distribution, which is difficult to satisfy in a highly dynamic environment.
Random weighting estimation (RWE) is an advanced computational method in statistics. It does not require the prior knowledge of the distribution of position parameters, and the obtained estimation is unbiased. RWE has been established for estimation of the covariance matrix of observation vector and predicted state vector in [20][21][22]. In [23], Kalman gain based on RWE is updated using the covariance matrix of innovation sequence. This method called RWE-AKFG is applied to denoise the FOG signal under static and dynamic environments. However, the inertia of its response increases in denoising the FOG dynamic signal, which is not tolerable for real time dynamic applications. To solve the contradiction between the noise level of the output signal and the inertia of the KF response, adaptive moving average (AMA) is used to detect the discontinuities in signal [24]. AMA based dual mode KF (AMADMKF) in [25,26] is proposed to filter the FOG static and dynamic signals, where DMKF means the proper KF gain parameter or the ratio Q/R is switched at different conditions. However, it is difficult to predefine the proper ratio Q/R to adapt the FOG rotation rate changes [27]. Based on the above research, AMA-DWT-DMKF is used to denoise the FOG static signal, disturbance signal, and the change rate signal successfully in [28], but it is much more complex and has a higher computation cost. Considering the nonlinearity in system and measurement models, Narasimhappa et al. in [29][30][31] provide various adaptive filters based on unscented Kalman filter such as adaptive unscented Kalman filter (AUKF), adaptive square root unscented Kalman filter (ASRUKF) and adaptive sampling strong tracking scaled unscented Kalman filter (ASST-SUKF) for denoising the FOG signal. The performance of these algorithms is verified in static and dynamic conditions, whereas UKF is more complex than KF. Hence, the purpose of this paper is to develop an AMA-and RWE-based double-factor adaptive KF algorithm, named AMA-RWE-DFAKF, which can denoise the FOG static and dynamic signal. The first adaptive factor is Kalman gain, which is updated by using RWE of the covariance matrix of innovation sequence in any condition. The second adaptive factor is the covariance matrix of predicted state vector, which is revised based on RWE only when the discontinuities are detected by AMA. In fact, it has the same adaptive filter mechanism as [17], except that different estimations are used. Experimental results show that the proposed algorithm can satisfy the lowest noise level and the lowest inertia in denoising FOG static and dynamic signals.
The outline of this paper is as follows: Section 1 is the introduction; the concept of adaptive moving average is reviewed briefly in Section 2; Section 3 gives a description about the principle of random weighting estimation; the proposed AMA-RWE-DFAKF is provided in Section 4; experimental results and discussions about the proposed method applied in FOG static and dynamic signals are presented in Section 5, and Section 6 is the conclusion.

Concept of Adaptive Moving Average
The adaptive moving average (AMA) can be used to detect discontinuities in the signal by comparing the sample variance with a threshold value, where the length of moving average is adaptive to follow the rate of change in signal [24][25][26]28]. A q-point moving average filter can be expressed as where x(t) denotes the input data, 2q + 1 is the moving average window size, N is the number of samples as one frame of the input data, and y(t) is the filtered data. The noise can be further reduced by applying an iterative adaptive moving average filter shown in Figure 1. The algorithmic steps of this filter are explained as Step 1: Calculate the absolute value of the differenced y(t) Step 2: Calculate the rate of change of D(t) Update q l (t) and q h (t) Step 4: Repeat the iteration on the filtered data Y(t) from step 1 to 3 until the maximum iterations is met.
The final filtered data Y(t) is obtained by n iteration in our work, i.e., n = 3. And the transition locations are detected by comparing the sample variance of Y(t) with a threshold λ. The sample variance is calculated in the window size as follows The threshold λ is 95% upper tail of exponential distribution, where the expected value of this distribution is the mean value of the above calculated sample variances in the current frame. The transition location τt is defined as

Principle of Random Weighting Estimation
Suppose that X1, X2, … , Xn are the independent and identically distributed random variables with common distribution function F(x). Let x1, x2, … , xn be the sample realizations. The corresponding empirical distribution function Fn(x) can be expressed as The random weighting estimation [21][22][23] of Fn(x) is defined as where I(Xi ≤ x) is the indicator function represented as and random vector [V1, V2, … , Vn] subjects to Dirichlet distribution D(1, 1, … , 1), that is, A joint density function of the random vector is defined as where [V1, V2, … , Vn] ∊ Dn, and Dn-1={[V1, V2, … , Vn-1]: Vi ≥ 0 (I = 1, 2, … ,n−1), Calculate the mean of Xi as follows Then, the random weighting estimation of ˆX E is where Xk is the state vector at epoch k, Zk is measurement vector, Φk,k-1 is the state transition matrix, Γk-1 is the system noise driving matrix, and Hk denotes the measurement matrix. Wk is the state noise with covariance matrix k W ∑ , Vk is the measurement noise with covariance matrix k V ∑ . Wk and Vk are assumed to be discrete white Gaussian noise with zero mean, known distributions and uncorrelated to each other, satisfying where Qk is process noise covariance matrix, Rk is measurement noise variance matrix, and δkj is the Kronecker δ function. The CKF algorithm is described by the following equations

Random Weighting Estimation for Kalman Gain
The predicted state vector is Accordingly, the innovation vector may be written as The variance of The By substituting Equation (31) into (24) and considering (30), the modified gain Kk can be described as

Random Weighting Estimation for Covariance Matrix of Predicted State Vector
In KF prediction stage, the covariance matrix of predicted state vector is That is By Equation (34), the covariance matrix of Wk-1 can be written as Take the average of Considering Equation (35), the random weighting estimation of Take the average of The random weighting estimation of Substituting Equation (38) into (41), we have The error question of predicted state vector can be defined as The random weighting estimation of The random weighting estimation of ˆk KF parameters Qk, Rk, and Pk impact not only on the noise level of output signal, but also on the inertia of KF response [27]. The inertia increases with the value of Rk while the noise level decreases. Conversely, the inertia decreases with the increase of value of Qk while the noise level increases. In the dynamic case, these fixed values are critical in KF denoising scheme due to the noise characteristic of the FOG is more complex and time varying. To adjust KF parameters for the real-time applications, a double-factor adaptive KF combined AMA and RWE as shown in Figure 2, called AMA-RWE-DFAKF, is proposed for denoising the FOG signal. The Kalman gain is updated as Equation (32) in any region and the covariance matrix of predicted state vector is modified as Equation (46) only in transition region, which in fact has the same adaptive filter mechanism as [17], except that different methods are used to estimate these adaptive parameters.

Experimental Results and Discussions
In this experiment, we test the filtering on real FOG signal under both static and dynamic environments. The experimental setup is shown in Figure 3. The setup has a single-axis FOG, three-axis turntable, power supply for FOG, and data processing computer. In the static condition, FOG is in zero rotation at room temperature, whereas in the dynamic condition, FOG is mounted on the three-axis turntable with different rotation rates.

Static Test Analysis
Under room temperature, the FOG static data is recorded for 3 h with a sampling frequency of 100 Hz as shown in Figure 4. In KF, an AR (2) model is established as the system state equation using the first 6000 samples. Here, initial state and error covariance matrix of the state are assumed to 0 [0, 0] T X  and P0 = diag( [1,1]). H = [1, 0] is the observation matrix. And the measurement (R) and process (Q) noise covariance matrix are initialized to 0.01 and 0.0001, respectively. All these considered algorithms have the same parameters. Details on RWE-AKFG and AMA-DMKF are fully available in [23][24][25][26]. In this paper, Q/R is chosen as 0.01 (k1) and 0.1 (k2) for non-discontinuity region and discontinuity region, respectively. As discussed in [27], the value of R is fixed. AMA-RWE-DMAKF is that Kalman gain is updated by the covariance matrix of innovation sequence using random weighting method, which is a combination of RWE-AKFG and AMA-DMKF. For AMA-RWE-DFAKF and AMA-RWE-DMAKF algorithms, we considered 4096 samples as one frame. However, for RWE-AKFG and CKF we denoised the signal sample by sample. There is no discontinuity location detected by AMA because of collecting FOG signal in motionless environment.

Prediction Stage
Random weighting estimation of P k,k-1

N=25
Output the filtered signal   Allan variance is a popular technique to quantify and identify the random noise like quantization noise (Q), angle random walk (N), bias instability (B), rate random walk (K) and rate ramp (R) before and after denoising the FOG static drift signal [11,26]. In Allan variance curve, different slope corresponds to different random noise. To compare the denoising performance, Allan variance curves are plotted in Figure 5. It can be seen that the curves after filtering by these four methods decline in some degree and the curve from the proposed method is the lowest. In Figure 5, slopes of −1/2 and 0 indicate the present of angle random walk and bias instability in this FOG signal. The random error values are tabulated in Table 1. It is seen that the angle random walk and bias instability are reduced by 100 times as compared to the original value. Moreover, AMA-RWE-DFAKF, AMA-RWE-DMAKF, and RWE-AKFG algorithms give a competitive performance in denoising FOG static signal, and these results have a clear advantage as compared with CKF.

Dynamic Test Analysis
For acquiring the dynamic FOG data, the three-axis turntable is used to generate series of reference angular rate in the dynamic ranges, i.e., ±10, ±20, and ±50°/s. In this study, the signal data is recorded for 1 h at room temperature with sampling frequency of 100 Hz shown in Figure 6 through a clockwise and counter-clockwise turntable rotation. The rotation rate is decreased in a stepwise manner starting from +50°/s and varying between 0°/s and 50°/s. An AR (2) model is established as the system state equation using the first 6000 differentiated samples. Here, the initial state and error covariance matrix of the state are assumed to 0 X = [0, 0, 0] T and P0 = diag ([1, 1, 1]), respectively. The observation matrix becomes H = [1, 0, 0]. The initial values of R and Q are fixed to 0.01 and 0.0001 as mentioned above. The ratio between Q and R is switched from two states 0.01 and 0.1 for AMA-RWE-DMAKF.
As already mentioned, the signal is divided into frames with N' = 4096 samples. For each frame, AMA is used to detect discontinuity locations shown in Figure 6. From these results, the effectiveness of AMA is proved. These four algorithms are applied to denoise FOG dynamic signal with the same initial parameters chosen as in the static condition, i.e., the measurement and process noise covariance matrix. Although RWE-AKFG and AMA-RWE-DMAKF give quite competitive results with AMA-RWE-DFAKF in static condition, but these fail to denoise signal in dynamic condition.
The denoising results for all samples are shown in Figure 7. To obtain a clear visuality, we have plotted only a portion of the signal in Figure 8 where we can compare the inertia of these four algorithms in the different transition. From Figure 8, we can see there exists some delay for every filtering in following trend of the signal. It is not acceptable for RWE-AKF due to the increase of the value of R while Q is fixed. Thus AMA-RWE-DMAKF with two Kalman gain k1 and k2 is developed to decrease the inertial, but it is difficult to pre-design these parameters. However, the proposed AMA-RWE-DFAKF algorithm can satisfy the lowest noise level and the lowest inertial by only adjusting the two parameters adaptively as described in Figure 2. Figures 7 and 8 indicate that the AMA-RWE-DFAKF denosies the FOG dynamic signal better than all other algorithms.   are plotted in Figure 11. The novel AMA-RWE-DFAKF has the minimum lag at transition times, which can satisfy both conditions of the lowest noise level and the lowest inertia.  Mean square error (MSE), root mean square error (RMSE), or signal-to-noise power ratio (SNR) [11,31] are generally employed to compare the performance of different denoising methods before and after denoising the FOG dynamic drift signal. The RMSE is defined as follows where ˆ() xt is the denoised signal, x(t) is the actual signal and N is the number of signal.
The RMSE results are calculated before and after denoising in Tables 2 and 3. It is observed that the AMA-RWE-DFAKF has the minimum RMSE compared with other algorithms. Thus the effectiveness of this improved algorithm is verified in denoising FOG signal under both static and dynamic conditions.  Take the rotation rate from −50°/s to +50°/s as an example, the window size of RWE in AMA-RWE-DFAKF is discussed shown in Figure 12, because it can affect the convergence speed and filtering accuracy. The window of size is set to 5, 25, and 55, denoting small, middle, and long window. It can be seen that the smaller window size (N = 5) gives the faster convergence speed shown in Figure 12a, but has the poor filtering accuracy shown in Figure 12b. While, the longer window size (N = 55) give the higher filtering accuracy, but has the poor convergence speed. To get a relatively higher accuracy and faster convergence speed, the window size (N = 25) is chosen. The filter's convergence speed shown in Figure 13 is about 0.2 s for AMA-RWE-DFAKF, 0.4 s for AMA-RWE-DMAKF, 1.2 s for CKF, and 30 s for RWE-AKF. Moreover, the proposed filter's convergence speed is also faster compared with that of CKF, RWE-AKF, and AMA-RWE-DMAKF when the rotation rate is varying in other different values such as ±100°/s, ±150°/s and ±200°/s.

Conclusions
In this paper, an AMA-RWE-DFAKF algorithm is proposed to denoise FOG static and dynamic signals. AMA is used to detect the discontinuities and RWE is used to estimate the double-factor in KF. The first adaptive parameter is Kalman gain updated by using RWE of the covariance matrix of innovation sequence. The second adaptive parameter is the covariance matrix of predicted state vector introduced to decrease the inertia at the discontinuities. In static condition, the performance of AMA-RWE-DFAKF is competitive with RWE-AKFG and AMA-RWE-DMAKF, but superior to CKF. Based on Allan variance analysis, the random errors like angle random walk and bias instability are reduced by 100 times. In dynamic condition, the minimum RMSE obtained by AMA-RWE-DFAKF show that it performs better than all considered algorithms. The effectiveness of this method is validated in denoising the single-axis FOG signal in both static and dynamic conditions. The future work is to verify this filtering in three-axis FOG and implement it in hardware.