A robust estimation method of GNSS/IMU fusion kalman filter

Aiming at the traditional gross error detection method of GNSS/IMU fusion Kalman filter, which simply retains or eliminates abnormal observation data and does not make full use of useful observation information, a new robust estimation method of Kalman filter is proposed. Based on the chi-square statistical hypothesis test, this method constructs continuous change measurement using weight coefficient, fully excavates measurable innovation between normal value and abnormal value, and establishes a new measurement update equation of Kalman filter robust estimation. Finally, through GNSS/IMU integrated navigation simulation, the advantages of the new Kalman filter are verified: no parameter adjustment is required, and the statistical error fluctuation is smaller than that of the traditional adaptive filter.

be directly calculated according to the KF innovation size to achieve a seamless and continuous transition of weight size.However, the ideas of the two adaptive filtering methods are not completely the same.By analyzing the characteristics of binary utilization of innovation weight in traditional chi-square detection, this paper gives a continuous utilization method of innovation weight, improves the binary value of weight to continuous value, and generalizes it, thus obtaining a multi-component chi-square detection method for multi-dimensional measurement.In this paper, the proposed new method is called Soft Chi-square Test KF (SC2TKF), while the traditional method can be called Hard Chi-square Test KF.Obviously, the new method is more universal, and the traditional hard chi-square test can be regarded as a special case of the new soft chi-square test method.At last, the KF simulation of the soft chi-square detection method is carried out by using the inertial navigation/satellite navigation combination, and the results show that the new method has better filtering accuracy and stability than Sage Husa adaptive filtering.

Traditional measurement fault chi-square detection principle
The state space model of stochastic linear systems is written as [6][7][8] / 1 where k X / 1  n is the state vector of dimension, k Z / 1  m is the dimensional measurement vector; Φ Γ H are the known system structure parameters, respectively, called n n .One step state transition matrix of the dimension l n  of dimension system noise allocation matrix and n m dimensional measurement matrix is depicted;  l is the system noise vector of dimension, and is the measurement noise vector of dimension, both of which are zero mean Gaussian white noise vector sequences, and they are mutually uncorrelated.The following basic assumptions of Kalman filtering about noise are met.
The Kalman filtering formula is: where Statistics can be constructed from measurement innovation and its mean square error matrix [1].
where k  obeys the degree of freedom as m chi-square distribution, i.e., 2

Measurement is normal denoted by
Measurement is anomaly denoted by This is the chi-square detection principle of the Kalman filter for measuring faults.Statistics can be calculated during filtering k  .According to its size, it is monitored whether the measurement is abnormal in real-time, and then it is decided whether to update the measurement.The measurement and its mean square error matrix update formula in Formula (3) can be rewritten as: Obviously, when k . Normal Kalman filtering measurement update is performed.And The measurement update is abandoned to isolate the abnormal measurement.

A new robust Kalman filter estimation method
The precondition for the effectiveness of the traditional chi-square detection method is that the filtering system model is accurate.In practical applications such as inertial navigation/satellite navigation integrated navigation, the system structural parameter modeling of Kalman filtering is relatively accurate and stable, while the noise parameters are difficult to be fully accurately modeled due to the change of motion state, imperfect modeling, system aging or operating environment, which will lead to the chi-square detection threshold Dm T .It is difficult to determine accurately in strict accordance with theoretical methods.If the threshold value is set too high, the probability of fault detection will be reduced, and there is a risk that more outliers will be introduced into the filtering measurement, which will increase the filtering error; If the threshold is set too small, the probability of false alarm will increase.The frequent false alarm will reduce measurement utilization, and the reduction of measurement correction will also reduce the accuracy of filter estimation.Of course, for high-precision inertial navigation systems, more than 50% of satellite navigation measurements are abandoned randomly.For example, even if the satellite navigation measurement is reduced from 1 Hz to 0.1 Hz, it has little impact on the integrated navigation performance; However, for low-precision inertial navigation systems, the change of satellite navigation utilization rate from 1 Hz to 0.5 Hz may have a significant impact on the combination accuracy.The result of the traditional chi-square detection method is binary, not 0 is 1, and there is no intermediate state.In radar target detection and other fields, this method mainly uses "yes/no" as the indicator, and its application is very reasonable.However, for integrated navigation and other occasions, there may be a large number of intermediate states (suspicious) between normal (trusted) and abnormal (discarded) measurement information, so it is not appropriate to simply use chi-square to detect binary results.This also goes against the commonly used measurement adaptive filtering and robust filtering methods.For example, in Sage Husa measurement adaptive Kalman filtering, the size of the measurement noise variance matrix is automatically adjusted by the size of the measurement innovation, which is equivalent to the comprehensive utilization of all measurement information.Compared with the normal measurement, the measurement weight is reduced when the measurement innovation is medium (suspicious) and is very small when the innovation is abnormal.If it tends to zero, it is equivalent to abandonment.According to the innovation utilization characteristics of adaptive filtering, Formula (6) can be improved to the following: Obviously, when the equivalent measurement innovation is abnormal, the equivalent filter gain in Formula ( 7) is k k  K .The utilization of innovation will gradually decline with the increase of innovation, which is a soft decline mode, so it is called the soft chi-square detection method in this paper.
Compared with Formula (7), the method of binary hard decline (from 1 to 0) of innovation utilization rate in the traditional chi-square detection Formula ( 6) can be called the hard chi-square detection method.
It is worth noting that if the measurement in Formula ( 1) is multidimensional, the traditional chi-square detection regards multidimensional measurement as a whole.As long as any component is abnormal, the chi-square detection method will also reduce the innovation utilization rate of other normal measurement components at the same time.This processing method is not very reasonable.To avoid this defect, it is necessary to do chi-square detection on each measurement component one by one.If, in the Kalman filtering model, the measurement components are uncorrelated, that is, the measurement noise mean square error matrix k R is a diagonal matrix, the chi-square detection of each sequential measurement update is equivalent to the processing of a single measurement component one by one after the sequential filtering processing is adopted.If k R is a nondiagonal matrix, for the new A is the innovation mean square error matrix k A of i , which is i Column diagonal elements and recorded statistics.
Each statistic ( )  is subject to a chi-square distribution with the degree of freedom of 1, that Concerning Formula (7), for the promotion and establishment of ( )  , the constructed Kalman filter measurement update method is as follows: In Formula (10), the filter gain k K is modified.The purpose is to ensure the subsequent mean square error matrix k P Symmetry of the calculation of Formula ( 7) is k ρ special case when all components are equal.In Formula (12), the parameter 1

D
T is a chi-square statistic with a degree of freedom of 1 ( )  .The threshold value is set for fault detection.The significance level is 0.05

Inertial navigation/satellite navigation integrated navigation model
The inertial navigation system/satellite navigation system loose integrated navigation system is taken as an example for simulation experiment verification.The system status is 15 dimensions, and the measurement is 3 dimensions.The specific modeling is as follows:  Sky" navigation coordinate system; b represents the "right front top" carrier coordinate system; INS GNSS , p p are the position output of inertial navigation system and satellite navigation system, including latitude, longitude, and altitude; In [6], there are specific expressions for ** M .

Simulation conditions
The PSINS toolbox software is used to simulate a section of a vehicle driving track, which includes stationary, acceleration and deceleration, uniform speed, turning, climbing, and other stages.The total simulation time is about 1000 s, and the driving mileage is about 7.5 km.The driving speed is shown in Figure 1, and the two-dimensional plan of the track's relative position change is shown in Figure 2. The mark "✭" in the figure is the starting point of vehicle driving.Inertial sensor data and satellite navigation positioning data are generated according to trajectory simulation, in which inertial data is sampled at 100 Hz, and satellite navigation measurement is conducted at 1 Hz.Then, inertial navigation solution and inertial navigation/satellite navigation integrated navigation Kalman filter solution are carried out.Various errors injected in the simulation process are listed in Table 1.In particular, the position measurement error of the satellite navigation system (three components of latitude and longitude) is set as Gaussian white noise with zero mean value, and the variance in the normal period is 2 (1m) , except for the following two periods: (1) The noise variance between 200 s and 400 s is set to 2 (10m) ; (2) It is set as pollution noise between 600 s and 800 s.Taking the satellite navigation altitude measurement error as an example, it is expressed as 2 2 N(0, (1m) ) w.p. 0.9 δ ~N(0, (100m) ) w.p. 0.1 where "w.p." means "with probability".Formula (14) represents noise δH .Under normal circumstances, the variance is approximately 2 (1m) , 90% of the Gaussian distribution is regarded as normal noise; Under abnormal conditions, the variance is 2 (100m) , 10% of the Gaussian distribution is regarded as outlier noise.
An example of a group of height measurement error simulations is shown in Figure 3.It can be seen from the figure that the overall fluctuation of height error becomes larger between 200 s and 400 s, and the simulated noise variance changes; However, individual error amplitude jumps greatly between 600 s and 800 s, simulating small probability outlier jumps.

Simulation results
The results of attitude misalignment angle, velocity error, and position error of inertial navigation/satellite navigation integrated navigation are shown in Figure 4.It can be seen from the figure that the error of integrated navigation increases with the measurement noise of satellite navigation between 200 s and 400 s, and the magnitude of this error and the variance of the whole range of the satellite navigation error are set to 2 (10m) .The results are consistent.Between 600 s and 800 s, the state estimation is not affected by the outliers of satellite conductance measurement noise, and the filter has a good outlier isolation effect, with only a small variance 2 (1m) .Like the measurement noise, it always maintains a high filtering accuracy.In addition, the paper also made a comparative simulation of the new method (SC2TKF) and the traditional Sage Husa measurement noise adaptive filter (SHAKF), carried out 50 Monte Carlo simulations, and counted the navigation parameter errors.The results showed that SC2TKF was more stable, of which the RMS (Root Mean Square) error statistics of latitude error were shown in Figure 5, and other errors were similar, so they would not be listed one by one.It is worth mentioning that there is also a fading factor in SHAKF that needs to be set carefully (b=0.5).If the setting of the fading factor is not appropriate, it will have a greater negative impact on the filtering results.In a word, the new method does not need to set any parameters and has the advantages of convenient use and strong adaptability.

Conclusion
In the actual integrated navigation system, in addition to the classical Sage Husa adaptive filtering based on measurement noise adaptation which is more practical, other adaptive methods and theories often have ideal assumptions or cumbersome parameter settings, which lack feasibility and are difficult to apply to complex high-dimensional systems, so there are few reports of practical applications.This paper proposes a new quality control method for the GNSS/IMU Kalman filter.It can effectively perform chi-square statistical hypothesis tests on each measurement component one by one, eliminate abnormal observations, retain useful information, avoid traditional overall tests, eliminate all measurement information, and effectively improve the stability of GNSS/IMU fusion positioning.
navigation misalignment angle, velocity error, position error, gyro random constant drift, accelerometer random constant deviation, respectively; g b w and a b w are the gyro white noise and accelerometer white noise; V is the measuring white noise for the position of satellite guidance receiver; n b C is the inertial navigation attitude matrix; n represents the "East North

Figure 1 .
Figure 1.Driving speed of vehicle.Figure 2. Plane track of vehicle driving.

Figure 2 .
Figure 1.Driving speed of vehicle.Figure 2. Plane track of vehicle driving.

Figure 3 .
Figure 3. Height measurement error of satellite guidance.

Figure 4 .
Figure 4. Estimation error based on soft chi-square detection.

Figure 5 .
Figure 5.Comparison of latitude errors of different filtering methods.