Improved cubature Kalman filter for vehicle state estimation and measurement

1. Introduction

Today, with the increasing number of vehicles on the road and the increasingly complex traffic environment, driving safety is facing severe challenges. Intelligent vehicles equipped with advanced driving assistance systems have become a research hotspot due to their ability of reducing the driver operation load and assisting the driver in achieving safe driving. Accurately and real-time obtaining vehicle driving status information is the key to achieve effective decision-making and control of intelligent vehicles. The active safety technology of automobiles has always been a focus of attention. Active safety can help improve the maneuverability and stability of vehicles, thereby reducing the probability of accidents. One of the key aspects of active safety technology is to accurately obtain the real-time status of the vehicle, including its yaw rate, longitudinal and lateral velocity, and side slip angle. There are two main ways to obtain vehicle status parameters. One is based on sensor measurement, but due to the high cost of sensors, algorithms are currently more commonly used for estimation. The acquisition of vehicle driving state parameters is the basis for the active safety of vehicles, but due to the cost of sensors or the inability to directly measure parameters, accurate estimation of vehicle driving state parameters by low-cost estimators is crucial [1-3].

The idea of Cubature Kalman Filter was originally presented by Ienkaran Arasaratnam and Simon Haykin [4]. Over the past few decades, intelligent connected vehicles have received increasing attention for their large benefits in improving road safety [5-8]. Compared with EKF, the UKF can be applied to nonlinear distributed systems, achieving higher computational accuracy [9-14]. Most estimation algorithms based on KF had higher estimation accuracy, but they were limited by the vehicle kinematic model and required accurate model parameters, so KF based algorithms had certain limitations [15-17]. In order to solve the problem of relying on kinematic models, some studies had applied machine learning algorithms to vehicle state estimation. Machine learning based regression algorithms did not rely on automotive kinematic models and inherent vehicle parameters, but estimated vehicle state parameters through model structure and training datasets [18]. At the same time the neural network and the sensor fusion technologies are used for the vehicle state estimation [19-21]. Cubature-Kalman filter is widely used for nonlinear dynamic estimations due to its high accuracy and numerical stability [22-26].

In recent years, scholars have found that CKF has strong adaptability, but its estimation accuracy is low or even divergent in strongly nonlinear system environments such as distributed drive electric vehicles. Zhao et al. estimated the yaw rate, longitudinal speed, and side slip angle based on the 3-DOF dynamic model based on the front wheel angle and longitudinal acceleration. By comparing the UKF and EKF algorithms, the efficiency and accuracy of the application of the UKF in the estimation of vehicle handling stability states and parameters were verified. The method described in the article controlled the driving or braking torque of the vehicle, effectively improving the slipping and locking conditions during driving and braking, ensuring the safety of the vehicle operation [27]. Lu et al. designed a nonlinear full dimensional observer and a UKF observer based on vehicle sensors, and compared and analyzed the two estimation methods under extreme conditions. The prediction performance of the nonlinear full dimensional observer was slightly higher under extreme conditions [28]. Pan et al. studied the lateral stability of vehicles based on the Uni-tire model and 2-DOF dynamic model, and designed a side slip angle estimator, which significantly improved the estimation effect and provided more accurate prediction values for the design of vehicle control strategies [29]. Particle Filter is widely used for nonlinear dynamic estimations especially in the field of vehicle state estimation [30-32].

The Kalman filter estimation algorithms studied above are all based on the improvement of third-order accuracy, which limits the improvement of their accuracy. Other estimation algorithms mainly fuse different algorithms, which are computationally complex and have high levels of algorithm constraints. They do not have high estimation accuracy for high-dimensional nonlinear models. In response to the limitations of traditional third-order CKF, this paper extends it using the fifth-order sphere-radius cubature rule to form a fifth-order CKF. At the same time, in the filtering process of the fifth-order CKF, a more stable singular value decomposition is used instead of Cholesky decomposition, further improving the estimation accuracy of the algorithm. During the CKF iteration process, it is necessary to generate cubature points. The generation of cubature points is related to the error covariance matrix. Generally, in the calculation process, the error covariance matrix needs to be subjected to Cholesky decomposition to obtain the volume points, which requires the error covariance matrix to maintain positive definiteness at all times. Errors may occur during the computer process, causing the covariance matrix to become non positive definite during the update process, resulting in the inability to perform Cholesky decomposition properly and causing the algorithm to fail in computation.

2. Mathematical model of vehicle state estimation problem

2.1. 3-DOF vehicle model

The vehicle state estimation model is established based on a 3-DOF vehicle model:

The dynamic equation of the 3-DOF vehicle model is as follows [33]:

1

${\dot{\omega }}_r=\frac{a^2{k}_1-b^2{k}_2}{I_z}\frac{{\omega }_r}{u}+\frac{a{k}_1+b{k}_2}{I_z}\beta -\frac{k_1}{I_z}\delta ,$

2

$\dot{\beta }=\frac{a{k}_1-b{k}_2-m{u}^2}{m}\frac{{\omega }_r}{u^2}+\frac{k_1+k_2}{m}\frac{\beta }{u}-\frac{k_1\delta }{mu},$

3

$\dot{u}=a_x+vr,$

4

$a_y=\frac{a{k}_1-b{k}_2}{mu}{\omega }_r+\frac{k_1+k_2}{m}\beta -\frac{k_1}{m}\delta .$

The side slip angle of the center of mass is:

5

$\beta =\arctan \left(\frac{v}{u}\right).$

Fig. 13-DOF vehicle model

3. SVD FCKF estimator

For the following discrete-time nonlinear dynamic systems:

where $x_k$ and $W_{m,1}=A_n/\left(n\right(n+2\left)\right)$ are the state and observation vectors of the system at time k; $W_{m,2}=(4-n)A_n/\left(2n\right(n+2\left)\right)$ and $e_j$ are the system state transition function and the observation function; $2n^2+1$ and $v_k$ are the process noise and observation noise which are uncorrelated and follow a normal distribution, $N_r=2$ and $N_s=2n^2$; $X_{k-1,i}$ is the covariance matrix of the process noise; $k-1$ is the mean square error matrix of the observation noise.

4. Numerical simulation and experimental verification

This article uses the SVD-FCKF method to filter and estimate the yaw rate and side slip angle as well as longitudinal velocity. A Carsim and Matlab/Simulink joint simulation platform is established for simulation and the results are compared and analyzed with the CKF method.

4.1. Numerical simulation

4.1.1. $\mathit{\mu }$ = 0.85

In the simulation sampling period is set as 0.001 s; the covariance matrix of the system noise is $Q=0.1I_{3\times 3}$ and the initial value of the covariance matrix of the measurement noise is $R_0=0.01I_{3\times 3}$.

The simulation results of the yaw rate and side slip angle as well as longitudinal velocity of the vehicle under the condition of $\mu$ = 0.85 are shown in Fig. 2.

From Fig. 2, it can be seen that the curve obtained by the SVD-FCKF estimator is more in line with the reference values than the curve obtained by the CKF estimator. And the effect is more significant at the peak and valley of the curve. The reason for this trend is that the steering angle at the peak and valley changes at high speed causing significant fluctuations in the vehicle state. At this time, the CKF processing highly nonlinear systems in the third-order dimension can easily accumulate errors, resulting in estimation results deviating from actual values. However, the SVD-FCKF can control the accumulation of errors and obtain more accurate estimation results through a fifth-order Taylor expansion. The SVD-FCKF estimator and CKF estimator maintained a good fit with the reference value in the first 1 second. But after 1 second, the estimated value of the CKF gradually deviated from the reference value, while the SVD-FCKF still maintained good estimation performance. This is because singular value decomposition can reduce the influence of noise in the estimation process and improve the robustness of the system and reduce the influence of state estimation errors and ensure that the estimation results always converge to the reference value. At the same time, singular value decomposition and Cholesky decomposition are two different matrix decomposition methods that are applicable to different types of matrices. Singular value decomposition has the following advantages over Cholesky decomposition: Cholesky decomposition is mainly used for decomposing symmetric positive definite matrices, while singular value decomposition can be used for decomposing any matrix, making the application scenarios of singular value decomposition more extensive.

In order to further compare the estimation accuracy of the algorithm, the root mean square error (RMSE) is used to evaluate and compare the estimation accuracy, as shown in Table 1. From Table 1, it can be seen that the RMSE of the yaw rate and side slip angle as well as the longitudinal velocity calculated by the SVD-FCKF algorithm is smaller than that of the CKF algorithm.

Fig. 2Simulation results under the condition of μ= 0.85: a) yaw rate; b) error of yaw rate; c) side slip angle; d) error of side slip angle; e) longitudinal velocity; f) error of longitudinal velocity

a)

b)

c)

d)

e)

f)

Table 1The RMSE indicators of the two algorithms

Evaluation index	State value	SVD-FCKF	CKF
RMSE	$r$ (rad/s)	0.00279	0.00566
	$\beta$ (rad)	0.000441	0.000672
	$v$ (km/h)	0.1629	1.1432

4.1.2. $\mathit{\mu }$ =0.2

The simulation results of the yaw rate and side slip angle as well as longitudinal velocity of the vehicle under condition of $\mu$ = 0.2 are shown in Fig. 3.

From Figs. 3 (a)-(b), it can be seen that the longitudinal velocity estimated by SVD-FCKF and CKF estimators are generally consistent with the reference values. However, the overall error of the SVD-FCKF is smaller than that of the CKF, indicating higher estimation accuracy. From Fig. 3(c), it can be seen that the longitudinal velocity errors estimated by the SVD-FCKF estimator under low adhesion coefficient condition are superior to those estimated by the CKF estimator. Although the estimation accuracy under low adhesion coefficient conditions is lower than that under high adhesion coefficient conditions, the overall estimation effect is stable, and the error is within a controllable range. At the same time, singular value decomposition can be used to solve least squares problems and principal component analysis as well as matrix compression and denoising applications. Cholesky decomposition is mainly used to solve linear systems of equations and calculate determinants, inverse matrices, etc. of matrices. At the same time, singular value decomposition is robust to small changes in the matrix, and even with small perturbations in the matrix. The changes in singular values are not too significant, which makes it perform better when dealing with noisy data.

Fig. 3Simulation results under the condition of μ =0.2: a) yaw rate; b) side slip angle; c) longitudinal velocity

a)

b)

c)

4.2. Experimental verification

A virtual test using the CarSim is conducted to verify the feasibility of the simulated results.

At the same time, this paper conducted real an experimental verification research for a vehicle. The real experimental vehicle is shown in Fig. 4. The measurement devices are shown in Fig. 5.

Fig. 4Real test vehicle

Fig. 5Measurement devices

The experimental results of the longitudinal velocity and yaw rate as well as side slip angle of the vehicle under the double lane changing condition are shown in Fig. 6.

Fig. 6Experimental results: a) longitudinal velocity; b) yaw rate; c) side slip angle

a)

b)

c)

The virtual and real vehicle experiment results demonstrate that the algorithm can accurately estimate the vehicle state parameters and has good accuracy. This is because the CKF Kalman filter estimation algorithm is based on the improvement of third-order accuracy, which limits the improvement of its accuracy. It is extended using the fifth-order sphere-radius cubature rule to form the fifth-order CKF. At the same time, in the fifth-order CKF filtering process, more stable singular value decomposition is used instead of Cholesky decomposition, further improving the estimation accuracy and robustness to anomalous observations of the algorithm. It can be seen that there are errors between the test and the simulation value. This is because that the model in the virtual test ignored the nonlinearity of the steering and suspension systems. However, the trend of the virtual vehicle results is consistent with the simulation results, which reflects the virtual situation of the vehicle more realistically verifying the effectiveness of the algorithm.

5. Conclusions

This article applies a SVD-FCKF algorithm estimator to address the issues of low estimation accuracy and poor stability in the process of vehicle state estimation. Based on the third-order CKF, the sphere-radius cubature rule is extended to a fifth-order form, constructing a fifth-order CKF. At the same time, the singular value decomposition is used instead of Cholesky decomposition in covariance matrix decomposition to enhance numerical stability. The effect of the algorithm under high and low adhesion coefficient conditions is simulated and verified. The results show that compared with CKF, the SVD-FCKF estimator has significantly improved the estimation accuracy and stability of the yaw rate and side slip angle as well as longitudinal velocity. It can maintain strong robustness under extreme conditions such as low adhesion coefficient achieving the expected effect and providing reference for active safety research of intelligent vehicles. The virtual experiment results are consistent with the simulation results, so the algorithm can accurately and timely estimate the state parameters. Its accuracy has been verified, and it also provides some theoretical and real vehicle data basis for vehicle stability control.

A higher-degree rule will translate to higher accuracy only if the integrand is well-behaved in the sense of being approximated by a higher-degree polynomial, and the weighting function is known to be a Gaussian density exactly. In practice, these two requirements are hardly met. For example, the integrand function in engineering may not be smooth, or the weight function may be unknown or non-Gaussian. In order to solve the problem, we can adopt an adaptive integration strategy, combining trapz and quad methods for error estimation or dynamically adjust the integration step size instead of relying on fixed high-order rules. And also, we can establish smooth processing for the non-smooth function through convolution operation and construct a smooth function column to approximate the original function or establish different order quadrature rules. In the future, how to solve the above issue should be studied.