A Robust Extended Kalman Filter Algorithm Based on a Sliding Window Fractional-Order Grey Prediction Model and Its Application in MINS/GNSS

TL;DR

This paper introduces a new robust filtering algorithm to improve navigation accuracy when GPS signals are slightly faulty.

Contribution

A novel robust extended Kalman filter algorithm using a sliding window fractional-order grey prediction model is proposed.

Findings

The proposed algorithm improves velocity and position accuracy by over 50% and 80% during GPS faults.

Simulation and vehicle experiments confirm the algorithm's effectiveness in handling small amplitude faults.

Abstract

To address the issue of reduced accuracy or even divergence in micro-electro-mechanical inertial navigation systems’/global navigation satellite systems’ (MINSs’/GNSSs’) integrated navigation systems caused by small amplitude fault in GNSS measurement information, this paper proposes a robust extended Kalman filter algorithm based on a sliding window fractional-order grey prediction model (SWFGM(1,1)-REKF). When GNSS signals are disrupted, this algorithm first detects system faults through a weighted index sequential probability ratio test (SPRT) detection. Then, it uses GNSS measurements predicted by a sliding window fractional-order grey prediction model (FGM(1,1)) to replace the faulty GNSS data and integrates them with MINSs. Finally, it combines robust estimation to construct a robust extended Kalman filter to correct the integrated information. Simulation and vehicle experiment results show the advancement of SWFGM(1,1)-REKF. When GNSS measurements experience small amplitude abrupt faults, compared with traditional robust extended Kalman filter algorithm based on a chi-square test, the proposed algorithm improves filtering accuracy of velocity and position. In the vehicle small amplitude mutation fault experiment, the velocity and position accuracy are increased by more than 50% and 80% respectively.

Figures

Full text

1. Introduction

The inertial measurement unit (IMU) based on a micro-electro-mechanical system (MEMS) has developed rapidly due to its small size and low power consumption. Micro-electro-mechanical inertial navigation systems/global navigation satellite systems (MINSs/GNSSs) have been widely applied in many fields such as mobile measurement, intelligent transportation, and vehicle navigation [1,2]. However, when GNSS is used in complex environments such as cities, canyons, and areas with electromagnetic radiation, the signal is disturbed and prone to failure [3,4]. This causes the accuracy of MINS/GNSS to decrease rapidly. Therefore, processing GNSS measurement fault information and enhancing the fault tolerance of the system are particularly important for improving the accuracy of MINS/GNSS [5,6].

The key to improving the fault tolerance of the system is to detect the system fault quickly and deal with it effectively. Currently, there are many methods for processing after GNSS fault detection, but there are few methods for processing during GNSS faults. The commonly fault-tolerant methods include INS independent solution by isolating GNSS fault observations, a filtering algorithm based on interacting multiple models and a robust adaptive filtering method based on a hypothesis test [7,8,9]. However, for low-precision MINS/GNSS, the long-term independent solution of INS will cause significant error accumulation, resulting in a rapid decline or even divergence of navigation accuracy. In the filtering algorithm based on interacting multiple models, since the system model is not completely known, the constructed model does not necessarily contain the current system model, which limits the accuracy improvement of MINS/GNSS [10,11]. A robust adaptive filtering method based on a hypothesis test reduces the weight of fault observations by calculating the equivalent weight matrix, thereby reducing the impact of faults on system accuracy [12,13,14]. Reference [15] proposes a robust extended Kalman filter (REKF) based on the sequential chi-square test. It calculates equivalent weight factors through a fault detection function and adjusts the noise covariance matrix in real time to improve navigation accuracy. At the same time, EKF filtering can also avoid the interference caused by nonlinear errors. Compared with ordinary EKF, the state estimation accuracy is more accurate [16,17,18]. However, the accuracy of such methods depends on the detection statistics. When the detection statistic’s value is large, the robust effect is good. When the detection statistic’s value is small, the robust effect is poor.

In recent years, methods based on time series prediction, such as machine learning and grey prediction models, have been widely used in integrated navigation systems [19,20]. Among them, the time series prediction method based on machine learning requires a large amount of sample data for model training [21,22,23]. If the number of training samples is small, the prediction accuracy will be poor, and the ideal effect will not be achieved. Among them, the grey prediction model [24] has a small amount of data and high prediction accuracy in a short time. Therefore, it has received extensive attention. Currently, the commonly used grey prediction model is the GM(1,1) model. The GM(1,1) model simplifies the randomness of the data and excavates the potential rules behind the data by processing the original data. Then, a differential equation model is established to predict. Finally, the prediction results are obtained by inverse operation. But the model has insufficient ability to deal with nonlinear sequences. In order to enhance the processing ability of nonlinear sequences, many scholars have introduced the idea of fractional-order into the processing of integrated navigation data. Reference [25] introduced the fractional-order idea on the basis of the grey model to construct the fractional-order grey prediction model. This model extends the first-order accumulation generation of the traditional grey model to the fractional-order accumulation generation, and finds the most suitable accumulation order by optimization, so that the processed data can present the exponential law more perfectly, so as to construct a more accurate differential equation model for prediction. This model has higher-order flexibility, can describe the inherent characteristics of data more flexibly and more accurately, and can better deal with nonlinear sequence data. However, the model does not update the data. With the increase in fault data, more and more fault data are involved in the calculation, which leads to a decrease in model prediction accuracy. So, in order to reduce the influence of fault data on the MINS/GNSS, enhance the fault tolerance of the system and improve the estimation accuracy of the system, we propose a robust extended Kalman filter algorithm based on a sliding window fractional-order grey prediction model (SWFGM(1,1)-REKF). The algorithm combines fault detection, data prediction and robust filtering to form a closed-loop combined fault data processing flow. First, the algorithm constructs a weighted index sequential probability ratio test algorithm based on the sequential probability ratio test (SPRT) to detect system faults. Secondly, the FGM(1,1) model is introduced, and the sliding window FGM(1,1) model is constructed to predict the GNSS observation information at the current time. The GNSS fault data is replaced and then combined with MINS, which greatly eliminates the fault data and reduces the influence of the fault data on the EKF filter. Finally, the algorithm calculates the equivalent weight factor according to the fault detection function value, constructs the equivalent weight covariance matrix of the observation vector, and applies the equivalent weight covariance matrix to the extended Kalman filter to further reduce the influence of the fault data, thereby improving the accuracy and robustness of the system state estimation.

1.1. MINS/GNSS Model

In the MINS/GNSS, the parameter error output by the navigation subsystem is used as the state variable of the integrated navigation system. The difference between the navigation parameters calculated by MINS and the navigation information output by the GNSS receiver is used as the measurement input of MINS/GNSS. The error model of MINS is used as the system equation of MINS/GNSS [26,27]. In this paper, the east-north-up coordinate system is selected as the navigation coordinate system. The loose combination was selected as the system combination. In the MINS/GNSS loosely integrated navigation model, the system state equation is obtained by selecting 15-dimensional state parameters. The state variable can be expressed as:

$$[eqn]$$

where $[eqn]$ is the attitude error of the carrier, $[eqn]$ , $[eqn]$ is the velocity error of the carrier, $[eqn]$ , $[eqn]$ is the position error of the carrier, $[eqn]$ , $[eqn]$ is the bias of the gyroscope, $[eqn]$ , and $[eqn]$ is the bias of the accelerometer, $[eqn]$ .

The system state equation of MINS/GNSS after discretization can be expressed as:

$$[eqn]$$

where $[eqn]$ is the 15-dimensional system state vector at time $[eqn]$ , $[eqn]$ is 15-dimensional nonlinear state transition function. In the calculation of the system equation, the first-order Taylor expansion is performed around the filter value, $[eqn]$ is the 15 $[eqn]$ 6 system noise matrix, $[eqn]$ is the 6-dimensional system noise matrix at previous time $[eqn]$ , the noise conforms to the normal distribution, and the corresponding noise matrix is $[eqn]$ .

The measurement equation of the MINS/GNSS after discretization can be expressed as:

$$[eqn]$$

where $[eqn]$ is the measurement matrix at time $[eqn]$ , $[eqn]$ is a nonlinear observation matrix, in the calculation of the system equation, the first-order Taylor expansion is performed around the filter value, and $[eqn]$ is the measurement noise matrix at time $[eqn]$ .

The discrete EKF can be expressed as:

$$[eqn]$$

where $[eqn]$ is the state transition matrix, $[eqn]$ is the observation matrix, $[eqn]$ is the Jacobian matrix, $[eqn]$ is the state one-step prediction from time $[eqn]$ to time $[eqn]$ , $[eqn]$ is the state one-step prediction covariance matrix from time $[eqn]$ to time $[eqn]$ , $[eqn]$ is the filter gain matrix at time $[eqn]$ , $[eqn]$ is the state estimation at time $[eqn]$ , $[eqn]$ is the state estimation covariance matrix at time $[eqn]$ , $[eqn]$ is the system noise covariance matrix at time $[eqn]$ , and $[eqn]$ is the measurement noise covariance matrix at time $[eqn]$ .

1.2. Weighted Index SPRT Method

In integrated navigation system, the chi-square test method is commonly used to detect system faults. However, this method has the problems of false detection and missed detection for small amplitude fault detection. In order to accurately and quickly detect small amplitude faults, this paper proposes a weighted index SPRT method.

In EKF, the innovation can be expressed as:

$$[eqn]$$

Then the $[eqn]$ -dimensional filtering innovation vector $[eqn]$ at time $[eqn]$ can be expressed as:

$$[eqn]$$

where $[eqn]$ is the $[eqn]$ measurement vector at time $[eqn]$ , $[eqn]$ is the one-step prediction state of the i-dimensional from time $[eqn]$ to time $[eqn]$ .

Since the difference between the MINS/GNSS is used as the measurement input of the MINS/GNSS. $[eqn]$ is an integer, and $[eqn]$ .

The $[eqn]$ -dimensional filtering innovation vector at moments 1~ $[eqn]$ are selected to form the innovation sequence. From the probability theory and mathematical statistics, it can be obtained that $[eqn]$ . $[eqn]$ is the expected value of the $[eqn]$ -dimensional innovation. $[eqn]$ represents the expected value of the $[eqn]$ -dimensional variance [13]. It can be seen from the above:

$$[eqn]$$ $$[eqn]$$

where $[eqn]$ is the observation matrix of the $[eqn]$ -dimension at time $[eqn]$ . $[eqn]$ is the $[eqn]$ -dimensional measurement noise covariance matrix at time $[eqn]$ .

When there is no fault in GNSS measurement data, $[eqn]$ . When there is fault in GNSS measurement data, $[eqn]$ [13]. Hypothesis testing: $[eqn]$ , it is that the system has no fault, $[eqn]$ , it is that the system has fault.

The likelihood function $[eqn]$ of the innovation vector constructed according to the hypothesis can be expressed as:

$$[eqn]$$ $$[eqn]$$

According to the likelihood function, the likelihood ratio $[eqn]$ can be expressed as:

$$[eqn]$$

By taking logarithms on both sides, the detection function of SPRT can be expressed as:

$$[eqn]$$

where $[eqn]$ represents the fault detection function. According to Formula (13)

$$[eqn]$$

The statistical change rate of the SPRT can be expressed as:

$$[eqn]$$

According to the modified Equations (12)–(14), we can see that in order to detect the end time of the fault, the statistic $[eqn]$ needs to fall below the detection threshold quickly at the end of the fault. However, after the end of the fault, when the measurement system returns to normal, $[eqn]$ tends to zero, which leads to the change rate of statistics $[eqn]$ also tends to zero. Finally, after the end of the fault, the statistic $[eqn]$ is always greater than the threshold, and the end of the fault cannot be detected. Therefore, in order to accurately detect the end time of the fault, we use the exponential weighted method to calculate the expected value of the innovation. Let

$$[eqn]$$

where $[eqn]$ represents the weighted coefficient. $[eqn]$ is the forgetting factor. The earlier the data, the smaller the weighting coefficient.

The weighted coefficient constructed by can be expressed as:

$$[eqn]$$

Then the expected value $[eqn]$ of the innovation vector constructed by the exponential weighted recursive estimation method can be expressed as:

$$[eqn]$$

where $[eqn]$ is the one-step prediction state vector error of the $[eqn]$ -dimension at time $[eqn]$ . $[eqn]$ denotes the $[eqn]$ -dimension measurement noise at time $[eqn]$ . In the calculation of the estimated innovation in the expected value of innovation, $[eqn]$ is 0.75.

Similarly, the estimated innovation vector covariance matrix can be expressed as:

$$[eqn]$$

where $[eqn]$ is the one-step prediction measurement vector error of the $[eqn]$ -dimension at time $[eqn]$ .

In order to eliminate the deviation between $[eqn]$ and $[eqn]$ in fault innovation mismatch. We define a deviation adjustment function $[eqn]$ . According to $[eqn]$ , we can get,

$$[eqn]$$

Then the deviation adjustment function is calculated as follows:

$$[eqn]$$

By introducing the calculated deviation adjustment function into the detection function, the fault detection statistic function after exponential weighting can be expressed as:

$$[eqn]$$

Set the detection threshold $[eqn]$

$$[eqn]$$

where $[eqn]$ is the missed alarm rate and $[eqn]$ is the false alarm rate. When $[eqn]$ , it is determined that the system has an unknown fault. When $[eqn]$ , it is determined that the system is no fault. In this paper, $[eqn]$ is set to 0.035, $[eqn]$ is $[eqn]$ [28], and the calculated threshold is 11.7.

2. SWFGM(1,1)-REKF Algorithm

2.1. Construction of SWFGM(1,1) Model

This method is designed to reduce the interference of fault data to the MINS/GNSS system and improve the accuracy of the system. Combined with the literature [24], we construct the SWFGM(1,1) model to predict the GNSS measurement information at the fault time. Then, the predicted GNSS velocity, position, and other measurement information are used to replace the GNSS velocity and position information with faults to improve the accuracy of MINS/GNSS. The SWFGM(1,1) model is constructed as follows:

It is assumed that the historical data sequence of GNSS measurements used for prediction can be expressed as:

$$[eqn]$$

where $[eqn]$ is the sequence, $[eqn]$ is a certain data, $[eqn]$ is the data is not calculated by addition, and is a collection of original data.

After the Formula (23) is cumulatively added $[eqn]$ times, the expression of an item in the determinant can be expressed as:

$$[eqn]$$ $$[eqn]$$

The r-order accumulation sequence can be expressed as:

$$[eqn]$$

According to the $[eqn]$ -order cumulative sequence, the adjacent mean sequence $[eqn]$ can be expressed as [29]:

$$[eqn]$$

where $[eqn]$ is the adjacent mean value at time $[eqn]$ after $[eqn]$ times addition.

The FGM(1,1) grey differential equation and its whitening differential equation can be expressed as:

$$[eqn]$$ $$[eqn]$$

where $[eqn]$ is the development coefficient and $[eqn]$ is the grey action quantity. Equation (28) is written as a matrix.

$$[eqn]$$

where

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$

According to the least squares method, we can get

$$[eqn]$$

Substitute $[eqn]$ into Equation (28). Let $[eqn]$ . The time response function can be expressed as [29]:

$$[eqn]$$

where $[eqn]$ is the initial time.

Therefore, the time response sequence of the FGM(1,1) grey differential equation can be expressed as:

$$[eqn]$$

where $[eqn]$ . According to Equation (30), the fitting value sequence can be expressed as:

$$[eqn]$$

In Equation (36), the data before time $[eqn]$ is a priori sequence, and the data after time $[eqn]$ is a prediction sequence. Based on the prediction model, the predicted values are obtained by inverse generation:

$$[eqn]$$

where $[eqn]$ can be calculated using Equation (30). Add $[eqn]$ , delete $[eqn]$ , and construct a new sequence:

$$[eqn]$$

Perform the operation of above Equations (37)–(39), we can get $[eqn]$ . And so on, we get $[eqn]$ .

In the FGM(1.1) model, the cumulative addition times of the data sequence have a crucial influence on the accuracy of the prediction results. In order to find the optimal cumulative addition number of the sequence, this paper uses the particle swarm optimization algorithm to find the optimal number. The particle swarm optimization algorithm is relatively simple and has fast convergence speed. It is an intelligent optimization algorithm inspired by the foraging behavior of birds. In the particle swarm optimization algorithm, the fitness function determines the accuracy of the solution, so its selection is very important [30]. In this paper, the average relative minimum error is used as the fitness function, and the optimal cumulative number of additions is determined by calculating the fitness value. The number of particles is set to 20, and the maximum number of iterations is 300. When the average relative error is minimized, the algorithm optimization stops and the iteration ends. The fitness function can be expressed as:

$$[eqn]$$

where $[eqn]$ represents the number of original data sequences. After multiple experiments, the original data is 7, and the size of the sliding window is set to 7, $[eqn]$ represents the fault initiation time, $[eqn]$ represents the fault dimension, and $[eqn]$ represents the original data prediction values of the GNSS fault dimension, $[eqn]$ represents the original data of the GNSS fault dimension.

2.2. SWFGM(1,1)-REKF Flow Chart

When the system has a fault, the GNSS observations predicted by the sliding window FGM(1,1) model are affected by fault information. In order to reduce the influence of measurement fault information on state estimation, the proportion of innovation in state estimation must be reduced. We use the fault detection function value to calculate the equivalent weight factor. The constructed equivalent weight covariance matrix can be expressed as:

$$[eqn]$$

Combined with robust estimation, $[eqn]$ is applied to EKF to correct the integrated information. The filter gain, state estimation, and mean square error update equations in the REKF can be expressed as:

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$

In summary, the SWFGM(1,1)-REKF flow is shown in Figure 1, with the main steps:

Step 1: Calculate the innovation, innovation expectation, and innovation covariance.

Step 2: Compute the weighted index SPRT statistic.

Step 3: Judging whether there is a fault in the system according to the threshold value. If there is no fault in the system, update the measurement normally. If there is a fault in the system, the SWFGM(1,1) is used to predict the current time observation. According to the fault detection function, the robust factor is calculated to construct the equivalent weight covariance matrix, and then the measurement is updated.

Step 4: Loop in turn until the end of the algorithm.

3. Experimental Results and Analysis

Small amplitude abrupt fault is a typical fault type of MINS/GNSS. Its characteristics have instantaneous occurrence and great change frequency. It is generally caused by hardware damage, signal interference, and other reasons. For example, GNSS receiver clock jump, complex environmental impact, human interference, etc., will cause a small amplitude mutation of measurement information, resulting in a small amplitude mutation failure of the combined system. In order to verify the advancement and reliability of the proposed algorithm, this paper designs simulation and vehicle experiments for experimental verification and analysis.

3.1. Simulation Experiments

In this paper, the PSINS toolbox software (PSINS220204) is employed to simulate the vehicle trajectory. The vehicle operation states include static, uniform velocity, acceleration, deceleration, and so on. The simulated vehicle driving trajectory is illustrated in Figure 2.

The MINS sampling frequency is 200 Hz, and GNSS sampling frequency is 10 Hz. In order to make the experiment more rigorous, we refer to the experimental parameter settings [31]. The system simulation parameters are shown in Table 1. The experimental parameter settings are shown in Table 2.

Three kinds of small amplitude faults are set in the experiment. The three faults were Simulation-a, Simulation-b, and Simulation-c. We used a traditional robust EKF algorithm based on a chi-square test and the robust EKF algorithm based on the sliding window fractional-order grey prediction model proposed in this paper for processing. The chi-square test adopts the sequential form. In order to facilitate writing, the two algorithms are represented by SCSTREKF and SWFGM(1,1)-REKF, respectively. The results of EKF algorithm processing when the data is fault-free are used as the comparison results. Finally, the results of the three algorithms are compared. The settings of the small amplitude faults were as follows.

Simulation-(a): The eastward velocity contained a fault during the period of 301 s–330 s, and the fault was 1 m/s.

Simulation-(b): The northward velocity contained a fault during the period of 561 s–590 s, and the fault was 1 m/s.

Simulation-(c): The altitude contained a fault during the period of 471 s–500 s, and the fault was 50 m.

The velocity errors and position errors are shown in Figure 3, Figure 4 and Figure 5.

As shown in Figure 3, Figure 4 and Figure 5, when the GNSS signal is disturbed by small amplitude faults, the velocity error curves and the position error curves based on the SCSTREKF algorithm are obviously divergent, and the error values are relatively large. These show that SCSTREKF cannot effectively suppress the impact of faults, which leads to a decrease in MINS/GNSS accuracy. The velocity error curves and position error curves based on SWFGM(1,1)-REKF are relatively convergent, and the error values are small, which are close to the error values when GNSS has no fault. These show that SWFGM(1,1)-REKF can effectively suppress the impact of faults, which improves MINS/GNSS accuracy.

The root mean square error (RMSE) of velocity and position can be summarized as Table 3. $[eqn]$ , $[eqn]$ , $[eqn]$ represent eastward velocity error, northward velocity error, altitude errors, respectively.

As shown in Table 3, when a GNSS signal is disturbed by small amplitude faults, compared with the traditional SCSTREKF, the accuracy of parameter estimation based on SWFGM(1,1)-REKF is higher. The RMSE of velocity is reduced by one order of magnitude, and the RMSE of position is reduced by 57.7%. Navigation accuracy has been greatly improved.

3.2. Vehicle Experiments

In this paper, we use the open MINS/GNSS vehicle integrated navigation experimental data to verify and analyze the algorithm. The running time is 1750 s, the sampling frequency of STIM300 is 200 Hz, and the sampling frequency of GNSS is 10 Hz. The measured data vehicle trajectory is shown in Figure 6.

Three kinds of small amplitude faults are set in the experiment. The three faults were Experiment-a, Experiment-b, and Experiment-c. We use GNSS Normal, SCSTREKF, SWFGM(1,1)-REKF three algorithms for processing. The settings of the small amplitude faults were as follows. Finally, the results of the three algorithms are compared. The settings of the small amplitude faults were as follows.

Experiment-(a) The eastward velocity contained a fault during the period of 711 s–750 s, and the fault was 1 m/s.

Experiment-(b) The northward velocity contained a fault during the period of 1001 s–1030 s, and the fault was 1 m/s.

Experiment-(c) The altitude contained a fault during the period of 801 s–830 s, and the fault was 50 m.

The velocity errors and position errors are shown in Figure 7, Figure 8 and Figure 9.

The RMSE of velocity and position can be summarized as Table 4.

As shown in Figure 7, Figure 8 and Figure 9 and Table 4, when GNSS signal is disturbed by small amplitude faults, the velocity error curves and the position error curves based on the SCSTREKF algorithm are obviously divergent, and the error values are relatively large. The velocity error curves and position error curves based on SWFGM(1,1)-REKF are relatively convergent, and the error values are small, which are close to the error values when GNSS has no fault. Compared with the traditional SCSTREKF, the accuracy of parameter estimation based on SWFGM(1,1)-REKF is higher. The RMSE of eastward velocity is reduced by 50.4%, the RMSE of northward velocity is reduced by 70.2%, and the RMSE of position is reduced by 81.0%. Navigation accuracy has been greatly improved.

From the simulation experiments and vehicle experiments, we can see that the proposed SWFGM(1,1)-REKF is superior to the traditional SCSTREKF. In traditional SCSTREKF, the chi-square test statistic is small, which leads to the large weight of fault observations in the measurement update equation, the weak robustness and the low navigation accuracy. SWFGM(1,1)-REKF uses the SWFGM(1,1) model to predict reliable GNSS observations, which reduces the weight of fault observations in the measurement update equation and effectively improves the accuracy of MINS/GNSS.

4. Conclusions

To address the issue of reduced accuracy or even divergence in MINS/GNSS integrated navigation systems caused by small amplitude fault in GNSS measurement information, this paper proposes SWFGM(1,1)-REKF. Simulation and vehicle experiment results show that the proposed SWFGM(1,1)-REKF is able to predict reliable GNSS observations to replace fault data, reduce the weight of fault observations in the update equations, and suppress the influence of a fault integrated navigation system. Compared with the traditional robust adaptive algorithm based on the chi-square test, the proposed algorithm improves the velocity and position accuracy. This paper provides a technical path to reconstruct reliable observation information under a high noise background, which provides reference for integrated navigation fault handling. At the same time, it also provides a new idea for the processing of high-noise GNSS time series such as gross error detection and repair in deformation monitoring, high-frequency GNSS seismic waveform recovery, and position prediction when a GNSS signal is interrupted in vehicle navigation.