Maximum correentropy-based robust Square-root Cubature Kalman Filter for vehicular cooperative navigation

As the core method of cooperative navigation, relative positioning plays a key role in realizing intelligent vehicle driving and vehicle self-assembling network collaboration algorithms. However, when the contamination rate of measurement noise is high, the performance of filtering will be seriously affected. To better address the filtering performance degradation problem due to noise contamination, this paper proposes a vehicular cooperative localization method based on the Maximum Correentropy Robust Square-root Cubature Kalman Filter (MCSCKF). The algorithm not only retains the advantages of Square-root Cubature Kalman Filter (SCKF) but also has strong robustness to non-Gaussian noise. The experimental results of tightly integrated vehicular cooperative navigation show that compared with the Extended Kalman Filter (EKF) and Cubature Kalman Filter (CKF), the localization accuracy of MCSCKF is improved by 35.08% and 31.83%, respectively, which verified the effectiveness in improving the accuracy and robustness of the relative position estimation.

With the rapid development of intelligent transportation systems, vehicular positioning technology has become an important research field.Relative position sensing is not only the core of cooperative localization but also the key technology of intelligent vehicle driving 1 .Global Navigation Satellite System (GNSS) such as Global Positioning System (GPS) and BeiDou Navigation Satellite System (BDS) are becoming increasingly mature and have been successfully applied to vehicle positioning.However, due to the complexity of our surrounding built environment, road conditions, and vehicle traffic, the integration of multi-satellite navigation systems increases the accuracy error to a certain extent and affects positioning performance.To solve such problems, wireless networks have facilitated the development of cooperative positioning in Vehicular Ad Hoc Networks (VANETs) 2 .
Xu et al. 3 proposed a method to obtain the relative position based on the Doppler shift, but it is infrastructuredependent to be realized.Alam et al. 4 proposed a Doppler-based cooperative positioning method for vehicular networks with GPS availability, proposing the use of Doppler shifts based on dedicated short-range communication (DSRC) signal carriers to improve GPS accuracy.The tight integration approach was proposed, taking into account this and emerging vehicular communication technologies, a method was proposed to improve relative positioning between two vehicles within a Vehicle Ad Hoc Network, fusing available low-level GPS data 5,6 .Feng Shen et al. proposed a new vehicular collaboration method, a novel tight cooperative positioning method based on the distance measurements of ultra-wideband (UWB), for relative positioning in new intelligent transportation systems.The method shares GPS pseudorange and Doppler shift measurements between participating vehicles, and then, each vehicle fuses GPS measurements and UWB-based distance to obtain relative position to avoid collision and improve driving safety 7 .
Although the accuracy of the relative distance estimation scheme for cooperative vehicle localization using a tight integration of GPS underlying data and UWB is improved, the data processing method used in the above technique is Extended Kalman Filtering (EKF).This method directly approximates the Gaussian integral with Taylor-expanded truncation, which can only achieve first-order accuracy and has a simple structure for systems with low nonlinearity.However, for tightly integrated GPS/UWB integration, the observation model has strong nonlinearities, since the EKF ignores higher-order terms, it will greatly reduce the filtering accuracy and may even cause divergence when the nonlinearity is high or the initial error is large.Therefore, the EKF needs to be improved and optimized.
To solve the nonlinear problems, the unscented Kalman filter (UKF) and the cubature Kalman Filter (CKF) are commonly used 8,9 .The unscented Kalman filter, first proposed by Juiler et al., was developed on the basis of

OPEN
School of Geomatics, Liaoning Technical University, Fuxin 12300, Liaoning, China.* email: zxt16205@163.comthe unscented transformation (UT) 10 .The elementary concept of UT is a method to compute the statistical properties of random variables that have been nonlinearly transformed, which approximates the a posteriori mean and variance of a nonlinear function by obtaining a set of sigma points through a certain sampling strategy and setting the corresponding mean weights and variance weights.The UKF approximates the statistical properties of the random quantity with a finite number of parameters, in other words, it conveys the statistical properties of the random quantity with a set of accurately chosen sampling points mapped by a nonlinear model, which fully reflect the true mean and covariance of the Gaussian density.The mean and covariance of the random quantities are then estimated by weighted statistical linear regression.The UKF does not introduce linearization error, thus it can reach the second-order accuracy of the Taylor series expansion, and there is no need to compute the Jacobian matrix.Therefore, it can be easily applied to the estimation of the state of a nonlinear system 11,12 .Driedger et al. 13 found that EKF and UKF were applied to evaluate the feasibility of optical navigation based on resident space objects, and experiments showed that UKF was more reliable than EKF.Deori et al. 14 used EKF and UKF to design and test the benchmark cart pendulum system and the underactuated offshore boom crane system, and the results showed that the accuracy of UKF was higher than that of EKF.However, some statistical properties of the Sigma points for the a posterior distribution of the nonlinear function are lost when the system dimension is higher, which can degrade the system estimation accuracy 15,16 .
In response to the above issues, Arasaratnam et al. 17 proposed CKF, an algorithm based on the third-order spherical-radial cubature rule, which is based on the a priori mean and covariance.The Sigma cubature points are selected by the cubature rule, then these cubature points are passed through a nonlinear function, and then the cubature points after the nonlinear function are passed through are weighted to deal with the approximation of the state a posteriori mean and covariance.Because the filtering process of CKF also needs to conduct the decomposition and inverse of the error covariance matrix, it is necessary to ensure the positive characterization of the error covariance matrix.However, in practice, it is often difficult to ensure the positive definiteness.Thereby the Square-root Cubature Kalman Filter (SCKF) is proposed 18 , which directly updating the recursion in the form of the square root of the covariance matrix not only reduces the computational complexity and improves the efficiency, but also ensures the positive characterization of the covariance matrix and effectively avoids the divergence problem of the filter.
For the filtering problem under non-Gaussian conditions, many scholars have proposed a number of robust methods.Particle filtering (PF) and its improved algorithms use particles to deal with non-Gaussian noise issues 19,20 .These particle-based filtering methods use a large number of particles to approximate the probability distribution of the state and therefore also suffer from high computational complexity.Huber-based Kalman filtering is another popular method in recent years that uses a maximum likelihood regression criterion to deal with problems caused by non-Gaussian noise 21 , Tseng et al. applied their fusion with CKF to GPS navigation processing, and the results showed that the problem of contamination of measurements due to outliers or deviations from the assumption of Gaussian distribution, as well as the problem of contamination of signals by non-Gaussian noise or outliers, has been greatly improved 22 .Nevertheless, Huber-based methods usually select measurements that contain large errors, which may lead to non-negligible errors in the filter.
In the current state, the maximum correntropy criterion (MCC) has been introduced in the filter to deal with the problems caused by non-Gaussian noise 23,24 .The maximum correntropy Kalman filters 24 are mainly applied to linear systems, and the maximum correntropy unscented Kalman filters 23,25 are extended for solving some nonlinear problems.Nonetheless, they are not applicable to high-dimensional nonlinear systems.Therefore, this paper proposes a new nonlinear filter, Maximum Correntropy Square-root Cubature Kalman Filter (MCSCKF) based on the use of MCC to change the measurement update process of SCKF.This method not only has the advantages of SCKF as well as the overall line of thought, but also has strong robustness to non-Gaussian noise.Based on the work of Shen et al. 7 , experiments on vehicular cooperative navigation with a tight integration of GPS/UWB are conducted to further improve the performance of cooperative localization and enrich the research field for nonlinearities in practical applications of vehicle cooperative navigation.The experimental results show that MCSCKF is suitable for high-dimensional nonlinear systems, and the algorithm improves the estimation accuracy of the relative position.
The rest of the paper is organized as follows.Firstly, "Measurement and system modeling" introduces the measurement and system model for tight integration.Secondly, "Filtering algorithms" provides a preliminary introduction to MCC and SCKF.Then MCSCKF along with this algorithmic flow is derived in "MCSCKF algorithm".After that, "Experimental analysis of on-board results" conducts on-board experiments and analyzes the results.Finally, "Conclusion" concludes the full paper with a summary and an outlook.

Measurement and system modeling
As shown in Fig. 1, only the relative motion of vehicle a and vehicle b in the vehicular ad hoc network is con- sidered for convenience, where the GPS and UWB observations are fused in the process of tight-integration cooperative localization.Vehicle a obtains the pseudo-range(ρ b ) and Doppler shift(ϑ b ) of vehicle b through UWB communication, then combines the local pseudo-range(ρ a ) and Doppler shift ( ϑ a )for double difference.At the same time, the UWB measures the distances of the two vehicles and carries out the data fusion process using MCSCKF to realize the relative localization of the vehicles.

GPS observations
The pseudorange between the receiver a and the satellite s at the moment t is defined as the following equation: where ρ s a (t) is the pseudorange between the receiver a and the satellite s at time t ; R s a (t) is the true distance between satellite s and receiver a ; δ a (t) is the clock difference of receiver a ; d s contains the satellite bias, atmos- pheric delay error, and other common errors of satellite s ; and ζ s a (t) includes the thermal noise, multipath prob- lem, and other non-disclosed systematic errors of receiver a associated with satellite s.
When receivers a and b observe both satellite s and satellite j , the pseudorange values derived from Eq. ( 1) can be eliminated by performing a pseudorange double-difference that eliminates the receiver's clock difference as well as other common satellite errors to obtain the following equation: Since the actual distance between the receiver and the satellite is about 20,000 km, and the GPS positioning error is negligible within a few tens of meters under these conditions, the apparent distance vector can be obtained by rough position estimation and a priori satellite ephemeris.
Similarly, the double differenced Doppler shift between receiver a and receiver b to satellite s and satellite j at the moment t can be defined as: where ⇀ v ab is the relative velocity vector of receiver a and receiver b , is the wavelength of the GPS L1 signal, and γ sj ab is the double differenced residual of the Doppler observation noise of receivers a and b with respect to satellite s and satellite j.

UWB observations
UWB is a communication technology that uses narrow non-sinusoidal pulses of nanoseconds and microseconds to transmit data over short distances with a high transmission rate, low transmission power, and high penetration capability.The principle of its distance estimation is to estimate the distance by the signal propagation time between the base station and the target carrier, and to use the product of the arrival time of the UWB signal of the target carrier measured by the base station and the propagation speed as the relative distance between them.When using UWB for ranging, it is necessary to have measurement information from at least three base stations at the same time.
In the work of Shen et al. 7 , it is assumed that there is no non-line-of-sight (NLOS) problem for UWB between two vehicles, and the distribution law is an approximate Gaussian distribution function.To verify the specific performance of the MCSCKF, the actual measured distance across the UWB transceiver from two vehicles in 7 is used.

GPS/UWB tightly coupled system modeling
The system equation is defined as: where τ is the observation period; X is the state vector; F is the state transition matrix, G is the process noise model, and D is the relative acceleration noise, which obeys a Gaussian distribution law with zero mean www.nature.com/scientificreports/and standard deviation of σ along each coordinate axis.Define the covariance matrix of the process noise as Q = σ 2 GG T , for receiver a and receiver b , with the following equation: in which I n is an identity matrix of n × n.
The observation equation for the relative localization of this experiment can be defined as: where y is the observation vector, which includes the GPS pseudorange, the Doppler shift, and the actual distance between receivers a and b based on the UWB measurements; h is a nonlinear function, which is derived from Eqs. ( 4) and ( 5) as well as the true relative distance between the two vehicles Rab = − → r ab T − → r ab ; if the number of visible satellites of receivers a and b is m + 1 , under this condition, the observation vector y and the measurement noise ζ are expressed as follows: Assuming that the observations are independent of each other, the measurement noise covariance matrix can be defined as the following equation: r are the variance of the pseudorange, the Doppler shift, and the UWB measurement error, respectively, it follows that.
where 1 is denoted as a matrix in which all elements are 1.

Maximum correntropy criterion
Given two random variables X ∈ R, Y ∈ R , assume that their joint distribution function is F XY (x, y) ; the entropy of correlation between the two is usually defined as: where E[•] denotes the expected value and κ(•, •) is the Mercer kernel function, in this paper, we choose the Gaussian kernel as the kernel function of entropy, denoted as follows: here, e = x − y , σ represent the kernel bandwidth while σ > 0.
In practice, the joint distribution function F XY (x, y) is often unknown and the number of available data samples is limited.Therefore, we often use the sample mean estimator to estimate the correlation coefficient: where e(i) = x(i) − y(i) , and {x(i), y(i)} N i=1 , denote the N sample data drawn from the joint distribution function F XY (x, y).
Using the entropy value as a cost function has a strong suppression effect on non-Gaussian noise.If a column of error data {e(i)} N i=1 is obtained, the MCC-based objective function is denoted as: Square-root Cubature Kalman Filter Square-root Cubature Kalman Filter (SCKF) is an effective method to solve the state estimation problem of nonlinear systems by transferring the square root form of the error covariance.In this paper, it can not only Vol.:(0123456789)  6) and ( 8) as an example, the steps of time update and measurement update for SCKF are shown below: Time update.Suppose that at time k , S k−1|k−1 is the square root of the covariance matrix , respectively.Evaluate the cubature points: where here, ξ denotes the n × n-unit matrix, [1] i denotes the i -th column vector.
Dissemination of cubature points: Estimate the square root of the a prior state and the corresponding covariance matrix: where Tria(•) denotes the QR decomposition of the matrix, X * k|k−1 from ( 23): Measurement update.Evaluate the cubature points: Dissemination of cubature points: Estimate the square root of the a priori measurements and the corresponding covariance matrix: here Y k|k−1 is denoted as shown below: Calculate the inter-correlation covariance matrix: with X k|k−1 in the equation: Calculate the Kalman gain: Estimate the square root of the a posterior state and the a posterior covariance matrix:

MCSCKF algorithm
Due to the excellent performance of correntropy in non-Gaussian noise environments 26 , we combine MCC with SCKF and use MCC to improve the robustness of SCKF.
First, from the nonlinear model described by Eqs. ( 6) and ( 8), the a priori estimated states and the corresponding square root covariance matrices are evaluated by Eqs. ( 18)- (23).
Then, the nonlinear regression model is constructed by combining Eqs. ( 8), (21), and ( 22) as follows. where , given by the square root of the covariance matrix ϕ k : Equation ( 34) is obtained by multiplying both sides of B −1 k simultaneously: the specific expression in the above equation is as shown below: Based on the above model, the cost function MCC is constructed as: where e i,k = d i,k − g i,k , and here d i,k , g i,k are the i − th component of D k , g(x k ) , respectively.Then the x k optimal estimate based on MCC can be obtained from the following equation: Let the first order derivative of the cost function is equal to zero, then we can derive: here, ψ e i,k = G σ e i,k • e i,k .Then, we define C i,k = ψ e i,k e i,k =G σ e i,k , and there it is: where According to (41), (40) can further be denoted as: In fact, the key to improving SCKF performance using MCC is to use C k to update the state covariance and the variance of the measurement noise.
The definition of k is the updated covariance matrix, denoted as follows: For the next derivation, we write k in block matrix form, such that: in fact, we can derive x,k 0 0 y,k EKF, classical CKF, and improved MCSCKF are compared in this experiment to analyze the data fusion and draw conclusions.
As shown in Fig. 3, the number of visible satellites needs to be greater than 4 satellites in all other common experiments.In the occasional case of less than 4 satellites, this leads to a decrease in the Kalman filtered innovation and the covariance matrix dimension of the observations, which results in the measurement update not being performed properly, and in order to ensure that the measurement update is performed properly, the Kalman filter can compensate for this by using the dynamic models of the system, setting the innovation of missing observation to zero, and the observation covariance matrix is changed to infinity as a consequence.Figures 4, 5, 6 and 7 show the resulting three-axis error as well as the overall error, respectively.
As shown in Fig. 7, EKF and CKF have basically the same position estimation accuracy, while the positioning accuracy of MCSCKF is significantly better than the two, which utilizes a nonlinear system to transmit the cubature law, and at the same time integrates the rules of Maximum Correntropy Criterion and Square-root Cubature Kalman Filter to inhibit the influence of colored noise in the actual measurements on the experimental data, and accordingly, it can be concluded that the precision, accuracy and robustness are better than EKF and CKF. Figure 8 represents the error cumulative distribution function (CDF) of the three different filters in the experiment, which shows that the applicability of MCSCKF is significantly better than that of the other two Kalman filters.
On this basis, in order to better analyze the effect of different filters on the position estimation accuracy of the experiments, the quality of the estimation is expressed using the Root Mean Square Error (RMSE).For further analysis, the accuracy ( e a ) and precision ( e p ) of the relative position estimation are also defined.
The quantitative results of the three can be expressed using the following equation:     where ⇀ r ′ and ⇀ r are the relative position estimated by the algorithm and the reference relative position of the RTK, respectively, m is the total number of calendar elements, ii is the ith value in the diagonal of the matrix, with The performance metrics of the three filtering algorithms are shown in the following Table 1.
Define the parameter ω to be used to indicate the enhancement effect of scheme B on the realization of scheme A, where ErrorA and ErrorB are the values of the error metrics for the three performances in Table 2.   www.nature.com/scientificreports/Using the above equation, the percentage increase of MCSCKF over the other two classical filtering algorithms among the three performances is calculated as shown in Table 2: Based on the above data, experimental results can be derived, which show that the relative position perception of MCSCKF has stronger robustness and localization accuracy than EKF and CKF, and also provides a more accurate system control method for tight combination cooperative navigation.

Conclusion
Based on the theory of robustness and adaptability, the data processing method of vehicle relative position localization is optimized, which further improves the vehicle positioning accuracy and the performance of cooperative navigation.Moreover, in response to the problem that the covariance matrix dimension of the Kalman filter is reduced due to the new information and observations of the Kalman filter, which leads to the failure of the measurement update, a Maximum Correentropy-based Robust Square-root Cubature Kalman Filter is proposed, which improves the data fusion method, and further improves the accuracy of vehicle relative positioning, and also improves the performance of cooperative navigation.further improves the accuracy of relative vehicle localization, and also opens up a new field for cooperative navigation at the same time.The in-vehicle navigation experiments yielded that the positioning accuracy and robustness of MCSCKF improved by 20.74% and 35.08% compared with EKF; the positioning accuracy and robustness of MCSCKF improved by 20.08% and 31.83%compared with CKF, thus verifying the robustness and superiority of MCSCKF.However, the algorithm proposed in this paper also has a number of limiting factors, which depend on the actual situation problems in engineering, and these problems also limit the method to have limitations in complex applications in practice.This point is also the subject of subsequent research work.
where ρ sj ab (t) is the double differenced pseudorange between the receiver a and receiver b to two satellites s and j , R sj ab (t) is the double differenced geometric distance from two receivers to two satellites, ζ sj ab (t) is the error of receiver and satellite that can not be eliminated by the double difference.where R sj ab (t) can be defined as: where ⇀ µ s and ⇀ µ j are the unit observation vectors from receiver a (or receiver b ) to the two satellites s and j , respec- tively, and ⇀ r ab is the relative position vector of receiver a and receiver b .Substituting Eq. (3) into Eq.(2) yields:

Figure 2 .
Figure 2. Schematic of the experimental settings.

Figure 3 .
Figure 3. Number of commonly visible satellites.

Figure 7 .
Figure 7.Comparison of three-axis distance errors.

Figure 8 .
Figure 8.Comparison of error cumulative distribution functions.

Table 1 .
Quantitative indicators of experimental results.

Table 2 .
Experimental results and performance improvement.