SINS/GNSS integrated navigation system based on maximum versoria filter

Chunhui ZHAO, Zhenyu YANG, Xiaoran CHENG, Jinwen HU, Xiaolei Hou

School of Automation, Northwestern Polytechnical University, Xi’an 710000, China

KEYWORDS Extended Kalman filter;Gaussian kernel function;Maximum versoria criterion;Missile-borne SINS/GNSS integrated systems

Abstract In the missile-borne Strapdown Inertial Navigation System/Global Navigation Satellite System (SINS/GNSS) integrated navigation system, due to the factors such as the high dynamics,the signal blocking by obstacles,the signal intefereces,etc.,there always exist pulse interferences or measurement information interruptions in the satellite receiver, which make nonstationary measurement process. The traditional Kalman Filter (KF) can tackle the state estimation problem under Gaussian white noise, but its performance will be significantly reduced under non-Gaussian noises. In order to deal with the non-Gaussian conditions in the actual missile-borne SINS/GNSS integrated navigation systems, a Maximum Versoria Criterion Extended Kalman Filter (MVC-EKF) algorithm is proposed based on the MVC and the idea of M-estimation, which assigns a smaller weight to the anomalous measurements so as to suppress the influence of anomalous measurements on the state estimation while maintaining a relatively low calculation cost.Finally, the integrated navigation simulation experiments prove the effectiveness and robustness of the proposed algorithm.

1. Introduction

The application of precision guided tactical weapons in modern military is more and more extensive.. There is a higher demand for the autonomous navigation function of missiles and other carriers. The navigation system is mainly required to have the characteristics of high precision, high reliability and strong anti-jamming ability on the premise of meeting the cost requirements. In missile-borne navigation system, it is difficult to obtain a high-precision ballistic missile motion state with only a single navigation technology.Strapdown Inertial Navigation System/Global Navigation Satellite System(SINS/GNSS)integrated navigation system is widely used.The combination of the two sub-navigation systems can achieve complementary advantages and improve the estimation accuracy and reliability of the navigation system.Under the condition of certain system hardware performance, the advanced fusion estimation method is an effective way to improve the accuracy, real-time performance and reliability of the integrated navigation system.

In the SINS/GNSS integrated navigation system, the most widely used fusion method is the Kalman Filter (KF) algorithm.In some common application environments, such as vehicle-mounted environments, the nonlinear Kalman filter algorithm can effectively obtain the optimal estimation of navigation parameters. But in the missile-borne environment, the satellite receiver flies with the missile body at high speed.Such a high dynamic environment makes the satellite signal received by the satellite receiver have a large Doppler frequency shift of change. The satellite receiver loop needs to expand the bandwidth to maintain tracking, so excessive noise is introduced,which reduces the quality of signal measurement and produces abnormal measurement information.In addition, due to the high dynamic characteristics of the missile and the flight attitude, the satellite receiver may not be able to track a satellite continuously and stably. This will cause nonstationary measurement process. For the integrated navigation system, the information measured by the satellite receiver is abnormal,that is, the measurement information in information fusion is abnormal, and the measurement noise presents a non-Gaussian distribution. Since the conventional KF algorithm is the optimal estimation under Minimum Mean Square Error(MMSE), the MMSE criterion is very sensitive to large outliers, which will lead to the reduced estimation accuracy of conventional KF or even the system divergence when the measurement information is abnormal.

For the filtering problem in integrated navigation systems,some robust filtering methods have been proposed by researchers,mainly including the following categories.The first type of method is sequential Monte Carlo sampling, in which particle filtering is widely used.Gordon et al.proposed a particle filter directly derived from the Bayesian estimator.Subsequently,some related algorithms have been proposed one after another.However, the probability density function must be approximated by a large number of particles, so the calculation of particle filtering is relatively large.The second type of method is the multi-model filtering method,and it is also used to solve the filtering problem under non-Gaussian conditions.However,this type of method also faces the problem of computational complexity, which increases significantly as the sub-Gaussian term increases. The third method is Hfiltering algorithm which is also used to enhance the robustness of the system.However,such methods need to adjust parameters based on experience or experimental data, and may only be suitable for certain specific situations. And the fourth type of method is M-estimation which is an effective method to solve the filtering problem under non-Gaussian condition,especially under the condition of outlier interference. Branko et al. proposed a statistical method M-estimation, which used the Huber function to enhance the robustness of linear system filtering in non-Gaussian noise environments or existing outliers.Since then, many scholars have extended this method to nonlinear Kalman filter.However,due to its own functional characteristics, for heavy-tailed impulse noise or large abnormal values, measurement values with large errors still affect the system. Inspired by the idea of M-estimation, some scholars take the Gaussian kernel function in the Maximum Correlation entropy Criterion (MCC) of Information Theoretic Learning (ITL) as the cost function instead of Huber function, and rededuce the improved filtering algorithm. This kind of method resists the interference of outliers better than the method based on Huber function. Using this kind of method, Liu et al. clarified the probability and geometric significance of MCC as a criterion of local similarity and established the relationship between MCC and M-estimation.A Kalman filter algorithm called MCC-KF was proposed,which integrates MCC and weighted least squares method into the traditional Kalman filter framework.Kulikova discussed the numerical stability of linear filtering technology MCCKF under the MCC method, and in order to improve the numerical stability, proposed the square root format of MCC-KF.These two Kalman filters only modify the weight of the measurement information, so the prediction process is basically the same as the traditional Kalman filter based on the Gaussian assumption. Therefore, MCC-KF and its square root version are sensitive to non-Gaussian process noise. In order to solve this problem, Chen et al. took Gaussian kernel function as cost function, equated the filtering problem with linear regression problem, and derived a Kalman filter based on maximum correlation entropy, which is called MCKF.In MCKF, both the prediction process and the measurement update process are adjusted. Subsequently, for the nonlinear system model, scholars proposed an MCC-based unscented Kalman filter.Aimed at the high-dimensional system model, the cubature Kalman filters based on MCC has also been proposed and applied.The filtering algorithm based on MCC have better resistance to outliers than the algorithm based on Huber function. However, the numerical stability based on the MCC filtering algorithm is low,and the Gaussian kernel function in the MCC involves exponential operation,so the computational cost is relatively high.

Therefore, this paper introduces a new criterion and proposes an improved nonlinear filtering fusion algorithm,namely Maximum Versoria Criterion Extended Kalman Filter(MVCEKF),which aims to obtain the motion parameters of the missile with high accuracy and reduce the calculation cost during nonstationary measurement process. The following contributions have been made: First, while considering the calculation cost and estimation accuracy, a robust filtering algorithm based on MVC is proposed, which can face the interference of abnormal situations. Compared with MCC, the numerical stability and calculation time of the method in this paper have been improved. Second, we analyze the influence of algorithm parameters on state estimation. Finally, by setting different simulation scenarios in the missile-borne integrated navigation system,it is verified that the algorithm in this paper is suitable for nonlinear systems and can effectively deal with abnormal interference problems.

The rest of the paper is structured as follows. In Section 2,the integrated navigation system is introduced,and some problems that may exist in the SINS/GNSS integrated navigation system in the missile-borne environment are analyzed. In Section 3, we derive the MVC-EKF algorithm and give the algorithm flow. In Section 4, the missile-borne SINS/GNSS navigation system simulation experiments are conducted to show the excellent performance of MVC-EKF. Section 5 concludes the paper.

2. Fundamentals and framework

2.1. Integrated SINS/GNSS navigation system

There are three models in the SINS/GNSS navigation systems which are loosely-coupled, tightly-coupled and deeply coupled.In the loosely-coupled integrated model, the SINS and the GNSS work independently and the combined filter integrates the position and velocity information of the two systems. The loosely-coupled structure is simple. However, when the number of stable tracking satellites of satellite receiver is less than 4, the receiver cannot perform positioning calculation,which will damage the performance of the integrated navigation system. The tightly-coupled structure is shown in Fig. 1. In this combination model, the information sent to the combined filter is the pseudo range and pseudo range rate information. The deeply-coupled system uses a GNSS vector tracking receiver, which has better system performance. As the degree of combination deepens, the overall performance of the integrated navigation system is superior, but the deeply-coupled integrated navigation system needs to change the internal structure of the satellite receiver, which is more complicated to implement.Therefore,the tightly-coupled integrated navigation structure is adopted in this paper.

2.1.1. State-space equation

We select Earth-Centered Earth-Fixed (ECEF) coordinate as the navigation coordinate and build the SINS/GNSS system model in ECEF which consists of a n-dimensional (n = 17)state vector x. The three-dimensional attitude angle, velocity and position of the missile in the ECEF coordinate system are defined as ψ，v and r respectively. δ represents the error between the INS output value and the true value. The state vector is expressed as

Fig. 1 Structure of tightly-coupled SINS/GNSS system.

where uis the unit vector of the line of sight from the GNSS receiver to the jth satellite in ECEF coordinate.

Due to the factors such as the high dynamics, the signal blocking, the signal interfereces, etc., nonstationary measurement process will occur. Thus the measurement information of the system may have outliers or interruptions.These conditions and their adverse effects on the integrated navigation system will be discussed in the following part.

2.2. Analysis of abnormal condition and existing methods

The Doppler effect exists when the satellite receiver is moving.That is,the influence of the Doppler frequency shift caused by the relative movement between the signal receiving device and the transmitting device needs to be considered. The basic calculation formula of Doppler frequency shift is

Δf=vf/c （8）

where vis the relative speed between the satellite receiver and a certain satellite, fis the signal frequency, and c is the speed of light.

Doppler shift could cause that the GNSS receiver loses lock.The faster the vehicle moves,the larger the Doppler shift will be. In order to deal with the Doppler shift problem, the satellite receiver loop needs to expand the bandwidth, but higher magnitude of measurement noise will be introduced consequently.In addition,the signal blocking and signal interfereces will cause larger measurement covariances. The above situation will cause nonstationary measurement process. For example, the signal blocking will cause intermittent interruption of the receiver’s measurement information, and the value of the measurement data will become zero. An example of the intermittent interruption in the pseudo range measurements is shown in Fig. 2, which is collected from a real missile experiment.

We assume that zobeys Gaussian distribution. However,due to the high dynamics and the signal blocking,the measurement will become nonstationary process. The measurenment has large covariances,which have an adverse effect on the performance of integrated navigation system. In the integrated navigation system,the pseudo range measurement information zis the difference between ρand ρ. When ρhas a large error because of certain reasons, znoise may obey the heavy tailed non-Gaussian distribution.It can be seen that when there are measurement outliers in the pseudo range and pseudo range rate information or the nonstationary measurement process, the system cannot meet the assumption of Gaussian distribution.

Fig. 2 Pseudo range data in case of intermittent interruption.

Since traditional KF is designed under the hypothesis of Gaussian distribution using MMSE standard,outliers can lead to low estimation accuracy or even system divergence. In the integrated navigation system fusion filtering algorithm, both the particle filter and the multi-model filtering method both face the problem of high computational complexity. Both of the robust filter estimation method based on Huber function and MCC can resist the interference of out liers. Compared with the two methods, the estimation method based on MCC (MCC-EKF) has better performance in abnormal environment than the method based on Huber function.However, the numerical stability of MCC-EKF is low., and it involves exponential calculations,so the calculation cost needs to be improved.The algorithm proposed in this paper borrows the idea of MCC-EKF,and it will be introduced in the following section.

3. Maximum versoria criterion wxtend Kalman filter

3.1. MCKF algorithm

The MCKF algorithm can deal with the abnormal interference by ～rand ～P, but its numerical stability and computational cost need to be improved. Therefore, this paper introduces a new function and proposes an improved filtering fusion algorithm, which aims to obtain the optimal estimation and solve the motion parameters of the missile reliably during nonstationary measurement process.

3.2. Versoria function

Based on Huber function and MCC-based algorithm, the weight function is introduced to re-derive the KF algorithm.The weight functions involved in Eq. (15) and Eq. (17) play a role in resisting outliers. The two weight functions derived based on the Huber function and the Gaussian kernel function under the MCC criterion are given as

where sgn（·）is signum function,γ is a regulator,exp（·）is exponential function and σ is also a regulator.

We compare the weight functions f（e）（γ=1） and f（e）（σ=2） as shown in Fig. 3. Although Huber function has a certain inhibitory effect during large error, the speed of weight function approaching 0 is slow with the increase of error.The suppression effect of Huber function does not work well. Gaussian kernel function suppression effort is better under large error.However,when the weight function changes with the error,the speed of approaching 0 is too fast.It makes～Por ～rapproach 0 and the numerical stability be low.Besides, the weight function of MCC criterion involves exponential calculations, which is expensive. Therefore, we want to find a better function to replace Huber function or Gaussian kernel function to deduce Kalman filter again. It makes the algorithm have the ability to face abnormal interference and have higher numerical stability and lower computational cost.

Fig. 3 Comparison of weight function.

Recently, the Maximum Versoria Criterion (MVC) was proposed by Huang and successfully applied to adaptive filter under impulse noise.It has the advantanges of low exponential calculation and small steady-state error in various non-Gaussian noise. Inspired by it, the versoria function is steep at the opening and gentle at the bottom, and does not need exponential operation, so the versoria function can replace the Gaussian kernel function to improve the Kalman filter.Therefore, we consider using MVC instead of MCC criterion to propose an improved fusion filtering algorithm, which has better performance when there is outlier interference in the measurement information or the measurement information is interrupted intermittently. In order to deduce the algorithm,we elicit the versoria function firstly. It is defined as

where a ＞0 is a regulator,p ＞0 is the shape parameter of the versoria function, and usually p=2. Normalize the function f（e）by dividing by 2a and take a=1,then the versoria function is shown in Fig. 3. According to Fig. 3, as the error increases, the weight function f（e）/2a gradually approaches zero. The value of f（e）/2a tends to 0 faster than the weight function f（e） and slower than the weight function f（e）,so the effect of suppressing anomalies is better than the algorithm based on Huber function,and it is similar to the filtering algorithm based on MCC criterion.In addition,the numerical stability of the filtering algorithm based on MVC is higher than that based on MCC. The f（e）/2a function does not involve exponential operation,so the calculation cost is lower.

3.3. MVC-based estimation method

After the analysis of the versoria function, it is a suitable choice to use the versoria function instead of Huber function and Gaussian kernel function. In this section, a new Kalman filter using MVC which has a better performance in non-Gaussian noise is derived. If we regard the real value of state as x,and the prediction value of state as^x,the error of state prediction is δ=-（x- ^x）. Combining prediction and

It is easy to notice that Cand Cof Eq. (33) and Eq.(34)tend to I and MVC-EKF will degenerate to the traditional EKF algorithm when parameter approaches infinity.We summarize the proposed MVC-EKF algorithm in Table 1.

4. Simulations

4.1. Simulation setup

In this section,MVC-EKF is applied to the missile-borne integrated navigation system to analyze the algorithm performance. The MVC-EKF algorithm is compared with the traditional EKF and MCC-EKF algorithms to analyze the estimation performance and calculation time of MVC-EKF.

The simulation trajectory is generated by trajectory simulation program.The parameter setting refers to a certain type of missile. The settings are as follows: missile diameter d=155 mm, missile mass m=50 kg, crosswind wind speed w=0 m/s,longitudinal wind speed w=0 m/s,atmospheric density ρ=1.21 kg/m, launch point at (109E，39N), elevation y=1000 m. The particle trajectory kinematics equations are as follows

where N represents the time steps of simulation. The second one is the mean calculation time, i.e.,

Table 1 MVC-EKF algorithm.

Simulations are run in three different scenarios to verify the effectiveness of the proposed algorithm. In Scenario I, the noise conforms to the Gaussian distribution. In Scenario II,the pseudo range and pseudo range rate measurement information are interfered by pulses. In Scenario III, the measurement information is interrupted intermittently. The platform for the simulations is MATLAB R2018a in Dell G3 computerwith the Intel(R) Core(TM) i5-9300H CPU of 2.40 GHz.

Fig. 4 Missile trajectory.

4.2. Simulation scheme results analysis

Firstly, when the system measurement noise is Gaussian distributed, we analyze the performance of the MVC-EKF algorithm and compare it with other filtering methods, e.g., EKF algorithm, and MCC-EKF algorithm. The comparison results are analyzed based on RMSE indicator.

Table 3 demonstrates the estimation performance of the three filters in the case of Gaussian noise for the RMSE defined in Eq. (38).

These results show that in the case of Gaussian noise, all three filters can effectively estimate states. The traditional EKF algorithm achieves the smallest RMSE, and the performance of the robust filter is worse than the traditional EKF filter.In addition,when the bandwidth is small,the performance of the Lupin filter may not be satisfactory. On the contrary,when the bandwidth parameter is large enough, it can achieve similar estimation with traditional EKF.

Secondly, we add impulse noise to the pseudo range and pseudo range rate information in satellite navigation. At the 15th second and 40th second,impulse noise with an amplitude of 100 is added to the pseudo range information, and impulse noise with an amplitude of 20 is added to the pseudo range rate information. The corresponding RMSEs are listed in Table 4,and the performance comparison of the three filters is shown in Fig. 5.

The impulse interference destroys the assumption that the noise is Gaussian distribution, which makes the performance of the traditional EKF significantly degraded. However, the performance of the remaining two robust filters is better than the performance of the traditional EKF. The algorithm can assign a small weight to the abnormal measurement value.

Thirdly, when the measurement information is abnormally interrupted at the 25th second, the filter estimation performance is shown in Fig. 6.

We further compare the mean calculation time Tamong different algorithms. As shown in Fig. 7 and Table 5, we can find that the mean calculation time of MVC-EKF algorithm is much smaller than that of MCC-EKF algorithm by 34.9%and only slightly larger than that of EKF algorithm by 8.1%.However,the MVC-EKF algorithm can maintain a similar estimation accuracy as the MCC-EKF algorithm.

Because the measurement information is interrupted intermittently, the assumption that the noise is Gaussian dis-tributed is destroyed. The performance of the traditional card EKF algorithm is drastically degraded,and the two robust filters can be reliably estimated with high accuracy.

Table 2 Value of the mean square error matrix of initial state estimation.

Table 3 RMSEs of position, velocity and attitude angle under Gaussian noise conditions.

Table 4 RMSEs of position, velocity and attitude angle under impulse noise conditions.

Fig. 5 Navigation RMSE results under impulse noise.

Fig. 6 Navigation RMSE results when measurement information is interrupted intermittently.

Fig. 7 TCost results of different algorithms.

Table 5 Improvement of MVC in Tcost as compared to other algorithms.

5. Conclusions

In this paper, an improved nonlinear Kalman filter algorithm called MVC-EKF was proposed. The algorithm was derived by using the maximum versoria criterion as the optimality criterion,instead of using the well-known minimum mean square error criterion. The performance comparisons of the MVCEKF with other Kalman filter algorithms including EKF and MCC-EKF were demonstrated by SINS/GNSS integrated navigation system simulations. The results show that the proposed MVC-EKF algorithm exhibited stronger robustness than other Kalman filter algorithms during nonstationary measurement process.In the future,we can consider adjusting the parameters of the function adaptively,so that the filter can further improve the estimation accuracy of the system under the assumption of Gaussian noise and non-Gaussian noise.

The authors declare that they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper.

This study was co-supported by the National Natural Science Foundation of China (No. 62073264), the Key Research and Development Project of Shaanxi Province, China (No.2021ZDLGY01-01 and 2020ZDLGY06-02),National Natural Science Foundation of China(No.61803309),China Postdoctoral Science Foundation(No. 2018M633574) and the Aeronautical Science Foundation of China (No. 2019ZA053008).