Vehicle Lateral Motion State Estimation Based on Adaptive Cubature Kalman Filter

Abstract: Accurate and reliable vehicle state information is very significant to the vehicle lateral stability control, while it is hard to get information such as body sideslip angle and lateral tire forces due to lack the nonlinearity of the tire. This paper presents a new combined method (adaptive cubature Kalman filter(ACKF) and adaptive proportion integral observer(APIO)) to estimate body sideslip angle, yaw rate and lateral tire force for vehicle system. Firstly, based on a four-wheel vehicle dynamics model, (ACKF) is used to estimate body sideslip angle and yaw rate with considering the nonlinear lateral tire force stage. Due to system nonlinearities and un-modeled dynamics, APIO is used to improve the estimated body sideslip angle by utilizing the estimated yaw rate and vehicle lateral speed. Then, ACKF is used to estimate the front and rear lateral tire forces based on the one-order tire dynamics model. By utilizing the partition coefficient calculated by the vertical force model, the front and rear lateral tire forces are further distributed to left and right wheels. For comparison, estimation model based on extended Kalman filter(EKF) is built and investigated. Simulation using Matlab/Simulink-CarSim and car test verifies the effectiveness of the proposed method. Corresponding Author: Zhen-yu ZHANG, M.D. Hunan University Changsha, Hu nan CHINA Corresponding Author E-Mail: 2523138212@qq.com Corresponding Author Secondary Information: Corresponding Author's Institution: Hunan University Corresponding Author's Secondary Institution: First Author: Zhi-cheng HE, Phd First Author Secondary Information: Order of Authors: Zhi-cheng HE, Phd Zhen-yu ZHANG, M.D. En-lin ZHOU Bao-lv WEI Order of Authors Secondary Information: Suggested Reviewers: Opposed Reviewers: Powered by Editorial Manager® and ProduXion Manager® from Aries Systems Corporation Additional Information: Question Response Powered by Editorial Manager® and ProduXion Manager® from Aries Systems Corporation

Abstract: Accurate and reliable vehicle state information is very significant to the vehicle lateral stability control, while it is hard to get information such as body sideslip angle and lateral tire forces due to lack the nonlinearity of the tire. This paper presents a new combined method (adaptive cubature Kalman filter(ACKF) and adaptive proportion integral observer(APIO)) to estimate body sideslip angle, yaw rate and lateral tire force for vehicle system. Firstly, based on a four-wheel vehicle dynamics model, (ACKF) is used to estimate body sideslip angle and yaw rate with considering the nonlinear lateral tire force stage. Due to system nonlinearities and un-modeled dynamics, APIO is used to improve the estimated body sideslip angle by utilizing the estimated yaw rate and vehicle lateral speed. Then, ACKF is used to estimate the front and rear lateral tire forces based on the one-order tire dynamics model. By utilizing the partition coefficient calculated by the vertical force model, the front and rear lateral tire forces are further distributed to left and right wheels.

Introduction
With the development of automotive technology, the lateral stability of vehicle has attracted more and more attention from researchers. For the research on the lateral stability of vehicle, it is necessary to use sensors to obtain real-time status information of vehicle, such as longitudinal speed, lateral speed, lateral acceleration, yaw rate, and body sideslip angle. However, due to expensive equipment, mass production models will not be equipped with sensors for body sideslip angle. Therefore the currently adopted measures are utilizing some estimation methods to get body sideslip angle [1][2][3][4][5].
Common vehicle state estimation methods include sliding mode observer (SMO), Luenberger observer, non-linear observers, and Kalman filters. Among them, Kalman filter and its improved algorithms have become a research focus because of their high stability and accuracy. Chen et al. [6] proposed an estimation method combining strong tracking filter with extended Kalman filter which overcomes the tire lateral force and longitudinal force calculation problems when the longitudinal acceleration and lateral acceleration are unknown. The test proved that the method can effectively estimate the tire longitudinal and lateral forces. Geng et al. [7] proposed a noise adaptive extended Kalman filter algorithm to estimate the state information including body sideslip angle and yaw rate in view of the problem that the estimation accuracy is reduced due to inaccurate prior information. Simulation showed that the method achieves effective estimation. Similarly, Zhang et al. [8] proposed an extended Kalman filter algorithm based on the adaptive strategy of sliding window of innovation sequence, which improved the estimation accuracy of body sideslip angle. Moreover, in [9], an augmented EKF observer considering model parameters variability and noisy measurement input Vehicle Lateral Motion State Estimation Based on Adaptive Cubature Kalman Filter ·3· was proposed for body sideslip angle and yaw rate estimation. Chen et al. [10] combined EKF with Luenberger observer and high-order sliding mode observer to estimate tire longitudinal force, lateral vehicle speed, body sideslip angle and yaw rate. Results showed an improvement in the estimation accuracy compared to standard EKF. Similarly, to minimize the effects of sensors errors and model nonlinearities, Pi et al. [11] combined model-based method and kinematics-based method to estimate body sideslip angle. Meanwhile, fuzzy-logic observer was implemented to determine the nonlinear factor and corning stiffness was identified to compensate the tire nonlinearities. The researches above are all based on EKF and its improved form to body sideslip angle and other vehicle state information, but EKF need to calculate state transition matrix by utilizing state function which maybe cause the problem of divergence [12][13][14][15][16][17][18].
To address issues associated with the complex calculation of Jacobian matrix and excessive large error caused by system nonlinearity, unscented Kalman filter(UKF) and cubature Kalman filter(CKF) are proposed. Zhou and Qi proposed an adaptive UKF algorithm which consider the impact of noise on the system to estimate body sideslip angle and yaw rate [19]. Similarly, Wang et al. [20] took the relationship of different road noise and sensors noise into account and utilized a road classification method working conjunction with AUKF algorithm. Results showed that the method improves the accuracy of roll rate, pitch rate and sprung mass velocity of vehicle body. In [21], Boada et al. combined adaptive neuro-fuzzy inference system (ANFIS) with UKF to estimate body sideslip angle, which in turn was used to control the vehicle and improves the stability. Moreover, to address the issues caused by tire nonlinearities, Jin and Yin [22] introduced an interacting multiple model (IMM) which consists of two UKF algorithm parts. The simulation in Carsim and Matlab verified the effectiveness of IMM.
Compared to first-order accuracy of Taylor expansion in EKF and second-order accuracy in UKF, CKF can promise three-order accuracy for nonlinear systems. CKF adopts spherical-radial cubature rule to approximate the multidimensional integrals [23][24]. A series of cubature points are used to compute integrals. For UKF, big weight of the sigma point in the center maybe leads to semi-positive and negative definite of covariance matrix, while CKF distribute same weight to sigma points and cubature points, which enhances the numerical stability [25][26][27]. In [28][29][30], researchers adopt adaptive law by utilizing innovation information and make use of suboptimal unbiased maximal posterior estimator for system and measurement noise to update covariance in real time. Chen et al. [31] proposed an adaptive CKF algorithm working conjunction with a longitudinal tire force observer to estimate vehicle state information. In this method, an adaptive law was introduced to CKF by setting different weights to innovation vector according to usefulness of information in the slide window.
It is noted that body sideslip angle, yaw rate and lateral tire forces are important for vehicle lateral control. However, most prior methods for vehicle states estimation are based on high-order state vectors which have strong connection with each other. To reduce the risk of semi-positive and negative definite to covariance matrix for CKF, a modular estimation strategy is proposed and applied to estimate vehicle states. Firstly, considering the lateral tire forces nonlinearities, body sideslip angle and yaw rate are preliminarily estimated by ACKF which makes use of innovation information in different tire sideslip angle section. Meanwhile, an adaptive proportion integral observer is introduced to compensate body sideslip angle by utilizing estimated yaw rate. Secondly, a one-order dynamics tire model is adopted to kinematics model. Sum of front lateral force and sum of rear lateral force are estimated by ACKF. In addition, by computing partition coefficient from vertical tire force model, lateral force of ACKF is distributed to four wheels.

Figure 1
Architecture of the method for state estimation This paper is organized as follows: Section 2 briefly describes the four-wheel vehicle dynamics model. The proposed observer for body sideslip angle and yaw rate is presented in detail in Section 3. In addition, a new observer for lateral tire force and lateral vehicle speed is discussed in Section 3. Its performance is tested by MATLAB/Simulink-Carsim co-simulation and real vehicle test in Section 4.  Section 5 is the conclusion. Section 6 is the declaration.
2 Four-wheel vehicle dynamics model

Vehicle dynamic model
The vehicle model presented in this paper is shown in Figure 1. The model is based on the following assumption: 1) neglect the pitch and roll angles of the vehicle and the effect of the suspension. 2) assume that the characteristics of each tire are the same, and the steering system is accurately adjusted, and the left and right front wheels have the same angle of rotation. 3) neglect the effect of vertical rolling resistance. The longitudinal axle of the vehicle is the axle of coordinate system ,and the lateral axle of the vehicle is the axle. The origin of coordinate system is fixed on the vehicle gravity. The symbols , , and are respectively corresponding to the front-left, the front-right, the rear-left and the rear-right. Under the above assumptions, the four-wheel dynamics model can be expressed by the following governing equation Equation of yaw motion: Equation of lateral motion: is the yaw rate, represents the moment of inertia, and are the steering angles of front-left and front-right wheel. and are the front tread and rear tread of the vehicle. and are the distances from vehicle gravity center to the front and rear axle.
stands for the lateral acceleration, represents the mass, is the longitudinal vehicle speed, is the lateral vehicle speed, represents the longitudinal acceleration.
is moment of vehicle gravity center in -plane. Four-wheel vehicle dynamics model can distinguish the difference in tire force between the left and right wheels, but the single-track vehicle model pays more attention to the influence of the center of mass slip angle and yaw rate on the vehicle. Therefore, single-track vehicle model is also implemented to estimate the vehicle states when estimating the lateral angle of mass and yaw rate. The dynamics model is shown as follows: Equation of yaw motion: Equation of lateral motion: where , is the sum of front-left and front-right lateral tire force, , is the sum of rear-left and rear-right lateral tire force,  4  5  6  7  8  9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  57  58  59  60  61  62  63  64  65 Vehicle Lateral Motion State Estimation Based on Adaptive Cubature Kalman Filter

Tire model
The normal vertical tire force equation , ( = , = ) for each wheel can be expressed as: is gravitational acceleration, ℎ is the height of vehicle gravity center. The where ( , ) and ( , ) are the front corning stiffness and rear corning stiffness which are related to tire sideslip angle of each wheel. Moreover, the piecewise functions Eq. (10) and Eq. (11) are used to describe the nonlinear characteristics between the lateral tire force and tire sideslip angle [14].

ACKF for Lateral State Information
The traditional EKF can only estimate the first-order accuracy of the body sideslip angle in the nonlinear region. UKF can achieve second-order accuracy by using the unscented criterion. But CKF can use cubature criterion to improve the estimation accuracy to third order. Therefore, CKF is adopted to estimate vehicle state information. Firstly, Kalman filter is implemented to estimate body sideslip angle and yaw rate. Then, body sideslip angle is corrected by estimated yaw rate, measured longitudinal vehicle speed and lateral vehicle speed via adaptive proportion integral observer(APIO).

Extended Kalman Filter
The continuous-time nonlinear system can be expressed by Eq. (12): where is the state vector, is the input vector, is the measurement vector. ( * ) and ℎ( * ) are the state function and measurement function, respectively. Moreover, represents the process noise vector, and represents the measurement noise vector. and are assumed to be zero-mean white satisfies Gaussian distributions, and uncorrelated.
The priori prediction of state is calculated by: is the state transition matrix, which is the Jacobian matrix of state function ( * ) with respect to .
+1| is calculated by Eq. (16): where , represent the dimension of the measurement function ( * ) and state vector . The priori error covariance matrix is given by: where is the process noise covariance matrix, and = ( , ). +1 is the state measurement matrix, which is the Jacobian matrix of the measurement function ℎ( * ) with respect to . And, +1 is calculated by Eq. (18): where represents the dimension of the measurement function ℎ( * ) . The Kalman gain is updated: where is the measurement noise covariance matrix, and = ( , ).
The posterior prediction of state is computed as: The posterior error covariance is calculated by:

ACKF for Body Sideslip Angle and Yaw Rate
CKF is firstly proposed by Canadian researcher Arasaratnam and Haykin to address a typical air-traffic control problem in 2009. The core idea of CKF algorithm is to approximate the posterior mean and covariance of the state by the third-order spherical radial volume criterion for the nonlinear Gaussian system to ensure that the posterior mean and covariance of any nonlinear Gaussian state is approximated by a third-order polynomial in theory.

Correction by Adaptive Proportion Integral Observer
In order to observe the body sideslip angle accurately, compensation algorithm is proposed to implemented and it estimates the body sideslip angle utilizing estimated yaw rate. The APIO is given by Eq. (41).
where ො represents the estimated lateral vehicle speed, and represents the longitudinal vehicle speed measured by sensors. መ stands for the body sideslip angle estimated by ACKF. and represent the proportion gain and integration gain, respectively. and is given by Eq. (41) and (42).

ACKF for Lateral Tire Force
In order to estimate the lateral tire force, ACKF algorithm is adopted. A widely used empirical tire model is mainly based on empirical formulations deriving from tire test data and an amount of parameters [32]. To avoid complex calculation and utilizing tire test data, linearized tire force models is adopted. Based on the bicycle model and one-order dynamics tire model [33], state space model for lateral tire force can be built. First of all, the lateral tire forces can be linearly approximated and are given as: where is the body sideslip angle estimated by APIO, and is the yaw rate estimated by ACKF. In order to illustrate the transient behavior of the tire, a typical dynamic model, the first-order dynamic model is used, which is expressed as follows: where , is relaxation time constant, defined as a constant associated with vehicle speed. ̅ , is the lateral tire force calculated by Eq.

Simulation and Experiments
To evaluate the performance of the proposed estimation method in this paper, the simulations have been carried out in the Matlab/Simulink and CarSim software. The estimation algorithms are designed in the Matlab/Simulink, and the vehicle model in CarSim is achieved. CarSim contains a variety of vehicle models with up to 27 degrees of freedom, rich in parameter variables, and can realize the virtual test of cars in various scenarios. What is more, CarSim provides the reference data for comparison with the proposed method. Two simulation tests are employed to demonstrate the proposed method. The first is a double lane change maneuver of a vehicle traveling at 80 km/h on a road surface with friction coefficients of 0.85. The second is a sine steer maneuver of a vehicle traveling at 48 km/h on a road surface with friction coefficients of 0.85. The relevant parameters of the vehicle model are shown in Table 1.

Figure 3
The steering wheel angle and longitudinal vehicle speed  Table 2 shows the MAE and RMSE of the body sideslip angle. MAE and RMSE of EKF is -3.008e-4 rad and 3.350e-3 rad, respectively, while MAE and RMSE of ACKF with APIO are smaller than those of EKF, which are -1.145e-4 rad and 1.421e-3 rad, respectively.  Table 2 that the accuracy of the estimated yaw rate is satisfactory and the MAE and RMSE are -2.014e-3 rad/s and 3.479e-2 rad/s, respectively and that of EKF are -2.310e-3 rad/s and 3.557e-2 rad/s, respectively. The estimation precision of EKF is slightly worse than that of the proposed ACKF.

Sine steer maneuver with longitudinal speed at 50 km/h
As shown in Fig. 8， the longitudinal vehicle speed is about 50 km/h and steering wheel angle is about from -100°to 100°. Fig. 9 shows the lateral vehicle speed and lateral acceleration. During the maneuver, the lateral vehicle speed is between -1 km/h and 1 km/h and the lateral acceleration is about from -5 m/s 2 to 5 m/s 2 .

·12·
can estimate the yaw rate more accurately than EKF as shown in Fig. 11. It can be found in Table 4 that the accuracy of the estimated yaw rate is satisfactory and the MAE and RMSE are -3.653e-3 rad/s and 1.327e-2 rad/s, respectively and that of EKF are -3.717e-3 rad/s and 5.484e-2 rad/s, respectively. The estimation precision of EKF is worse than that of the proposed ACKF.

Car test verification
In order to better verify the effect in the real car, the experiment was carried out. The vehicle test system is shown in Fig. 13. The car used in the test is a sport utility vehicle. ABDynamics steering robot is a computer-controlled vehicle robot test product, which is specially used for dangerous condition of vehicle dynamics test. It can learn a path by manual driving, and then create this path from the recorded data. Dual antenna sensor is a GPS data acquisition system with high accuracy and the ability to measure longitudinal speed, lateral speed and sideslip angle at 100 Hz. Longitudinal and lateral acceleration can be measured by IMU. The collected data can be output in various forms such as CAN, RS232, analog and digital. CAN bus output includes: speed, sideslip angle, pitch angle and roll angle and yaw rate. Steering angle sensor, wheel speed sensor and cylinder pressure sensor can measure the steering wheel angle, wheel speed and cylinder pressure. Wheel speed and cylinder pressure is used for early correction of other signals.   1  2  3  4  5  6  7  8  9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  57  58  59  60  61  62  63 64 65 ·14· Figure 14 The steering wheel angle and longitudinal vehicle speed Figure 15 The lateral acceleration and lateral vehicle speed As shown in Fig. 16, the body sideslip angle estimated by ACKF+APIO is more accurate than EKF and ACKF. Table   6 shows the MAE and RMSE of the body sideslip angle.
MAE and RMSE of EKF are 4.0340e-3 rad and 1.004e-2 rad respectively, while MAE and RMSE of ACKF with APIO are smaller than those of EKF, which are 6.003e-4 rad and 3.197e-3 rad respectively. Fig. 17 shows the estimated values and actual values of yaw rate, and the proposed algorithm can estimate the yaw rate more accurately than EKF as shown in Fig. 17. It can be found in Table 6 that the accuracy of the estimated yaw rate is satisfactory and the MAE and RMSE are 1.315e-3 rad/s and 5.867e-3 rad/s, respectively and that of EKF are -7.371e-3 rad/s and 3.837e-2 rad/s, respectively. The estimation precision of EKF is worse than that of the proposed ACKF.
Due to the lack of the measurement equipment of lateral tire force, it is difficult to show the reference of the tire force.
However, lateral tire force estimation is still shown in Fig.   18.

Conclusions
A novel observation algorithm to simultaneously estimate the body sideslip angles, yaw rate and lateral tire forces of all the four wheels is proposed in this paper. It mainly consists of four blocks: an observer block for body sideslip angle by ACKF, a correction block for body sideslip by APIO, a lateral tire force observer block and lateral tire force distribution block. Main contributions of this paper can be summarized as follows. Firstly, an ACKF is utilized to estimate the body sideslip angle and yaw rate. Secondly, the body sideslip angle is corrected by the measured and estimated yaw rate by APIO. Thirdly, the lateral tire force is estimated by ACKF and distributed to four wheels. Moreover, the simulation and car test are carried out to demonstrate the proposed method.