LYU Xu ,HU Baiqing ,DAI Yongbin ,SUN Mingfang ,LIU Yi ,and GAO Duanyang

1.College of Electrical Engineering,Naval University of Engineering,Wuhan 430033,China;2.Beijing Huahang Radio Measurement Research Institute,Beijing 100000,China;3.School of Electrical Engineering,Liaoning University of Technology,Jinzhou 121001,China;4.School of Electronics and Information Engineering,Harbin Institute of Technology,Harbin 150001,China

Abstract: High-precision filtering estimation is one of the key techniques for strapdown inertial navigation system/global navigation satellite system (SINS/GNSS) integrated navigation system,and its estimation plays an important role in the performance evaluation of the navigation system.Traditional filter estimation methods usually assume that the measurement noise conforms to the Gaussian distribution,without considering the influence of the pollution introduced by the GNSS signal,which is susceptible to external interference.To address this problem,a high-precision filter estimation method using Gaussian process regression (GPR) is proposed to enhance the prediction and estimation capability of the unscented quaternion estimator(USQUE) to improve the navigation accuracy.Based on the advantage of the GPR machine learning function,the estimation performance of the sliding window for model training is measured.This method estimates the output of the observation information source through the measurement window and realizes the robust measurement update of the filter.The combination of GPR and the USQUE algorithm establishes a robust mechanism framework,which enhances the robustness and stability of traditional methods.The results of the trajectory simulation experiment and SINS/GNSS car-mounted tests indicate that the strategy has strong robustness and high estimation accuracy,which demonstrates the effectiveness of the proposed method.

Keywords: integrated navigation,Gaussian process regression(GPR),quaternion,Kalman filter,robustness.

1.Introduction

Integrated navigation is a special technology that comprehensively processes data from multiple navigation devices.It effectively overcomes the limitations of a single system,and enhances the accuracy and performance of navigation [1].The strapdown inertial navigation system/global navigation satellite system (SINS/GNSS)combination is better and has a large variety of applications.The two systems complement each other well and have the advantages of strong reliability,continuous output,and high positioning accuracy [2].Nevertheless,similar to platform inertial navigation systems,SINS suffers from a loss of navigation accuracy over time and cannot correct the error by itself.GNSS as a source of auxiliary information is used to correct the system error accumulated over time due to gyroscope drift and accelerometer deviation [3].In the course of practice,information fusion technology plays a decisive role in the integrated navigation system.

Information fusion is the key to integrated navigation,and its realization depends on modern filtering technology.In 1960,the Kalman filter (KF) algorithm was adopted by Kalman [4],which has become one of the most important methods in modern filtering technology.However,the estimation accuracy of the traditional Kalman filter cannot be satisfied in strongly nonlinear systems.In particular,when the expression of a nonlinear function is more complicated,it is easier to approximate the probability distribution of the output of the nonlinear function than to approximate the nonlinear function [5].Compared with the linear approximation extended Kalman filter (EKF),the unscented Kalman filter (UKF) uses the unscented transformation (UT) for state and variance propagation [6].UKF has a higher estimation accuracy and reliability in nonlinear systems.It has received extensive attention from domestic and international experts and scholars and has become a hotspot of research in this field [7].In 2003,the unscented quaternion estimator(USQUE) was proposed by Crassidis for application to the field of spacecraft attitude estimation [8].In [9],the USQUE calculation problem is optimized for the integrated navigation attitude estimation,which reduces the complexity of the algorithm and shortens the calculation time.In [10],by comparing the state estimation performance of USQUE,multiplicate extended Kalman filter(MEKF),and Euler-KF in the integrated navigation system,experimental tests indicate that USQUE has the best accuracy and the strongest stability.

In the practice of the SINS/GNSS system,the natural environment is complex and changeable,resulting in GNSS inevitably being polluted and interfered,affecting the overall performance of the system.The measurement of pollution will degrade the estimated performance of USQUE,where negative sigma points are generated,and it is not always guaranteed that the calculated covariance matrix is positive definite [11].For example,if the carmounted SINS/GNSS system is used in a bustling city center,satellite blockage or information interference may occur [12].When the SINS/DVL system of the submersible actually operates,it may be subject to problems such as changes in ocean currents or anomalous current interference caused by topography [13].Therefore,it is necessary for the USQUE algorithm to deal with interference problems in practical applications.In [14],a method of comparing the square of the Mahalanobis distance from the point to zero with the predetermined quantile of the Chi-square distribution was used to enhance the robustness of KF.However,this method is only applicable to linear systems.The Masreliez-Martin UKF algorithm is robust and automatically adjusts the covariance matrix of the measurement process by fading factors,but this scaling method is difficult to guarantee the positive nature of the variance [15].In [16],to handle the noise pollution and profitability problems in measurement,a robust adaptive mechanism was constructed.This control strategy adopts a method to reduce the weight of polluted measurement information and improve the anti-interference ability of the navigation.However,the algorithm is still disturbed by abnormal measurement information,and is equipped to fundamentally solve the measurement pollution problem,which ultimately leads to low estimation accuracy of the system.In fact,non-Gaussian distribution and outlier interference are common.In [17],at present,the more common robust method ofMestimation is the weighted measurement of the noise covariance matrix.Reduce the filter gain arrayK,thereby reducing the influence of the measurement information.Although this method has certain robustness,for the SINS integrated navigation system with low and medium accuracy,weakening the measurement update will cause certain damage to the filtering accuracy.Therefore,it is necessary and urgent to design a robust mechanism to deal with outliers in nonlinear systems and non-Gaussian environments.

With the development of machine learning,the above problems can be solved.Gaussian regression is a nonparametric regression method,which is based on Bayesian theory and continuously updates the posterior probability distribution through measured data.The final posterior distribution basically fits the true distribution[18].Gaussian process regression (GPR) has the advantages of hyperparameter adaptive acquisition and probabilistic output,and is easy to be implemented.At the same time,GPR can be combined with predictive control,adaptive control and Bayesian filtering very conveniently[19].Therefore,the GNSS information suffers interference,and due to the introduction of pollution observation results,the estimation accuracy and stability are reduced.A control strategy and framework with enhanced robust performance are proposed.On the basis of USQUE,a GPR-based robust USQUE (GPR-USQUE) algorithm is proposed.This method uses the innovation sliding window and GPR measurement sliding window in the system online detection,and performs filtering algorithm measurement.The focus is on building a robust control strategy framework without relying on system models.In SINS/GNSS direct velocity loose combined simulation and car-mounted experiment,compare the estimation effects of USQUE and GPR-USQUE algorithms on attitude and other information.The test results illustrate that the GPR-USQUE algorithm improves the anti-interference performance of the traditional method without reducing the overall estimation accuracy of the system.It is proved that the researched algorithm has good robustness and stability.

The structure design and specific content of the paper are as follows: In Section 2,we introduce the basic equation model of direct SINS/GNSS integrated navigation.In Section 3,the GPR-USQUE algorithm is described and a robust control strategy framework is developed.The effectiveness and feasibility of the developed method are verified by simulation and car-mounted testing in Section 4.Evaluation is given in Section 5.

2.Model basic equation

This section highlights the system modeling of the SINS/GNSS direct velocity loose combination.The system uses GNSS auxiliary measurement information to perform measurement updates to reduce the accumulation of errors in the SINS system time update process.The framework of the SINS/GNSS integrated navigation system is shown in Fig.1.

Fig.1 SINS/GPS integrated navigation system

2.1 System equation

The main framework is set in this Subsection.The Earth frame is represented bye,iis the inertial frame,bis the coordinate system of SINS (Right-Forth-Up),andnis the navigational coordinate (East-North-Up,ENU).The posture part of the state is represented as a quaternion.Therefore,the system state is defined asThe SINS/GNSS direct integrated navigation equation[20],is as follows:

For integrated navigation systems composed of lowprecision inertial navigation.The estimation accuracy of the equipment error has a great influence on the overall performance provided by the integrated navigation.The information update model of the gyroscope is written as

where εbis the gyro bias,ηgvand ηguare white-noise.Similarly,the accelerometer measurement model is written as

whereis the accelerometer bias,ηavand ηauare whitenoise.In summary,(1)-(3) constitute a direct time-continuous state differential equation.

2.2 Measurement equation

The measurement model considers the position as the external auxiliary information of the SINS/GNSS velocity loose combination mode.The velocity is represented byV=[VE VN VU]T.The system observation model is written as

3.Method derivation

3.1 USQUE

The USQUE algorithm effectively solves the quaternion normality constraint problem when it comes to nonlinear filtered pose estimation.It also uses hierarchical computation to solve the problem of matching variance in the filtering process [21].The USQUE algorithm uses a modified Rodrigues parameter (MRP) to transfer sampling points,and the outer layer still retains the quaternion for attitude update.The specific algorithm flow is shown as follows:

The update of the USQUE algorithm mainly includes time propagation,measurement update,and attitude acquisition.

3.1.1 Time propagation

Generate sigma points by state and variance.

where ωiis the weight.

3.1.2 Measurement update

Filter update,given by

3.1.3 Attitude acquisition

Finally,the attitude part in the state is set to zero,the error accumulated in the attitude update process is released,and the filtering period at the next moment is entered.

3.2 GPR robust machine

GPR is one of the Bayesian methods.This method provides a principled way of dealing with uncertainty.At the same time,a confidence interval with upper and lower bounds of probability can be generated.This is essential for making decisions.In actual estimation,we are not only concerned about the estimated value itself,but also about the uncertainty of the estimated value.As a nonparametric method,GPR can adjust the expression ability of the model according to the amount of training data.On the contrary,neural networks need a lot of training data to support [22].

In the nonlinear modeling section,the set of training data,wherexirepresents input data,andyirepresents output.The relationship between input features and output can be defined as

wheref(x) is the true value,ε is independently distributed Gaussian noise,ε～N(0,σn).

The Gaussian process is also called the normal random process.In this data collection,all random variables conform to Gaussian joint distribution [23].Gaussian process is determined by the covariance function and the mean function.In order to determine the GPR model in the case of nonlinearity,the kernel function can be used to map the multi-dimensional input to the high-dimensional space.The kernel function can be any positive definite covariance matrix [24].The squared exponent covariance is used as a kernel function in this paper,as follows:

where Cov(·) is corresponding varian{ce,σf,w},σnare parameters.The parameter set θ=σf,w,σnis the hyperparameter,which is generally obtained by the maximum likelihood method.Thus,the Gaussian process is finally transformed into a problem to find the minimum for the objective function under the equation condition.The objective function can be given by

Since the objective function (17) is non-abrupt,it is impossible to find the optimal solution.The suboptimal solution of the objective function can be calculated by using particle swarm and other optimization algorithms.Through multiple iterations,the value of the optimal solution is gradually approached,and the corresponding hyperparameters are obtained.

The covariance and σ*the mean μ*are estimated,and the Gaussian process is finally determined as follows:

As mentioned above,in this paper,the GPR method is used to construct a robust mechanism for integrated navigation.By establishing the data model of the measurement information,the GPR model output can be used to replace the data update when the measurement is abnormal.This robust machine is applied to the USQUE algorithm,and the reliability of the filter is improved.

3.2.1 State equation update

Please refer to sate update equation of (7)-(11),which is same as USQUE.

3.2.2 Observation update

Step 1The training data input set is constructed in the form of a sliding window of measurement information.The window length isk.The time series that can establish measurement information is {y1,y2,···,yk}.Set the sliding window tok.The historical time data fromy1toyk-1is used as input,and timeykis used as output.Therefore,one piece of mapping conditionf:Rm→R can be represented as

Through sample training and learning,the GPR model can be obtained.

Step 2Get an estimate ofyat the timek,and then obtain the innovation sequence.

Calculation innovation,which is the difference between the predicted and the measurement in the measurement update,is defined as follows:

Step 3The method of identifying outliers in the measurement information is as follows:

whereTDis the threshold.

The key to effective outlier detection isTD.Traditional methods are based on selection based on experience and lack theoretical basis [19].In this paper,a sliding window method is used to calculate the meanand standard deviationCrk-1of the current time series.The3σ principle is adopted in this method and defined as follows:

The threshold is further labeled as

When the variance of the information sliding window is detected to be greater than the defined threshold,it is determined that the measurement information at that moment contains the profit group value.At this time,the GPR model predictionykis used instead of measurementykto update.

Step 4Calculate the stateand variancePkofk,the filter gain moment is trueKk.

The closed-loop calculation can be referred according to (21)-(28).The GPR innovation process of robust USQUE is presented in Fig.2.

Fig.2 The GPR innovation process of robust USQUE

3.2.3 Attitude update

The essential update of attitude is calculation ofalso uses the relationship between δσ and δq.uses the quaternion multiplication formula which is the same as (13).

It is worth noting that when a certain data is not part of the training data set,the correlation of the GPR training data will be reduced.Therefore,based on the GPR robust strategy of sliding window,this paper finally selects the length of the sliding window as 20 according to the experimental data.

4.Simulation and field test

In this section,the robust USQUE algorithm studied is evaluated through simulation and SINS/GNSS inertial navigation field test.

4.1 Simulation study

To illustrate the validity of the proposed GPR-USQUE algorithm.The following uses the SINS/GNSS direct velocity loose combination as the application background to conduct a simulation study.Compare the performance of USQUE and GPR-USQUE algorithms in attitude estimation.

Simulate the trajectory as shown in Fig.3,including states of rest,constant velocity,acceleration,descent,climb,yaw,pitch,and roll.

Fig.3 The simulation trajectory

The initial position is 34.246°latitude,108.909 7°longitude,and a height of 380 m.The total simulation time is 1 200 s.SINS update frequency is 100 Hz.GNSS calculation time is 1 s.The IMU parameters are set as follows:the gyro drift is 1°/h,the angle walk coefficient isthe accelerometer constant zero offset isand the accelerometer walk coefficient is 10 μg.Fig.4 and Fig.5 give the carrier attitude and velocity changes,respectively.

Fig.4 Change of attitude

Fig.5 Change of velocity

The system state includes attitude,velocity,position and device error.The initial state is defined as follows:

The system and measurement noise matrix is given as

In order to evaluate the estimated performance of the performance of USQUE and GPR-USQUE under the condition of observing the contamination distribution,the so-called contamination case,the case when the observation contains wild values.The windowkinvolved in GPRUSQUE is 20 s.The velocity information of the navigation system during normal operation is used as the input of the GRP training setx=[x1,x2,···,xk],where theith inputrepresents the integrated navigation output at theith moment the velocity in the east-north-up coordinate system.In the simulation period of 200 s,600 s,and 1 000 s for 5 s continuously,the velocity information observed suddenly becomes 1.2 times.

The robust strategy based on GPR is applied to the USQUE model,and the position error of the simulation result is shown in Fig.6.The δL,δλ,δHrespresent latitude,precision and altitude errors respectively.According to the experimental results,in the case of measuring the pollution distribution,the GPR-USQUE algorithm is used in the integrated navigation system.The filtering results are convergent and the filtering algorithm is reliable,indicating that the GPR robust strategy can effectively suppress measurement interference.

Fig.6 Position error of GPR-USQUE

The attitude estimation performance of the USQUE and GPR-USQUE algorithms is compared.Fig.7-Fig.9 show the attitude error curves.

Fig.7 Pitch estimate errors

Fig.8 Roll estimate errors

Fig.9 Yaw estimate errors

We can learn from Fig.7-Fig.9,the pitch,roll,and yaw angles are significantly different in contrast.Generally,the yaw angle plays an important role in the comparison of attitude estimation accuracy.The experimental results demonstrate that the traditional USQUE algorithm is not robust when outlier interference occurs in the measurement information.Affected by interference,the attitude error is relatively large.On the contrary,the GPRUSQUE algorithm proposed in this paper has better antiinterference performance,higher accuracy of attitude estimation,and strong reliability.

4.2 Vehicle test

To deeply understand and verify the advantages of the control strategy development in this paper,a car-mounted experiment is carried out.In the sports car experiment,the main equipment used low-precision micro electromechanical system (MEMS) and NovAtel ProPak6 GNSS receiver.Inertial Exploer 8.60 (IE 8.60) is a high-precision integrated navigation post-processing software from NovAtel.It is used to perform the SINS/GNSS differential tight combination bidirectional smoothing process,and the navigation results are used as reference values.The gyroscope constant drift is 0.01°/h,accelerator bias is 0.005 g.In this experiment,the GNSS update frequency is 1 Hz,inertical measurement unit (IMU) frequency is 125 Hz.Because of the loose combination used by the system,only the inertial navigation update is performed when there is no measurement information.The experiment is carried out from Qingdao,China,and the movement is about 700 s.The trajectory map is shown in Fig.10.

Fig.10 Test trajectory around the sports grounds

First,for GNSS data,between 100 s and 500 s,the outliers increases by 100 m/s.The final attitude and attitude error curves are plotted in Fig.11 and Fig.12.From Fig.11 and Fig.12,we can get the USQUE filtering performance is very well during 0-100 s,which means that the state statistics in the system model obey the Gaussian assumption.On the contrary,the measurement information containing outliers has a great negative impact on the traditional USQUE algorithm,after 100 s.According to the test results,the attitude curve of the GPR-USQUE filtering method in the attitude estimation of integrated navigation converges quickly and with high accuracy,and the attitude error is significantly reduced in the overall period.In the comparison of the attitude error of the field test,we can know that the GPR-USQUE algorithm has a small attitude error and robustness.The attitude error of the traditional USQUE algorithm jumps,which affects the reliability of the system.

Fig.11 Attitude estimate

Fig.12 Attitude estimate errors

As we all know,in the case of four observable satellites,GNSS measurement can output high-precision velocity and position information.Affected by the environment,vehicle GNSS information will be obscured.From the perspective of practical application,the algorithm developed in this paper can effectively solve the above problems.

When the robust mechanism detects that the measurement is unreliable,GPR effectively processes the measurement and replaces the update,performs integrated navigation,and maintains high filtering accuracy.The above discussion is validated by the results of velocity and position error comparisons in Fig.13 and Fig.14.

Fig.13 Velocity estimate errors

Fig.14 Position estimate errors

It can be seen from Table 1 that GPR-USQUE has a 24%-56% reduction in the mean value of attitude error,and the mean square error is also reduced accordingly.The experimental results illustrate the attitude estimation accuracy and strong robustness.

Table 1 Mean values and mean variances of errors of attitude

5.Conclusions

In the attitude estimation of the integrated navigation system,the outlier problem of the pollution distribution of the observation information seriously affects the system performance.A control strategy and framework with enhanced robust performance are proposed in this paper.This paper evaluates the development method through simulation and car-mounted test data.Test results verify that the proposed algorithm can effectively search for outliers and perform robust processing.It can reduce the error caused by the observation uncertainty and improve the robustness and stability.The disadvantage of developing a robust strategy is that it relies on reliable previous external measurements.Improved algorithms that can resolve the coexistence process and measurement uncertainty are under development and expected to be proposed in the near future.