An Adaptive Multi-Dimensional Vehicle Driving State Observer Based on Modified Sage–Husa UKF Algorithm

An accurate vehicle driving state observer is a necessary condition for a safe automotive electronic control system. Vehicle driving state observer is challenged by unknown measurement noise and transient disturbances caused by complex working conditions and sensor failure. For the classical adaptive unscented Kalman filter (AUKF) algorithm, transient disturbances will cause the failure of state estimation and affect the subsequent process. This paper proposes an AUKF based on a modified Sage–Husa filter and divergence calculation technique for multi-dimensional vehicle driving state observation. Based on the seven-degrees-of-freedom vehicle model and the Dugoff tire model, the proposed algorithm corrects the measurement noise by using modified Sage–Husa maximum posteriori. To reduce the influence of transient disturbance on the subsequent process, covariance matrix is updated after divergence is detected. The effectiveness of the algorithm is tested on the double lane change and Sine Wave road conditions. The robustness of the algorithm is tested under severe transient disturbance. The results demonstrate that the modified Sage–Husa UKF algorithm can accurately detect transient disturbance and effectively reduce the resulted accumulated error. Compared to classical AUKF, our algorithm significantly improves the accuracy and robustness of vehicle driving state estimation. The research in this paper provides a reference for multi-dimensional data processing under changeable vehicle driving states.


Introduction
According to the released Global Status Report on Road Safety 2018 [1] by WHO, the annual road traffic deaths reached 1.35 million in 2016. An effective solution towards safe driving is a worldwide problem. The stability-control technique is one of the most important techniques in vehicle active security [2].
Since the 1990s, automotive electronics have developed rapidly. Many methods have been proposed to improve automotive safety, including anti-lock brake system (ABS) based on tire dynamic model [3], traction control system (TCS) based on slip ratio [4], electric stability program (ESP) based on measured rolling angle speed, acceleration, and yaw moment of a vehicle [5]. An accurate vehicle driving state is a precondition for these systems.
However, vehicle driving state cannot be obtained by vehicular sensors and requires additional sensors because of complex vehicle driving conditions, low accuracy, and high cost of vehicular sensors. nonlinear vehicle dynamic model and a simplified Magic Formula tire model. Their method could effectively detect the change in vehicle state errors. To estimate process noise, Qingya Wu et al. [19] applied Sage-Husa sub-optimal maximum posteriori noise estimator to adaptive simplified spherical simplex unscented Kalman filter. Their approach was more efficient than AUKF and effectively improved computational accuracy. To further improve the Sage-Husa algorithm, Gang Li et al. [20] eliminated the estimation of process noise and the error correction term of measurement noise estimation. However, their algorithm was applied in an optimized system and did not consider transient disturbances.
In practical application, vehicle driving states are affected by many factors including working conditions and weather. The prior variables of the measurement noise are unknown. Although UKF and AUKF can track the changing state, the estimation results will lag true values because of the system's internal unchangeable process noise [21]. When working conditions change drastically, the measurement information transmitted to the observer is inaccurate. The disturbance caused by the changing working condition will cause the failure of state estimation and affect the subsequent process. When prior noise statistics are unknown or inaccurate, the Sage-Husa estimator and Sage-Husa maximum posterior have shown their potentialities in estimating the measurement and process noise in various fields [22][23][24][25]. However, the Sage-Husa estimator and Sage-Husa maximum posterior will diverge after being disturbed.
Therefore, we propose an AUKF based on a modified Sage-Husa filter and divergence calculation technique for multi-dimensional vehicle driving state observation. The noise estimator is based on a modified Sage-Husa maximum posteriori estimation. Its purpose is to perform an online estimation on the mean and error covariance of measurement noise. Besides, we use the divergence calculation technique to modify the covariance matrix when a disturbance occurs. To evaluate the practical value of our method, we add a severe disturbance of 0.1 s in the robustness test. The result shows that the modified Sage-Husa UKF algorithm can effectively reduce the impact of disturbances.
The main contributions of our work are shown as follows: 1.
We propose a modified Sage-Husa UKF algorithm. It effectively increases the filtering convergence speed and reduces the influence of transient disturbance on subsequent estimation.

2.
We introduce divergence calculation to detect transient disturbances and solve the divergence problem of Sage-Husa maximum posteriori. Meanwhile, the introduction of divergence calculation technique reduces the amount of calculation. 3.
We apply the modified Sage-Husa UKF algorithm on vehicle driving state estimation. The simulation result proves the robustness of our approach, which provides a reference for multi-dimensional data processing under changeable vehicle driving states.
As for the rest of this paper, Section 2 introduces the seven-degrees-of-freedom vehicle model and the Dugoff tire model. Section 3 introduces the proposed approach of vehicle driving state estimation based on the Modified Sage-Husa UKF algorithm. Section 4 introduces the simulation and robustness test results of our proposed approach. Section 5 concludes the paper.

Models
Our proposed method is based on the nonlinear seven-degrees-of-freedom dynamic vehicle model and the Dugoff tire model. The seven-degrees-of-freedom vehicle model contains all the variables we are concerned with. It was established to connect the measured variables from sensors with the obtained state variables. However, some new state variables were involved in the vehicle model, for example, the longitudinal and lateral tire forces. Since these variables could not be measured directly, the Dugoff tire model was involved to calculate and obtain these variables.

Seven-Degrees-of-Freedom Vehicle Model
In this paper, we apply the estimation based on the seven-degrees-of-freedom vehicle model. The established seven-degrees-of-freedom dynamic vehicle model contains three longitudinal, lateral, and yaw degrees of freedom (DOFs), and four rotational DOFs of the wheels [26]. We can obtain longitudinal and lateral motion including speed and acceleration, yaw rate, side slip angle, and torque from this model. In Figure 1, the ISO coordinate system is set with the mass center of a vehicle as the origin, where the right part of the x-axis and the upper part of the y-axis is positive. The vehicle is longitudinal symmetry about the x-axis. The set of dynamic equations obtained from the model is shown as follows: where m is the total mass of the vehicle; β is side slip angle; u is longitudinal speed; v is lateral speed; ω is yaw rate; a x is longitudinal acceleration; a y is lateral acceleration; Γ is torque around the z-axis; I z is car inertia around z-axis; δ is wheel steering angle; F xi is tire longitudinal force; F yi is tire lateral force; i = 1, 2, 3, 4 represents the wheel at the front left, front right, rear left and rear right; δ i represents the steering angle of the specific wheel responding to the wheel steer angle δ; F w is wind resistance; a, b represent the distance from the mass center to the front axis and the back axis; t f , t r represent the distance of wheels at the front axis and back axis.
Sensors 2020, 20, x FOR PEER REVIEW 4 of 22 The vehicle is longitudinal symmetry about the x-axis. The set of dynamic equations obtained from the model is shown as follows:

Dugoff Tire Model
Pacejka Magic Formula [27], Dugoff model [28], and Burckhardt model [29] is mostly used in these years to predict the behavior of tires. Comparing with the other two models, the Dugoff model is more suitable for UKF, and UKF can perfectly track the road adhesion coefficient. In this paper, we use the Dugoff tire model to simulate tire longitudinal (F x ) and lateral force (F y ). The schematic is shown in Figure 2. In this tire model, the tire longitudinal and lateral forces are closely related to parameters including longitudinal slip ratio λ, tire longitudinal (C x ) and lateral stiffness (C y ). The tire longitudinal and lateral force can be represented by Equations (2) and (3). where and is the current road adhesion coefficient; is the vertical load of each tire; is the introduced boundary condition; ε is the speed influence factor. We obtain a road adhesion coefficient based on [30].
Based on the seven-degrees-of-freedom vehicle model and Dugoff tire model, state equation can be written as: System observation equation can be written as: The tire longitudinal and lateral force can be represented by Equations (2) and (3). where and µ is the current road adhesion coefficient; F z is the vertical load of each tire; L is the introduced boundary condition; ε is the speed influence factor. We obtain a road adhesion coefficient based on [30].
State variable x(t) is represented as Observation variable y(t) is represented as y(t) = a x a y r T . Control input u(t) = [δ] is represented as. Based on the seven-degrees-of-freedom vehicle model and Dugoff tire model, state equation can be written as: Sensors 2020, 20, 6889

of 22
System observation equation can be written as: where k is the discrete time; Λ is the noise driving matrix; X(k) is the state at time k; Y(k) is the observation matrix at time k; W(k) is input white noise; and V(k) is observation noise.

Adaptive Unscented Kalman Filter (AUKF)
Compared to the unscented Kalman filter (UKF), AUKF is adaptive to process noise. UKF algorithm obtains the probability distribution of state variables by unscented transformation (UT). The nonlinear function is approximated based on probability density. The algorithm avoids the linearization of nonlinear systems [31].
The state and observation error equations can be represented as: where x k is the L-dimension state parameter of t k epoch; Λ is the noise driving matrix; W k is system noise matrix; Q k , R k is the covariance matrices of W k , V k . The description of AUKF is shown as follows [32,33]: 1. Sampling points construction. For L-dimension column-vector X, sampling points χ k−1 can be constructed by its estimation valueχ k−1 and variance P k−1 .
where ω i , ω c i are the weights of sampling points. Equations (11)-(13) calculates the one-step prediction of Sigma points, the one-step prediction of system state variables, and the covariance matrix of system state variables. Another set of Sigma points needed to be constructed based on Equation (10).
Equations (14)- (20) calculate the estimated observation variable, the mean of estimated observation variable, the covariance matrix of estimated observation variable, the covariance matrix of true observation variable, Kalman gain, state update, and covariance matrix update. σ k is the average value of noise V k , and is usually assigned to be zero in AUKF.
The innovation is defined as: The real-time estimated covariance matrix of the innovation sequence is calculated as: where P ε(k) is the covariance and M is the scale coefficient. The update of the R k is This process estimates measurement noise and shows adaptiveness.

Divergence Calculation
Suppose there is a system described by the following state-space model where k is the discrete time; φ is the state transition matrix; H is the observation matrix; Λ is the noise driving matrix; X k is the state at time k; Y k is the observation matrix at time k; W k is input white noise; V k is observation noise.
We assume that W k and V k are zero mean; the covariance matrices of W k and V k is Q k and R k ; E W k , V j = 0, where j represents random time.

Innovation is defined as
We haveŶ We can obtain Equation (28) if we substitute Equations (25) and (27) into Equation (26): The estimated variance of the measured value is Then The innovation covariance matrix can be expressed as where P k+1 is the covariance matrix of the predicted error. When the estimation converges, the innovation ε k is white noise that obeys the normal distribution. Hence, we can obtain Equation (32), where ζ xy is the correlation coefficient, and the value is between zero and one. The closer ζ xy approaches to zero, the weaker the correlation.
The correlation coefficient of innovation can be used to judge the divergence. The critical value for identifying the filter divergence in this paper is ζ 0 = 0.4. If Equation (33) is satisfied, the filter diverges.
Therefore, the discriminant formula can be obtained by substituting Equation (31) into Equation (33): Since V k+1 and H X k+1|k are not related, then When ζ 0 = 0.4, there is at least a 97.61% probability for successful recognition, and the error rate is 0.04-2.39%.

Modified Sage-Husa Unscented Kalman Filter
Based on the traditional UKF, Sage-Husa algorithm, and divergence calculation technique, we propose a modified Sage-Husa UKF algorithm. The algorithm is designed to be more applicable in the field of vehicle driving state estimation than classical AUKF. The description of the proposed algorithm is shown as follows:

1.
Sampling points construction and time update. These two stages are the same as the classical AUKF mentioned above.
where ζ 0 = 0.4 is the predetermined correlation coefficient; R k is the covariance matrix of V k . If the filter diverges, we should correct the covariance matrix P k|k−1 [34]: where ρ is within zero to one and assigned to be 0.95. A large ρ will reduce the residual information effect before time k, and strengthen the effect of the current residual vector. If the filter is convergent, then which is the same as classical AUKF.

3.
Measurement update is the same as classical AUKF.
where H k is the transition matrix of the nonlinear equation measuring equation.
only has a small effect on the system and can be ignored.

Experiments and Analysis
The experiments are performed on CarSim and Simulink. We establish a simulation model based on the models and algorithms mentioned in Sections 2 and 3. The Modified Sage-Husa UKF algorithm is tested on both double lane change and Sine Wave road conditions. We focus on five parameters (longitudinal speed, lateral speed, yaw rate, longitudinal acceleration, and lateral acceleration) because these parameters are sensitive to changes and more state variables can be further calculated based on these five parameters. We first present normal simulation tests to verify that the proposed approach can reach or even superior to the performance of classical AUKF. Since our approach mainly aims at the weak robustness and the slow adaptive speed of AUKF, we present a robustness test to compare the performances of two methods under severe disturbances.

Simulation Platform
CarSim released by MSC in 1996 is a vehicle dynamics simulation software. It can simulate the vehicle response to the road, air, and driver behavior in the real driving process. Carsim can simulate three to six times the speed of actual motion and allows users to manually set parameters. It can directly establish a Simulink model, which is friendly with MATLAB users. This paper verifies our proposed approach based on CarSim and Simulink co-simulation. Driving state data is provided by CarSim and the on-line estimation on the modified Sage-Husa UKF algorithm is performed on Simulink.
We first set the road condition as double lane change in Carsim, with a maximum speed of 60 km/h. We then test the performance of the algorithm under Sine Wave road condition, with a maximum speed of 90 km/h. The parameters of the vehicle model are shown in Table 1. We import the simulation data from CarSim to Simulink and co-simulate with the established Dugoff tire model and seven-degrees-of-freedom vehicle model.

Simulation Model
The established simulation model is shown in Figure 3. The output state variables of CarSim is time linear, including wheel steering angle δ, longitudinal acceleration a x , lateral acceleration a y , and yaw rate r. We can calculate the current tire longitudinal and lateral force F xi and F yi by inputting longitudinal and lateral acceleration, yaw rate, wheel steering angle, and estimated longitudinal and lateral speed into the Dugoff tire model. We can obtain current estimated longitudinal and lateral acceleration, yaw rate, and longitudinal and lateral speed by inputting F xi , F yi and current measured accelerate and yaw rate into Vehicle Driving State Observer based on Modified Sage-Husa UKF algorithm. Longitudinal and lateral speed are regarded as output values and are involved with tire force calculation of the next moment. Here we suppose the acceleration only changes a little and the simulation result proves our hypothesis.

Simulation Model
The established simulation model is shown in Figure 3. The output state variables of CarSim is time linear, including wheel steering angle , longitudinal acceleration , lateral acceleration , and yaw rate r. We can calculate the current tire longitudinal and lateral force and by inputting longitudinal and lateral acceleration, yaw rate, wheel steering angle, and estimated longitudinal and lateral speed into the Dugoff tire model. We can obtain current estimated longitudinal and lateral acceleration, yaw rate, and longitudinal and lateral speed by inputting , and current measured accelerate and yaw rate into Vehicle Driving State Observer based on Modified Sage-Husa UKF algorithm. Longitudinal and lateral speed are regarded as output values and are involved with tire force calculation of the next moment. Here we suppose the acceleration only changes a little and the simulation result proves our hypothesis.  The main difference between the classical AUKF vehicle driving state estimation model and our model is the adaptive part. Instead of the classical AUKF algorithm, the proposed Vehicle Driving State Observer is based on the Modified Sage-Husa UKF algorithm.
We compare the proposed Modified Sage-Husa UKF algorithm and classical AUKF based on double lane change and Sine Wave road conditions. The trajectories of the two road conditions are shown in Figure 4.

Simulation Results and Analysis
In the simulation, the initial measurement noise covariance matrix R 0 is The initial process noise covariance matrix Q 0 is The modified Sage-Husa UKF algorithm will adaptively iterate covariance matrix R. Hence the initial condition can be randomly selected within the proper range and the matrix Q must be positive definite.  Table 2.

Double Lane Change Road Condition
Figures 5a-9a demonstrate the AUKF and modified Sage-Husa UKF algorithm estimation results. Figures 5b-9b indicate the errors of two methods compared with true values. We apply RMSE (root mean square error) and MAE (mean absolute error) error evaluation and the results are shown in Table 2.

Double Lane Change Road Condition
Figures 5a-9a demonstrate the AUKF and modified Sage-Husa UKF algorithm estimation results. Figures 5b-9b indicate the errors of two methods compared with true values. We apply RMSE (root mean square error) and MAE (mean absolute error) error evaluation and the results are shown in Table 2.          Figure 14b indicate the errors of two methods compared with true values. We apply RMSE (root mean square error) and MAE (mean absolute error) error evaluation and the results are shown in Table 3.  Figures 10b-14b indicate the errors of two methods compared with true values. We apply RMSE (root mean square error) and MAE (mean absolute error) error evaluation and the results are shown in Table 3.          Our approach is adaptive to the changing system and remains excellent performance when the road condition changes. The initial conditions in Sections 4.3.1 and 4.3.2 are the same. Since the initial matrices R 0 and Q 0 are set under double lane change road condition, AUKF performs undesirable in the Sine Wave road condition. Especially in Figures 13b and 14b, the proposed approach demonstrates its superiority.

3.
Our approach can update the covariance matrix. The introduction of the divergence calculation in our approach effectively improves the adaptivity and anti-interference ability. According to Figure 13b, the error curve of AUKF estimation is rough, which is with changeable extremum. This is caused by the comparatively more extreme road condition, which challenges the sensors and observers. AUKF performs undesirable because of its slow updating of the noise covariance matrix. The involvement of divergence calculation in the proposed method accelerates the process of convergence and performs desirable when the road condition changes.

4.
It can be noticed that the estimation is undesirable in Figure 11a. This is because the Sine Wave road condition under 90 km/h is so extreme that most vehicle states reach their extreme values. Under this road condition, the direction of lateral speed changes with high frequency and small amplitude. The undesirable estimation of tire forces negatively affects the estimation of lateral speed. Nevertheless, the proposed approach tracks the changing trend and keeps convergent, which demonstrates its stability. Besides, it is comparatively unsafe to drive under 90 km/h in a real scenario. The test in Section 4.3.1 indicates that our approach can accurately estimate vehicle driving states under safe driving conditions.

Robustness Test
We simulate the change of measurement error or system error in the driving process. The purpose of this test is to simulate transient disturbances caused by an onboard processor failure in extreme conditions.

Double Lane Change Road Condition
We add a drastic disturbance with a length of 0.1 s at the 6.58th second. In this 0.1 s, longitudinal, lateral speed, yaw rate, longitudinal and lateral acceleration are set to be 15 m/s, 0.2 m/s, 0.2 rad/s, 15 m/s 2 , 15 m/s 2 respectively when the real value is 16.67 m/s, 0.01 m/s, 0.01 rad/s, 0.21 m/s 2 , −0.12 m/s 2 respectively. Figure 15a, Figure 16a, Figure 17a, Figure 18a, Figure 19a demonstrate the AUKF and Modified Sage-Husa UKF algorithm estimation results. Figure 15b, Figure 16b, Figure 17b, Figure 18b, Figure 19b indicate the errors of the two methods compared to the true values. We apply RMSE and MAE error evaluation and the results are shown in Table 4.
driving states under safe driving conditions.

Robustness Test
We simulate the change of measurement error or system error in the driving process. The purpose of this test is to simulate transient disturbances caused by an onboard processor failure in extreme conditions.

Double Lane Change Road Condition
We add a drastic disturbance with a length of 0.1 s at the 6.58th second. In this 0.1 s, longitudinal, lateral speed, yaw rate, longitudinal and lateral acceleration are set to be 15 m/s, 0.2 m/s, 0.2 rad/s, 15 m/s 2 , 15 m/s 2 respectively when the real value is 16.67 m/s, 0.01 m/s, 0.01 rad/s, 0.21 m/s 2 , -0.12 m/s 2 respectively. Figures 15a-19a demonstrate the AUKF and Modified Sage-Husa UKF algorithm estimation results. Figures 15b-19b indicate the errors of the two methods compared to the true values. We apply RMSE and MAE error evaluation and the results are shown in Table 4.      Figure 15b, Figure 16b, Figure 17b, Figure 18b, Figure 19b, the Modified Sage-Husa UKF algorithm and AUKF both converge quickly to the true value after being disturbed. For the states shown in Figure 17a, Figure 18a, Figure 19a which exist measured values, the convergence speed of our approach is much faster than AUKF. In these three situations, our approach has improved convergence speed by 75%, 33%, and 67.2% respectively.

Sine Wave Road Condition
We add a drastic disturbance with a length of 0.015 s at the 10th second.  Table 5.
which exist measured values, the convergence speed of our approach is much faster than AUKF. In these three situations, our approach has improved convergence speed by 75%, 33%, and 67.2% respectively.  Table 5.        Figure 24b, the AUKF is sensitive to disturbances and takes a longer time at adapting. 3.
As shown in Figure 9b, Figure 10b, Figure 11b, Figure 12b, Figure 13b, the accuracies of our approach are more stable when facing severe disturbances. This indicates that our approach can adapt to the most optimistic measurement noise covariance matrix.

4.
As shown in the test under double lane change road condition, our approach can reflect the small state changes during the driving process, which is meaningful and important to the vehicle safety control system. This is because AUKF uses a windowing mask to eliminate the influence of state change on current estimation. The proportional coefficient M is set according to experiences. M cannot be determined or will be inaccurate if unknown changes occur. 5.
According to Table 4, it can be noticed that in the lateral speed comparison, the RMSE value of the proposed approach is larger than that of the classical AUKF. There are two reasons. Firstly, the larger RMSE value here indicates that the proposed approach is more sensitive to the state change. It needs to be acknowledged that since we are performing on-line estimation when the disturbance is added, the system cannot distinguish whether the changing observation value is caused by a sudden change of vehicle state or just an exerted disturbance. The standard error is sensitive to the extremely large value or the extremely small value during the measurement. The proposed approach is sensitive to the state change, and the exerted disturbance is twenty times of the true value at that moment. Secondly, our approach is with a smaller MAE value under the circumstance of a larger RMSE value. This indicates our approach is with stronger robustness than classical AUKF. MAE value is the absolute value summation of the difference between the true value and the estimated value, which reflects the stability of the system. 6.
According to Tables 4 and 5, the proposed approach remains high accuracy and stability when the transient disturbance occurs, and it is with stronger robustness comparatively.

Conclusions
When the road condition changes frequently and drastically, traditional vehicle driving state estimation methods perform undesirable. The unknown measurement noise and transient disturbances will cause the failure of state estimation and affect the subsequent process. We propose an AUKF based on a modified Sage-Husa filter and divergence calculation technique to solve these problems. We mainly improve the classical AUKF algorithm in two aspects. First, we introduce the divergence calculation technique to detect transient disturbances and solve the divergence problem of Sage-Husa maximum posteriori. Second, we modify the covariance matrix when the transient disturbance occurs.
Our approach can excellently face transient disturbances and has strong robustness through standard simulation tests and robustness tests under double lane change and Sine Wave road conditions. The proposed Modified Sage-Husa UKF algorithm fully satisfies the actual engineering precision requirements under simulation. The results of our approach rival the measurements obtained from high-cost and high-accuracy sensors. Besides, the simulation results show the feasibility of applying our approach to vehicle driving state estimation.
In our future work, we will first improve our algorithm, including the verification of more complicated working conditions and applications in a real car. Then we will add application scenarios for vehicle driving state estimation. The estimated data can combine with IoT (Internet of Things) and big data. For example, the vehicle driving states of a car getting out of control can share with surrounding cars. This will avoid secondary accidents. Data sharing could also share those repeated measuring state variables. It will save computational power and improve accuracy.
Author Contributions: Conceptualization, methodology, software, validation, formal analysis, investigation, data curation, Z.L.; software, validation, formal analysis, investigation, data curation, visualization, writing-original draft preparation, Z.F.; resources, writing-review and editing, supervision, project administration, funding acquisition, Q.X. All authors have read and agreed to the published version of the manuscript.