Vehicle dynamics prediction via adaptive robust unscented particle filter

Accurate knowledge of the vehicle dynamics response is a critical aspect to improve handling performance while ensuring safe driving at the same time. However, it poses a challenge since not all the quantities of interest can be directly measured due to cost and/or technological reasons. Therefore, combining the principle of robust filtering and unscented particle filtering algorithm, a filter estimation method of vehicle state is proposed to estimate driving state parameters of a vehicle. The adaptive robust unscented particle filter (ARUPF) is used to realize the longitudinal and lateral velocity as well as the side slip angle of the vehicle. The CarSim and Matlab/Simulink co-simulation platform is established to verify the estimation algorithm. The results show that based on the adaptive robust unscented particle filter algorithm, the vehicle driving states can be estimated, the measurement parameters can be effectively filtered, and the estimation accuracy is higher.


Introduction
In recent years, since the development of computer industry technology, communication technology and other related fields, the automobile control system has become more and more complex. And it can simultaneously receive more sensor information and make timely and reasonable control decisions. For a complete vehicle control system, accurate and reliable vehicle state information is an important support for vehicle motion planning and dynamic control, and the accuracy of vehicle state information will directly affect the quality of vehicle motion control. However, some vehicle driving states, especially the side slip angle of the vehicle, are difficult to measure directly by on-board sensors. Therefore, in order to reduce production and design costs, it is necessary to design a reliable vehicle driving state estimation method to replace physical sensors by soft measurement. 1 At present, there have been many studies on the estimation method of vehicle driving state, among which, the model-based vehicle driving state estimator is the most common method. Researchers usually design a vehicle state estimator based on a recognized vehicle model and use the corresponding estimation algorithm to design a vehicle state estimator. And the existing readily available sensor information is used as the input of the estimator to calculate the required vehicle state information. And also, many advanced estimation theories have been applied in vehicle driving state observation, including sliding mode algorithm, neural network algorithm, particle swarm algorithm, Kalman filter algorithm, and so on. Among them, the Kalman filter and its improved algorithm are widely used, and have achieved good estimation results. The extensive application of advanced vehicle dynamics control system provides good handling performance for vehicles and improves the safety of the driving process greatly. For the purpose of applying simpler, more precise and intelligent control to the vehicle, the control unit should be able to collect more and more precise parameters. Using limited sensors and an effective dynamic model, the parameter estimation method is used to obtain as many state parameters as possible with the required accuracy, which can not only accurately judge the state changes during the driving process of the vehicle, but also improve the robustness of the control system. 2 The problem of vehicle state estimation has been widely studied. A brief review is presented in what follows.
Based on the velocity vector observations, Zhang et al. 3 proposed an optimal mounting parameter estimation scheme. Zheng et al. 4 proposed and performed a piecewise linear frequency domain identification method relying on finite impulse response models to tackle the nonlinearity issue in the signal estimation problems. Anandhalli et al. 5 proposed a method to detect vehicles violating traffic rules especially overspeeding. Herlambang et al. 6 designed a H-infinity method for the state estimation of vehicle position. Tsanakas et al. 7 proposed a novel approach for generating VVT, which facilitated their use in emission modeling. Cai et al. 8 demonstrated and opened for academic research six sets of electric vehicle data. Zhang et al. 9 formulated the heading constraint using position and velocity components in the base state for a circular trajectory and constructed pseudo-measurements to incorporate the heading constraint into the estimators. Tian et al. 10 proposed a method to realize a comprehensive evaluation of wheel dynamics state by information fusion. Based on object-oriented analysis, Kemsaram et al. 11 presented a design model of a stereo vision system for cooperative automated vehicles. Zhang et al. 12 proposed a distributed bearing-based formation control scheme to extend the application domain. Yang et al. 13 adopted a scheduler to determine which systems to be observed and which sensors to access the network. Rafatnia and Mirzaei 14 proposed a new method for stability controller design of vehicle state estimation. Yang and Li. 15 presented sufficient and necessary design conditions for achieving the attacks. Zeng et al. 16 proposed a Kalman Filter-based scheme to evaluate the road roughness indirectly. Chen et al. 17 proposed a neural network method to identify a vehicle system. Long et al. 18 carried out an estimation method in order to solve the problem that key state parameters such as mass-center sideslip angle of the vehicle could not be directly detected in the torque control of four hubmotor driving vehicles. Wang et al. 19 proposed an adaptive robust UKF based on the Sage-Husa filter and the robust estimation technique for AUV acoustic navigation. Liu et al. 20 proposed a filter estimation method of vehicle state based on the principle of robust filtering and the unscented particle filter algorithm in order to solve the problem that the measured values of key state parameters such as the lateral velocity and yaw rate of the vehicle.
To sum up, the above literatures had used a variety of methods to identify states and parameters of vehicles effectively, but some of the methods used were difficult to achieve effective estimation for systems with many parameters and strong nonlinearity. Some researchers did not implement the algorithms in onboard real-time systems. In addition, when estimating vehicle parameters, less vehicle states and parameters were considered. For some researchers, the usability of the proposed model should be improved. In the standard particle filter algorithm, the UPF algorithm based on robust adaptive factor is used for particle update. On the basis of the UKF algorithm, the improved unscented transformation algorithm is used to transform the vehicle state transfer function and sensor perception function. The robust estimation principle and adaptive filtering are organically combined. And the state noise covariance matrix is adjusted as an entirety through the adaptive factor to reduce the influence of abnormal state information of the vehicle.
The traditional particle filter algorithm has the defect of particle degradation in the iterative process, resulting in waste of computing resources and low accuracy of estimation results. In order to solve the above problems, the filtering algorithm is often optimized by increasing the number of particles, resampling, and selecting a reasonable suggested density function. Adaptive robust traceless particle filter algorithm uses the traceless transformation algorithm to calculate the mean and covariance for each particle and establish a reasonable suggested density function, and combines the robust filter estimation algorithm to automatically adjust the gain matrix and system variance. So that the sample point distribution is well consistent with the maximum likelihood function.
The ARUPF algorithm has the advantages of robust estimation and traceless particle filter. After selecting the important density function by using the traceless transformation, equivalent weight function and adaptive adjustment factor, the importance of particles is sampled. And the sensor sensing vector is used reasonably to effectively suppress the pollution of state parameters and the abnormal disturbance of sensor sensing value, reducing the degree of particle degradation in the process of vehicle state estimation and overcoming the shortcomings of single filtering algorithm. The dynamic equation of the 3-DOF vehicle model is as follows:

3-DOF vehicle model
where, u and v are the longitudinal and the lateral speed; v r is the yaw rate, a x and a y are the longitudinal and lateral acceleration; I z is the moment of inertia around the z axis of the vehicle; a and b are the distances of front and rear axles from the center of gravity; m is the vehicle mass; d is the front steering angle; c f and c r are the synthesized stiffness of front and rear tire.
The side slip angle of the center of mass is: Tire model The force between the tire and the road surface (longitudinal and lateral force) has an important impact on the driving safety of the vehicle. The lateral forces of front and rear wheels can be expressed as 21 : where c f and c r are the lateral stiffness of the front and rear tires. a f and a r are the front and rear slip angles.
Adaptive robust unscented particle filter algorithm

Initialization
When k = 0, the state variable x 0 and the error variance matrix and P 0 are initialized as follows: Then the mean and variance of the augmented vector x a are: where R v and R n are the variances of process noise and perceptual noise respectively. The superscript a indicates that the random variables of the process and perceptual noise are expanded into the state variables. In general, e À4 ł a ł 1.

Filter execution
Calculation of the sigma point. The UKF algorithm is used to update particles and calculate the Sigma points.
where i = 1, 2, Á Á Á , L. L is the number of sampling points, and L = 2n + 1; n is the dimension of the state variable.
(1) Time domain updating : ð12Þ where the scale parameter l = a 2 (L + k) À L. b is the prior knowledge parameter containing the x distribution.
The value of b is generally based on prior knowledge (In special cases, for the Gaussian distribution, b = 2).
(2) Perception updating The measurement covariance is predicted.
The state and measurement covariance are predicted.
The Kalman gain is: (3) The ARUPF algorithm It is set thatx k is the vehicle state estimation vector; P = diag( P 1 , P 2 , Á Á Á , P k ) is the equivalent weight matrix; ∂ k is the adaptation factor of the equivalent weight function; p x k is the weight matrix of x k ; p k is the equivalent weight matrix of the perception vector Z k ; A k is the sensor perception design matrix.
The IGG (Institute of Geod-esy & Geophysics) method is used to generate the equivalent weight function: where V k = A kxk À z k is the residual vector of the perception value z k of the sensor; the reference value range of the adjustment factor is K 0 2 (1, 1:5); the reference value range of the adjustment factor is K g 2 (3, 8).
The principle of the adaptive robust filtering is: where r is the continuous nondecreasing convex function; p i is the ith diagonal component of the weight matrix of the perception vector. Equation (23) can be obtained by addressing extreme value for equation (22): The adaptive factor ∂ k is constructed as: where a k is an adaptive factor; the reference value range of the adjustment factor c 0 is c 0 2 (1, 1:5); the reference value range of the adjustment factor c 1 is c 1 2 (3, 8); tr is the trace operator for matrices.
Both the weight function and the adaptive factor are important adjustment factors, and the structure is similar. The weight function is obtained from the judgment residual. The adaptive factor is judged and selected by the difference Dx k between the state estimated value and the predicted value. The Kalman gain and mean as well as state covariance of the sampled particle are calculated using the equivalent weights and the adaptive factors as follows: From equations (25)-(27), it can be seen that when the motion model is abnormal, the adaptive factor ∂ k decreases, which reduces the interference of the abnormal motion prediction model. When the perception model is abnormal, the element of the equivalent weight matrix P z k , z k decreases. It makes the importance density function be closer to the actual distribution and weakens the influence of abnormal sensor perception information on pose estimation. If the equivalent weights P z k , z k = P z k , z k and ∂ k = 0, x Ã i, k and P Ã i, k are the sample mean and variance obtained by the UKF algorithm.

Calculation of the weight
The resulting weight is then normalized.
Resampling. On the premise that the total number of particles remains the same as N, the particles with low weight in the particle set are eliminated. And the supplementary particles are derived from the particles with higher weight by the residual sampling method.
The estimation formula 1= Then the obtained result is compared with the established threshold to judge the severity of particle degradation. The smaller of value of the estimation formula (w i, k ) 2 is, the more serious of the degradation becomes. In this case, the posterior density obtained above can be resampled, getting N new particles again.
The weight of each sample point after re-sampling is set as 1/N: Filter output A set of weighted particles is output. And the estimated value of the vehicle state is calculated based on the information of these particles.
Since the equivalent weight and adaptive factor are used to optimize the particle distribution after unscented transformation when selecting the importance density function, a better sampling distribution function is provided for the particle importance sampling process.

Numerical simulation
The Carsim and Matlab/Simulink software are used to conduct co-simulation experiments to verify the estimation algorithm. The Carsim software specially builds various vehicle dynamic simulation models according to vehicle dynamics characteristics. The simulation performance of the models is close to that of real vehicles. The simulation results are used by many international automakers and parts suppliers. Some of its operating parameters as vehicle sensor measurement parameters are input into the Simulink simulation software. The vehicle dynamics model and the ARUPF algorithm are established in the Simulink software to estimate the states of the vehicle. And then the estimated values are compared and verified with the vehicle model in the Carsim software.
The key parameters of the vehicle are shown in Table 1. The B-type vehicle model is selected in the Carsim, and the ISO double lane change test road is used as the simulation condition.
Based on the ARUPF algorithm, some parameters of the Carsim vehicle model are taken as the real value of the vehicle state measured by the sensor, and input to the Simulink vehicle dynamics model to carry out the state estimation. Figure 2 shows the known input information, and then the ARUPF algorithm can achieve effective estimation of vehicle motion state parameters. Figure 3 shows the simulation and comparison results of the key variables simulated with the different methods (ARUPF and UKF) for passing a double lane change road maneuver.
It can be seen from Figure 3. that the real value output by Carsim software is close to the ARUPF state estimation value of the simulation dynamics model  built in Simulink, which verifies the correctness of the proposed algorithm. The estimated value of the UKF is prone to large estimation errors in the later stage. The ARUPF state estimation algorithm used can effectively solve the problem of accuracy degradation caused by sample depletion of the traditional particle filter algorithm. And its state estimation accuracy is better than that of the UKF algorithm. The proposed ARUPF filter has better filtering effect on process noise and better adaptability as well as anti-interference ability. The estimation effects of different algorithms (ARUPF and UKF) are then compared under the condition of tracking a double lane change road. From the curves of the longitudinal vehicle speed and longitudinal vehicle speed error in Figure 3(a) and (b), it can be seen that compared with the UKF algorithm, the ARUPF is better in terms of accuracy, stability, convergence and real-time performance. Figure 3(c) indicates that for the lateral speed, the ARUPF algorithm is overall better than the UKF method and has better convergence. As shown in Figure 3(d), it can be clearly seen that the ARUPF has higher overall estimation accuracy than UKF, and has good stability and convergence. From Figure 3(e), it can be seen that the overall slip angle estimation accuracy of the ARUPF algorithm is higher and has better convergence relative to the UKF method.
Then the root mean square error (RMSE) and average absolute error (MAE)of the estimation value relative to the virtual test value are given to verify the accuracy of the proposed algorithm.
From Table 2, it can be seen more intuitively that the estimation accuracy of the ARUPF algorithm is significantly higher than that of the UKF algorithm.

Experimental verification
A real-vehicle fishhook test is carried out to verify the effectiveness of the proposed ARUPF algorithm. The relative measurement equipments are shown in Figure 4.  The detailed description of the sensors including the brand and the performance parameters is shown in Table 3. Figure 5 is the comparison result of the estimated and test values.
From Figure 5, it can be seen that the trends of the simulated and experimental values are consistent indicating the correction of the proposed algorithm. This is because that the ARUPF algorithm has the advantages of robust estimation and unscented particle filtering. After comprehensively selecting the important density function by using unscented transformation, equivalent weight function and adaptive adjustment factor, the importance sampling of particles is carried out, and the sensor perception vector is used reasonably. It effectively suppresses the pollution of state parameters and suppresses the abnormal disturbance of sensor perception values, reducing the degree of particle degradation in the process of estimation overcoming the shortcomings of a single filtering algorithm. The experimental results show that the ARUPF algorithm has strong adaptability and fault tolerance to unknown noise and perceptual anomalies. And the obtained filter value is closer to the real distribution, which improves the positioning accuracy of the vehicle state estimation.

Conclusions
Based on a 3-DOF dynamic model of electric vehicle and the magic formula tire model, the estimation of longitudinal and lateral velocity as well as side slip angle is established which having the advantages of low    implementation cost, easy engineering and strong antiinterference ability. It provides a new idea for the state estimation of electric vehicles. A new method using equivalent weight matrix to solve the problem of coarse error interference of vehicle sensor data is presented. The IGG method is used to generate equivalent weight function. And through adaptive adjustment of the weight matrix, it can effectively suppress random errors caused by nonlinear factors in the process of vehicle sensor detection, reducing the influence of data distortion caused by interference, and improving the accuracy of vehicle state estimation. A new state estimation method of electric vehicle is proposed by using the principle of adaptive robust filtering combined with the unscented particle filtering algorithm. The ARUPF method has the advantages of good noise filtering effect and high precision. A joint simulation platform of Carsim and Matlab/Simulink is established to carry out simulation analysis and verification of the vehicle state estimation. The simulation results show that the vehicle state estimation based on the ARUPF algorithm has the characteristics of high precision, strong anti-interference ability and good stability.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This research was supported by the Science and Technology Program Foundation of Weifang under Grant 2015GX007. The first author gratefully acknowledges the support agency.

Data availability
The related data used to support the findings of this study are available from the corresponding author upon request.