# Vehicle State Estimation Based on Adaptive Fading Unscented Kalman Filter

Authors: Yingjie Liu; Dawei Cui
Journal: Mathematical Problems in Engineering
 (2022)

---

## Abstract

Aiming at solving problem of vehicle state estimation, an adaptive fading unscented Kalman filter(AFUKF) algorithm was proposed. Based on this purpose, a 7-DOF nonlinear vehicle model with the Pacejka nonlinear tire model was established firstly. Then, the vehicle state estimator based on Kalman filter was designed to solve the problem of vehicle state estimation. The simulation verification shows the effectiveness and reliability of the designed estimator for vehicle state estimation. Compared with other traditional methods, the calculation accuracy is higher for the AFUKF algorithm to solve the problem of vehicle state estimation. The study can help drivers easily identify key state estimation in safe driving area.


---

## Body

## 1. Introduction

Nowadays, vehicle has become an indispensable means of transportation in people's lives. And people are paying more and more attention to the safety of automobiles. People have higher and higher requirements for the steering stability and active safety of automobiles accompanied by continuous development and application of information technology in the automotive field. The safety of automobiles includes two aspects: active safety and passive safety. Active safety refers to how to minimize or avoid the occurrence of traffic accidents through the design of vehicles; passive safety refers to the design of vehicles to minimize the damage to occupants in the event of an accident. The automobile active safety control system can effectively improve the handling stability of the vehicle and avoid the occurrence of traffic accidents. And real-time and accurate acquisition of the vehicle driving state and road adhesion coefficient is a necessary prerequisite for realizing active safety control. Vehicle speedometer, gyroscope, and road surface direct recognition technology is expensive and difficult to popularize in automotive active safety control in a short time. Therefore, how to apply low-cost sensors to accurately estimate vehicle driving status and road attachment information based on related theories has become a research hot issue in automotive active safety control.The problem of vehicle state estimation has been widely studied. A brief review is presented as follows.Katriniok et al. [1] proposed an extended Kalman filter-based estimator to estimate the vehicle’s yaw rate and the longitudinal and lateral velocity. Wang et al. [2] estimated the tire cornering stiffness and the sideslip angle using a novel fuzzy adaptive robust cubature Kalman filter (FARCKF). Chen et al. [3] estimated the longitudinal force, yaw rate, and the lateral vehicle speed for four-wheel independent driven electric vehicle using a fusion method. Wang et al. [4] estimated vehicle state in case of partial loss of sensor data by a novel adaptive fault-tolerant extended Kalman filter. In order to estimate a class of nonlinear systems perturbed by noise, Hashlamon et al. [5] proposed a new adaptive extended Kalman filter. To improve the active safety of vehicles, Wang et al. [6] proposed a robust cubature Kalman algorithm on the estimation of the vehicle state. In order to estimate the longitudinal and lateral forces, Regolin et al. [7] presented a novel estimation algorithm. Strano et al. [8] proposed a constrained unscented Kalman filter to estimate the vehicle sideslip angle. Jonathan et al. [9] discussed a vehicle state observer for the estimation of the yaw rate, quantity position. Song et al. [10] explored the estimation problem for vehicle states, including lateral velocity and attitude improving the safety of land vehicles. Giseo et al. [11] presented a new method for the estimation of vehicle sideslip angle. Lv et al. [12] proposed a practical estimation method for the lateral and longitudinal velocities of electric vehicles. Zhang et al. [13] proposed a scheme for the estimation of sideslip angle, yaw rate, and vehicle speed. Zhang et al. [14] proposed a novel modified UKF state estimation methodology for the stability control of 4WIDEV. Julian et al. [15] presented a novel holistic method for vehicle state estimation. Gao et al. [16, 17] proposed a new methodology to address the problem of tightly coupled GNSS/INS integration. Focusing upon modeling and design, Martin et al. [18] proposed a modular estimation approach aiming at the estimation of vehicle sideslip angle. Gu et al. [19] proposed an adaptive fading factor UKF to estimate the states of vehicles. Szabat et al. [20] presented a novel algorithm to estimate the mechanical state variables and parameters in a drive system. Liu et al. [21] proposed an adaptive fading Bayesian UKF to estimate the state of unmanned aircraft systems. Fu et al. [22] proposed an optimized algorithm based on the Elman neural network and UKF which can meet the requirements of future vehicle driving state estimation for vehicle driving estimation. Gao et al. [23] presented a new methodology of distributed state fusion to deal with the fusion estimation problem for multisensory nonlinear systems by using the sparse-grid quadrature filter.As a strong nonlinear complex system, the dynamic model of the vehicle usually has unavoidable errors with real system so that the model cannot reflect the real physical process. So, the observed values do not correspond to the model, resulting in problems such as large filtering errors and even divergence. Although the traditional (UKF) has the characteristics of high estimation accuracy, it is based on the standard Kalman filter and belongs to the infinitely growing memory filter. When performing filter estimation at any time, all data before the current time are used, resulting in insufficient use of the current sensor measurement value, and UKF estimation requires accurate mathematical models and noise statistical characteristics, otherwise, the filtering accuracy will be reduced or even diverged. So, the UKF needs to be designed based on a high-precision mathematical model and accurate noise statistical characteristics.Aiming at the above problem, on the basis of the traditional UKF, the paper adopts the attenuation memory filter to exponentially weight the noise and observation data, gradually reduce the weight of old observation data, and correspondingly increase the weight of new observation data. Thereby, the adverse effect of old observation data on filtering is gradually reduced, the performance and accuracy of the estimation algorithm are improved, and the stability of the filter is enhanced for solving the problem of vehicle state estimation.


## 2. Mathematical Model of Vehicle State Estimation Problem

### 2.1. 7-DOF Nonlinear Vehicle Model

A 7-DOF nonlinear vehicle model is established, as shown in Figure1. The vehicle model has the following yaw: longitudinal and lateral and turning motion of the four wheels degrees of freedom [21].Figure 1
7-DOF vehicle model.The differential equation of longitudinal motion of the vehicle is(1)u˙=ax+vr,ax=Fxflcosδ+Fxfrcosδ+Fxrl+Fxrr−Fyflsinδ−Fyfrsinδm,where v and u are the lateral and longitudinal speed, m is the vehicle mass, ax is the longitudinal acceleration, and δ is the front steering angle.The differential equation of lateral motion of the vehicle is(2)v˙=ay−ur,ay=Fxflsinδ+Fxfrsinδ+Fyflcosδ+Fyfrcosδ+Fyrl+Fyrrm,where ay is the lateral acceleration.The differential equation of yaw motion of the vehicle is(3)Γ=tf2Fxfl′−tf2Fxfr′+tr2Fxrl−tr2Fxrr+aFyfl′+aFyfr′−bFyrl−bFyrr+Mzfl+Mzfr+Mzrl+Mzrr,where tf and tr are the front and rear tracks, Γ is the yaw moment around the z axis, Fxij'=Fxijcosδ−Fyijsinδ, Fyij'=Fyijcosδ+Fxijsinδ, Fxij is the longitudinal forces generated on tires, Fyij is the lateral forces generated in the tires, and Mzij is the aligning torque of each wheel.(4)r˙=ΓIz,where r is the yaw rate of the vehicle, and Iz is the moment of inertia around the z axis.The vertical load of each tire can be described as(5)Fzfl,zfr=mg2±mayhtfbl−12maxhlFzrl,zrr=mg2±mayhtral+12maxhl,where Fzij is the vertical load of each wheel and h is the centroid height.The slip angle of each tire is(6)αfl,fr=δ−arctgv+aru±tf/2rαrl,rr=−arctgv−bru±tr/2r,where αij is the slip angle of each tire.The sideslip angle of the vehicle is(7)β=arctgvu.The slip ratio of each tire is(8)sij=reωij−uwijuwij,where ωij is the angular velocity of each wheel and re is the rolling radius of wheel.The wheel center speeduwij of each wheel is(9)uwfl,wfr=vcog±rtf2±aβuwrl,wrr=vcog±rtr2∓bβ,where a and b are the distances of front and rear axles from the center of gravity, and L=a+b is the wheelbase.


### 2.2. Tire Model

The nonlinear characteristics of tires have a very important influence on the steering characteristics and driving stability of automobiles. Vehicle control research often requires the establishment of accurate tire models. The lateral and longitudinal forces and aligning torque are calculated using the Pacejka nonlinear tire model.(10)yx=DsinCarctgBx−EBx−arctgBxYX=yx+svx=X+sh,where Y is the output representing the longitudinal and lateral forces Fx and Fy as well as the aligning torque Mz; the parameters B, C, D, E, and S are related to the tire load, wheel camber angle, and adhesion conditions of the road surface; the input X represents the slip ratio S and the slip angle α.


### 2.3. Nonlinear Vehicle System Containing Noise

According to the 7-DOF nonlinear vehicle model, the longitudinal velocityu, the lateral acceleration ay, and the yaw rate r of the vehicle are set as the observed variables of the system y=u,ay,rT and the state variables are set as x=u,v,ax,ay,r,βT. The control inputs which are the steering angle and the angular velocity of each wheel are z=δ,ωfl,ωfr,ωrl,ωrrT. Considering the influence of process noise vector ωt and system noise vector νt, the state and observation equations can be obtained as follows:(11)x˙t=fxt,zt,ωtyt=hxt,zt,νt,


## 2.1. 7-DOF Nonlinear Vehicle Model

A 7-DOF nonlinear vehicle model is established, as shown in Figure1. The vehicle model has the following yaw: longitudinal and lateral and turning motion of the four wheels degrees of freedom [21].Figure 1
7-DOF vehicle model.The differential equation of longitudinal motion of the vehicle is(1)u˙=ax+vr,ax=Fxflcosδ+Fxfrcosδ+Fxrl+Fxrr−Fyflsinδ−Fyfrsinδm,where v and u are the lateral and longitudinal speed, m is the vehicle mass, ax is the longitudinal acceleration, and δ is the front steering angle.The differential equation of lateral motion of the vehicle is(2)v˙=ay−ur,ay=Fxflsinδ+Fxfrsinδ+Fyflcosδ+Fyfrcosδ+Fyrl+Fyrrm,where ay is the lateral acceleration.The differential equation of yaw motion of the vehicle is(3)Γ=tf2Fxfl′−tf2Fxfr′+tr2Fxrl−tr2Fxrr+aFyfl′+aFyfr′−bFyrl−bFyrr+Mzfl+Mzfr+Mzrl+Mzrr,where tf and tr are the front and rear tracks, Γ is the yaw moment around the z axis, Fxij'=Fxijcosδ−Fyijsinδ, Fyij'=Fyijcosδ+Fxijsinδ, Fxij is the longitudinal forces generated on tires, Fyij is the lateral forces generated in the tires, and Mzij is the aligning torque of each wheel.(4)r˙=ΓIz,where r is the yaw rate of the vehicle, and Iz is the moment of inertia around the z axis.The vertical load of each tire can be described as(5)Fzfl,zfr=mg2±mayhtfbl−12maxhlFzrl,zrr=mg2±mayhtral+12maxhl,where Fzij is the vertical load of each wheel and h is the centroid height.The slip angle of each tire is(6)αfl,fr=δ−arctgv+aru±tf/2rαrl,rr=−arctgv−bru±tr/2r,where αij is the slip angle of each tire.The sideslip angle of the vehicle is(7)β=arctgvu.The slip ratio of each tire is(8)sij=reωij−uwijuwij,where ωij is the angular velocity of each wheel and re is the rolling radius of wheel.The wheel center speeduwij of each wheel is(9)uwfl,wfr=vcog±rtf2±aβuwrl,wrr=vcog±rtr2∓bβ,where a and b are the distances of front and rear axles from the center of gravity, and L=a+b is the wheelbase.


## 2.2. Tire Model

The nonlinear characteristics of tires have a very important influence on the steering characteristics and driving stability of automobiles. Vehicle control research often requires the establishment of accurate tire models. The lateral and longitudinal forces and aligning torque are calculated using the Pacejka nonlinear tire model.(10)yx=DsinCarctgBx−EBx−arctgBxYX=yx+svx=X+sh,where Y is the output representing the longitudinal and lateral forces Fx and Fy as well as the aligning torque Mz; the parameters B, C, D, E, and S are related to the tire load, wheel camber angle, and adhesion conditions of the road surface; the input X represents the slip ratio S and the slip angle α.


## 2.3. Nonlinear Vehicle System Containing Noise

According to the 7-DOF nonlinear vehicle model, the longitudinal velocityu, the lateral acceleration ay, and the yaw rate r of the vehicle are set as the observed variables of the system y=u,ay,rT and the state variables are set as x=u,v,ax,ay,r,βT. The control inputs which are the steering angle and the angular velocity of each wheel are z=δ,ωfl,ωfr,ωrl,ωrrT. Considering the influence of process noise vector ωt and system noise vector νt, the state and observation equations can be obtained as follows:(11)x˙t=fxt,zt,ωtyt=hxt,zt,νt,


## 3. Adaptive Fading Unscented Kalman Filter

### 3.1. Design of the Algorithm

The sampling time of the algorithm is defined asΔt. Discretizing the state and the observation equations in equation (11), equation (12) can be obtained as follows:(12)xk+1=fxk,zk+ωkyk=hxk,zk+νk,where xk and yk are the state and output vectors at time k, respectively; zk is the control input vector at time k; ωk and νk are the process and observation noise vectors at time k, respectively.The process noiseωk and the system observation noise νk are both independent Gaussian white noises. And the noise characteristics are(13)Eωk=qk,Eνk=rkEωk−qkωk−qkT=Qkδk,where, qk and Qk are the mean and covariance matrixes of the process noise at time k, respectively; rk is the mean value of the system observation noise at time k; δk is the Krone function; E⋅ is the mathematical expectation.


### 3.2. Adaptive Fading Unscented Kalman Filter Algorithm

The designed algorithm uses the covariance matching criterion to determine the divergence trend of the observer and designs an exponential weighted fading factor to adaptively correct the predicted covariance updating the proportion of data information to enhance the utilization of the newly observed data.At the same time, the suboptimal Sage–Husa noise estimator is adopted to estimate the unknown system process noise and estimate and correct the statistical characteristics of the noise ensuring the observer being in the best observation state.The specific steps of the algorithm are as follows:Step 1.
The initial value of the state estimation value of the filterx^0 and the initial value of the error covariance P0 can be expressed as(14)x^0=Ex0,P0=Ex0−x^0x0−x^0T.Step 2.
UT transformation
Firstly, the statistical characteristic of the Sigma point set is calculated.
For the n-dimensional state vector, the mean and variance vector of all Sigma point sets (sampling points) is assumed asx^k|k, Pk|k respectively.(15)Xk|ki=x^k|ki=0x^k|k+n+λPk|k1/2ii=1,2,⋯,nx^k|k−n+λPk|k1/2ii=n+1,n+2,⋯,2n,where Xk|ki is the state vector at sampling point k; n+λPk|k1/2i is the ith column of the square root of the variance matrix.
Then, the weight of each sampling point in the Sigma point set is calculated.(16)Wmi=λn+λi=0Wci=λn+λ+1−Γ2+κi=0Wmi=Wciλ2n+λi=1∼2n,where Wci and Wmi are the system mean and the weight of the covariance, respectively; λ is the scaling parameter and λ=Γ2n+τ−n; Γ is the scale parameter, and the choice of its value determines the distribution of the sampling points. A smaller positive number (10−4≤Γ≤1) is set usually. The value chosen for Γ in this paper is 0.01; τ is the proportional coefficient, τ=3−n usually; κ is a non-negative weight coefficient, which can combine the dynamic errors of the higher-order terms in the formula effectively. For the state variables affected by the Gaussian distribution, κ=2 is optimal.Step 3.
Time updating(17)xk+1|ki=fxk|ki,zki,x^k+1|k=∑i=02nWcixk+1|ki,Pk+1|k=∑i=02nWmix^k+1|k−xk+1|kix^k+1|k−xk+1|kiT+Qk,yk+1|ki=hxk+1|ki,zk+1i,y^k+1|k=∑i=02nWciyk+1|ki.Step 4.
Divergence determination and adaptive correction
Due to the data updating and utilization of the filter observer are not timely, the available data are stale, and the system noise leads to the divergence of the filter easily. Therefore, the trend of the filter divergence needs to be judged. So, the covariance matching criterion is used to judge the divergence.(18)y˜k+1Ty˜k+1≤ζtrEy˜k+1y˜k+1T,y˜k+1=yk+1−y^k+1|k,where y˜k+1 is the residual sequence matrix at time k+1; ζ is the preset adjustable coefficient greater than 1; tr⋅ is the trace calculation operation.
If the discriminant (18) is correct, then go to Step 5. If it is not correct, the filter diverges. Then, the adaptive weighting coefficient λk is designed to correct the prediction covariance Pk+1|k.(19)Pk+1|k=λk∑i=02nWmix^k+1|k−xk+1|kix^k+1|k−xk+1|kiT+Qk.
The adaptive weighting coefficientλk is determined by equations (20)–(22).(20)λk=λ0λ0≥11λ0<1,(21)λ0=trC0,k+1−RkTtr∑i=02nWmixk+1|ki−x^k+1|kyk+1|ki−y^k+1|kT,(22)C0,k+1=y˜k+1y˜k+1Tk=0ρC0,k+y˜k+1y˜k+1T1+ρk>0,where Rk is the covariance matrix of the system observation noise at time k; ρ is the fading coefficient, and 0<ρ≤1.
The larger theρ, the larger the proportion of the residual vector, and the smaller the proportion of information before time k, which improves the utilization of current data effectively.
When the vehicle is in a sudden change of working conditions such as braking, the filter observer is easy to diverge. The exponential weighted fading factor can correct and predict covariance adaptively reducing filtering divergence and still maintaining a stable estimation under normal operating conditions.Step 5.
Measurement updating
The system prediction variance matrixPykyk obtained by the weighted summation is(23)Pykyk=∑i=02nWmiyk+1|ki−y^k+1|kyk+1|ki−y^k+1|kT+Rk.
The system prediction covariance matrixPxkyk obtained by the weighted summation is(24)Pxkyk=∑i=12nWmixk+1|ki−x^k+1|kyk+1|ki−y^k+1|kT.
The gain matrixKk+1 is(25)Kk+1=Pxk+1yk+1Pyk+1yk+1−1,X^k+1|k+1=X^k+1|k+Kk+1yk+1|k−y^k+1|k,Pk+1|k+1=Pk+1|k−Kk+1PykykKk+1T.Step 6.
Process noise estimate updating
Due to the unknown characteristics of system noise, the suboptimal Sage–Husa noise estimator is used to estimate the mean matrixq^k and covariance matrix Q^k of the unknown system process noise.(26)q^k+1=1−dk+1q^k+dk+1X^k+1|k+1−∑i=02nWmiX^k+1|ki,Q^k+1=1−dk+1Q^k+dk+1kk+1y^k+1|ky˜k+1|kTKk+1T+pk+1|k+1−∑i=02nWcixk+1|ki−x^k+1|kΨk+1|ki−y^k+1|kT,where Ψk+1|ki=hx^k+1|ki+rk; dk+1=1−g/1−gk+1; g is the forgetting factor, 0.95< g < 0.99. The use of the forgetting factor g can limit the memory length of the filter. The larger the g is, the more the measurement data will have an effect on the current real-time estimation.
Under the premise that the noise statistical characteristics of the observer algorithm match the noise statistical characteristics of the actual model, the AFUKF algorithm can overcome the divergence of the observer effectively, correcting the prediction covariance adaptively. And the suboptimal Sage–Husa noise estimator can effectively track the statistical characteristics of noise improving the convergence of the estimation algorithm and filtering accuracy.


## 3.1. Design of the Algorithm

The sampling time of the algorithm is defined asΔt. Discretizing the state and the observation equations in equation (11), equation (12) can be obtained as follows:(12)xk+1=fxk,zk+ωkyk=hxk,zk+νk,where xk and yk are the state and output vectors at time k, respectively; zk is the control input vector at time k; ωk and νk are the process and observation noise vectors at time k, respectively.The process noiseωk and the system observation noise νk are both independent Gaussian white noises. And the noise characteristics are(13)Eωk=qk,Eνk=rkEωk−qkωk−qkT=Qkδk,where, qk and Qk are the mean and covariance matrixes of the process noise at time k, respectively; rk is the mean value of the system observation noise at time k; δk is the Krone function; E⋅ is the mathematical expectation.


## 3.2. Adaptive Fading Unscented Kalman Filter Algorithm

The designed algorithm uses the covariance matching criterion to determine the divergence trend of the observer and designs an exponential weighted fading factor to adaptively correct the predicted covariance updating the proportion of data information to enhance the utilization of the newly observed data.At the same time, the suboptimal Sage–Husa noise estimator is adopted to estimate the unknown system process noise and estimate and correct the statistical characteristics of the noise ensuring the observer being in the best observation state.The specific steps of the algorithm are as follows:Step 1.
The initial value of the state estimation value of the filterx^0 and the initial value of the error covariance P0 can be expressed as(14)x^0=Ex0,P0=Ex0−x^0x0−x^0T.Step 2.
UT transformation
Firstly, the statistical characteristic of the Sigma point set is calculated.
For the n-dimensional state vector, the mean and variance vector of all Sigma point sets (sampling points) is assumed asx^k|k, Pk|k respectively.(15)Xk|ki=x^k|ki=0x^k|k+n+λPk|k1/2ii=1,2,⋯,nx^k|k−n+λPk|k1/2ii=n+1,n+2,⋯,2n,where Xk|ki is the state vector at sampling point k; n+λPk|k1/2i is the ith column of the square root of the variance matrix.
Then, the weight of each sampling point in the Sigma point set is calculated.(16)Wmi=λn+λi=0Wci=λn+λ+1−Γ2+κi=0Wmi=Wciλ2n+λi=1∼2n,where Wci and Wmi are the system mean and the weight of the covariance, respectively; λ is the scaling parameter and λ=Γ2n+τ−n; Γ is the scale parameter, and the choice of its value determines the distribution of the sampling points. A smaller positive number (10−4≤Γ≤1) is set usually. The value chosen for Γ in this paper is 0.01; τ is the proportional coefficient, τ=3−n usually; κ is a non-negative weight coefficient, which can combine the dynamic errors of the higher-order terms in the formula effectively. For the state variables affected by the Gaussian distribution, κ=2 is optimal.Step 3.
Time updating(17)xk+1|ki=fxk|ki,zki,x^k+1|k=∑i=02nWcixk+1|ki,Pk+1|k=∑i=02nWmix^k+1|k−xk+1|kix^k+1|k−xk+1|kiT+Qk,yk+1|ki=hxk+1|ki,zk+1i,y^k+1|k=∑i=02nWciyk+1|ki.Step 4.
Divergence determination and adaptive correction
Due to the data updating and utilization of the filter observer are not timely, the available data are stale, and the system noise leads to the divergence of the filter easily. Therefore, the trend of the filter divergence needs to be judged. So, the covariance matching criterion is used to judge the divergence.(18)y˜k+1Ty˜k+1≤ζtrEy˜k+1y˜k+1T,y˜k+1=yk+1−y^k+1|k,where y˜k+1 is the residual sequence matrix at time k+1; ζ is the preset adjustable coefficient greater than 1; tr⋅ is the trace calculation operation.
If the discriminant (18) is correct, then go to Step 5. If it is not correct, the filter diverges. Then, the adaptive weighting coefficient λk is designed to correct the prediction covariance Pk+1|k.(19)Pk+1|k=λk∑i=02nWmix^k+1|k−xk+1|kix^k+1|k−xk+1|kiT+Qk.
The adaptive weighting coefficientλk is determined by equations (20)–(22).(20)λk=λ0λ0≥11λ0<1,(21)λ0=trC0,k+1−RkTtr∑i=02nWmixk+1|ki−x^k+1|kyk+1|ki−y^k+1|kT,(22)C0,k+1=y˜k+1y˜k+1Tk=0ρC0,k+y˜k+1y˜k+1T1+ρk>0,where Rk is the covariance matrix of the system observation noise at time k; ρ is the fading coefficient, and 0<ρ≤1.
The larger theρ, the larger the proportion of the residual vector, and the smaller the proportion of information before time k, which improves the utilization of current data effectively.
When the vehicle is in a sudden change of working conditions such as braking, the filter observer is easy to diverge. The exponential weighted fading factor can correct and predict covariance adaptively reducing filtering divergence and still maintaining a stable estimation under normal operating conditions.Step 5.
Measurement updating
The system prediction variance matrixPykyk obtained by the weighted summation is(23)Pykyk=∑i=02nWmiyk+1|ki−y^k+1|kyk+1|ki−y^k+1|kT+Rk.
The system prediction covariance matrixPxkyk obtained by the weighted summation is(24)Pxkyk=∑i=12nWmixk+1|ki−x^k+1|kyk+1|ki−y^k+1|kT.
The gain matrixKk+1 is(25)Kk+1=Pxk+1yk+1Pyk+1yk+1−1,X^k+1|k+1=X^k+1|k+Kk+1yk+1|k−y^k+1|k,Pk+1|k+1=Pk+1|k−Kk+1PykykKk+1T.Step 6.
Process noise estimate updating
Due to the unknown characteristics of system noise, the suboptimal Sage–Husa noise estimator is used to estimate the mean matrixq^k and covariance matrix Q^k of the unknown system process noise.(26)q^k+1=1−dk+1q^k+dk+1X^k+1|k+1−∑i=02nWmiX^k+1|ki,Q^k+1=1−dk+1Q^k+dk+1kk+1y^k+1|ky˜k+1|kTKk+1T+pk+1|k+1−∑i=02nWcixk+1|ki−x^k+1|kΨk+1|ki−y^k+1|kT,where Ψk+1|ki=hx^k+1|ki+rk; dk+1=1−g/1−gk+1; g is the forgetting factor, 0.95< g < 0.99. The use of the forgetting factor g can limit the memory length of the filter. The larger the g is, the more the measurement data will have an effect on the current real-time estimation.
Under the premise that the noise statistical characteristics of the observer algorithm match the noise statistical characteristics of the actual model, the AFUKF algorithm can overcome the divergence of the observer effectively, correcting the prediction covariance adaptively. And the suboptimal Sage–Husa noise estimator can effectively track the statistical characteristics of noise improving the convergence of the estimation algorithm and filtering accuracy.


## 4. Numerical Simulation and Experimental Verification

### 4.1. Numerical Simulation

A certain type of vehicle is verified by a simulation test on the virtual prototype software ADAMS.And the real vehicle used for test is the same with the virtual vehicle model in Adams. That is to say, the virtual vehicle model in Adams is determined according to the given real test vehicle. The parameters of the vehicle dynamics model in ADAMS are the same as our test vehicle shown in Table1.Table 1
Simulation parameters.
ParameterValuem (kg)1525Iz (kg∙m2)2440a (m)1.48b (m)1.08h(m)0.432tf (m)1.52tr (m)1.59re (m)0.33ADAMS/Car is a template-based modeling and simulation tool that greatly accelerates and simplifies the modeling steps. Users only need to input the necessary data in the template to quickly construct a high-precision virtual prototype of the whole vehicle including the body, suspension, transmission system, engine, steering mechanism, and braking system, and simulate it. Using the database function of ADAMS/Car, bushings, limit blocks, shock absorbers, etc. can be effectively selected to assemble various subsystems, saving users’ time to repeatedly input data each time. The dynamic response of the whole vehicle under various test conditions is visually displayed through high-speed animation, and characteristic parameters indicating handling stability, braking, riding comfort, and safety are output reducing the dependence on physical prototypes. A complete suspension or vehicle assembly consists of the following four levels: characteristic files, template files, subsystem files, and assembly files.Firstly, the model of each subsystem is built which is then assembled to form a vehicle model. When using ADAMS software to establish the model of each subsystem of the vehicle, the following steps are followed: simplifying the model, determining hard points and dynamic parameters, creating component geometry, defining constraint template parameter variables, and testing the sequence of communicators. Combined with the parameters of the vehicle, as shown in Table1, the vehicle model is established in the ADAMS/Car module. In order to facilitate the modeling, the vehicle needs to be simplified. The simplified model mainly includes the front and rear suspension systems, the steering system, the braking system, the power system, the tires, the body, etc. The hard point coordinates, performance, and structural parameters of each assembly are mainly obtained with reference to the real vehicle. The built subsystem models are assembled through the corresponding communicators, and the vehicle model can be obtained, as shown in Figure 2.Figure 2
Vehicle model in ADAMS.The double lane change road is used. The entire operation is set to 10 seconds, and the sampling time is 0.01 second.Observational measurements with noise interference are shown in Figure3.Figure 3
Observational measurement:s (a) yaw rate, (b) lateral acceleration, and (c) longitudinal velocity.
(a)(b)(c)Figure4 shows the simulation results for a vehicle passing a double lane change road.Figure 4
Simulation results for a vehicle passing a double lane change road: (a) estimated longitudinal velocity, (b) estimated lateral velocity, (c) estimated longitudinal acceleration, (d) estimated lateral acceleration, (e) estimated yaw rate, and (f) estimated sideslip angle.
(a)(b)(c)(d)(e)(f)From Figure4, it can be seen that trend of the curves between the estimated and the virtual test values is consistent indicating the designed AFUKF algorithm having a good response speed and estimation accuracy as well as strong adaptive and antijamming capabilities.The estimation effects of different algorithms (AFUKF, CKF, and UKF) are then compared under the condition of tracking a double lane change road. Figure5 is the simulation result of the key states.Figure 5
Comparison results of the key variables simulated with the different methods (AFUKF, CKF, and UKF) for passing a double lane change road maneuver. (a) Estimated longitudinal velocity, (b) estimated lateral velocity, (c) estimated longitudinal acceleration, (d) estimated lateral acceleration, (e) estimated yaw rate, and (f) estimated sideslip angle.
(a)(b)(c)(d)(e)(f)From Figure5, it can be seen that the tracking ability of the estimated value obtained by the AFUKF algorithm relative to the virtual test value is higher than that of the CKF and UKF algorithm. The AFUKF algorithm realizes the judgment of the divergence trend of the observer by applying the covariance matching criterion. At the same time, the adaptive weighting coefficient can correct the prediction covariance, which enhances the utilization of the new observed data. Aiming at the influence of system noise, the suboptimal Sage–Husa noise estimator is used to estimate and correct the unknown system process noise in real time. And the forgetting factor is used to limit the memory length of the estimator, which enhances the real-time tracking ability of the nonlinear observer and the stability of the numerical estimation. It can be seen from Figure 5 that the difference between the three methods in the estimation of longitudinal acceleration is obvious, and the deviation between the CKF and the virtual test value as well as the UKF method and the virtual test value are obviously larger than that of the AFUKF algorithm. The CKF makes full use of the cubature integral value to calculate the multidimensional function integral, which has the characteristics of high efficiency. However, if the system state changes suddenly, the effect of CKF on tracking and estimating the state will become worse. The same trend can also be seen from Figure 5 that because the model uses inaccurate initial parameters and the UKF algorithm does not have a parameter correction estimator, its estimation accuracy is significantly lower than that of the AFUKF algorithm. When estimating the yaw rate state quantity, there is a serious distortion. So, the algorithm can enhance the stability of filtering and improve the estimation accuracy of the algorithm for vehicle state estimation problem.The error indicators which are the average absolute error (MAE) and root mean square error (RMSE) described as equations (27) and (28) of the different algorithms given in Table 2.(27)MAEx=∑k=1Nxk|k−xactualkN,(28)RMSEx=∑k=1Nxk|k−xactualk2N,where xk|k and xactualk are the estimated and the virtual test values at time k, respectively.Table 2
Error Comparison between the AFUKF, CKF, and the UKF algorithm.
AFUKFCKFUKFMAEv (m/s)0.06330.06380.0658ay (m/s2)0.4580.4760.547r (rad/s)0.01620.01640.0220β (rad)0.00170.00220.0024RMSEv (m/s)0.1170.1200.121ay (m/s2)0.660.6790.65r (rad/s)0.02610.02710.0291β (rad)0.003360.003400.00343Table2 and Figure 5 indicate that the AFUKF algorithm has higher estimation accuracy compared with the CKF and the UKF algorithms, for the vehicle estimation problem under the same simulation conditions.


### 4.2. Experimental Verification

In order to further verify the effectiveness of the proposed AFUKF algorithm in vehicle state estimation, a real-vehicle test of a certain type of vehicle is carried out on a double lane change road field. The test is carried out in accordance with ISO/TR3888-2004, and the test vehicle speed is 80 km/h (±3 km/h).A gyroscope (Figure6(a)) is installed on the vehicle to collect the yaw rate and lateral acceleration of the vehicle in real time. A GPSSD-20 speed instrument (Figure 6(b)) is used to measure the longitudinal speed of the vehicle. In addition, a steering torque/angle tester (Figure 6(c)) is used to measure the steering wheel.Figure 6
Measurement equipment: (a) gyroscope, (b) GPSSD-20 speed instrument, and (c) steering torque/angle tester.
(a)(b)(c)Figure7 shows the comparison result of the estimated value obtained by the AFUKF method and the actual vehicle test value of the three state variables.Figure 7
Comparison result of the estimated and test values. (a) Test and estimated values of the longitudinal velocity. (b) Test and estimated values of the lateral acceleration. (c) Test and estimated values of the yaw rate.
(a)(b)(c)From the comparison, it can be seen that there is a certain error. This is because the adopted Pacejka tire model still has a certain deviation when simulating the mechanical properties of the real vehicle tires. At the same time, the measurement error as well as installation position of the sensor are also important reasons for the deviation between the estimated value and the test value. In addition, this parameter uses the historical value of the last moment in the discretization process, which causes the cumulative effect of errors.However, the estimated value basically keeps in line with the experimental value in trend indicating the correction of the proposed AFUKF method for solving the vehicle state estimation problem.


## 4.1. Numerical Simulation

A certain type of vehicle is verified by a simulation test on the virtual prototype software ADAMS.And the real vehicle used for test is the same with the virtual vehicle model in Adams. That is to say, the virtual vehicle model in Adams is determined according to the given real test vehicle. The parameters of the vehicle dynamics model in ADAMS are the same as our test vehicle shown in Table1.Table 1
Simulation parameters.
ParameterValuem (kg)1525Iz (kg∙m2)2440a (m)1.48b (m)1.08h(m)0.432tf (m)1.52tr (m)1.59re (m)0.33ADAMS/Car is a template-based modeling and simulation tool that greatly accelerates and simplifies the modeling steps. Users only need to input the necessary data in the template to quickly construct a high-precision virtual prototype of the whole vehicle including the body, suspension, transmission system, engine, steering mechanism, and braking system, and simulate it. Using the database function of ADAMS/Car, bushings, limit blocks, shock absorbers, etc. can be effectively selected to assemble various subsystems, saving users’ time to repeatedly input data each time. The dynamic response of the whole vehicle under various test conditions is visually displayed through high-speed animation, and characteristic parameters indicating handling stability, braking, riding comfort, and safety are output reducing the dependence on physical prototypes. A complete suspension or vehicle assembly consists of the following four levels: characteristic files, template files, subsystem files, and assembly files.Firstly, the model of each subsystem is built which is then assembled to form a vehicle model. When using ADAMS software to establish the model of each subsystem of the vehicle, the following steps are followed: simplifying the model, determining hard points and dynamic parameters, creating component geometry, defining constraint template parameter variables, and testing the sequence of communicators. Combined with the parameters of the vehicle, as shown in Table1, the vehicle model is established in the ADAMS/Car module. In order to facilitate the modeling, the vehicle needs to be simplified. The simplified model mainly includes the front and rear suspension systems, the steering system, the braking system, the power system, the tires, the body, etc. The hard point coordinates, performance, and structural parameters of each assembly are mainly obtained with reference to the real vehicle. The built subsystem models are assembled through the corresponding communicators, and the vehicle model can be obtained, as shown in Figure 2.Figure 2
Vehicle model in ADAMS.The double lane change road is used. The entire operation is set to 10 seconds, and the sampling time is 0.01 second.Observational measurements with noise interference are shown in Figure3.Figure 3
Observational measurement:s (a) yaw rate, (b) lateral acceleration, and (c) longitudinal velocity.
(a)(b)(c)Figure4 shows the simulation results for a vehicle passing a double lane change road.Figure 4
Simulation results for a vehicle passing a double lane change road: (a) estimated longitudinal velocity, (b) estimated lateral velocity, (c) estimated longitudinal acceleration, (d) estimated lateral acceleration, (e) estimated yaw rate, and (f) estimated sideslip angle.
(a)(b)(c)(d)(e)(f)From Figure4, it can be seen that trend of the curves between the estimated and the virtual test values is consistent indicating the designed AFUKF algorithm having a good response speed and estimation accuracy as well as strong adaptive and antijamming capabilities.The estimation effects of different algorithms (AFUKF, CKF, and UKF) are then compared under the condition of tracking a double lane change road. Figure5 is the simulation result of the key states.Figure 5
Comparison results of the key variables simulated with the different methods (AFUKF, CKF, and UKF) for passing a double lane change road maneuver. (a) Estimated longitudinal velocity, (b) estimated lateral velocity, (c) estimated longitudinal acceleration, (d) estimated lateral acceleration, (e) estimated yaw rate, and (f) estimated sideslip angle.
(a)(b)(c)(d)(e)(f)From Figure5, it can be seen that the tracking ability of the estimated value obtained by the AFUKF algorithm relative to the virtual test value is higher than that of the CKF and UKF algorithm. The AFUKF algorithm realizes the judgment of the divergence trend of the observer by applying the covariance matching criterion. At the same time, the adaptive weighting coefficient can correct the prediction covariance, which enhances the utilization of the new observed data. Aiming at the influence of system noise, the suboptimal Sage–Husa noise estimator is used to estimate and correct the unknown system process noise in real time. And the forgetting factor is used to limit the memory length of the estimator, which enhances the real-time tracking ability of the nonlinear observer and the stability of the numerical estimation. It can be seen from Figure 5 that the difference between the three methods in the estimation of longitudinal acceleration is obvious, and the deviation between the CKF and the virtual test value as well as the UKF method and the virtual test value are obviously larger than that of the AFUKF algorithm. The CKF makes full use of the cubature integral value to calculate the multidimensional function integral, which has the characteristics of high efficiency. However, if the system state changes suddenly, the effect of CKF on tracking and estimating the state will become worse. The same trend can also be seen from Figure 5 that because the model uses inaccurate initial parameters and the UKF algorithm does not have a parameter correction estimator, its estimation accuracy is significantly lower than that of the AFUKF algorithm. When estimating the yaw rate state quantity, there is a serious distortion. So, the algorithm can enhance the stability of filtering and improve the estimation accuracy of the algorithm for vehicle state estimation problem.The error indicators which are the average absolute error (MAE) and root mean square error (RMSE) described as equations (27) and (28) of the different algorithms given in Table 2.(27)MAEx=∑k=1Nxk|k−xactualkN,(28)RMSEx=∑k=1Nxk|k−xactualk2N,where xk|k and xactualk are the estimated and the virtual test values at time k, respectively.Table 2
Error Comparison between the AFUKF, CKF, and the UKF algorithm.
AFUKFCKFUKFMAEv (m/s)0.06330.06380.0658ay (m/s2)0.4580.4760.547r (rad/s)0.01620.01640.0220β (rad)0.00170.00220.0024RMSEv (m/s)0.1170.1200.121ay (m/s2)0.660.6790.65r (rad/s)0.02610.02710.0291β (rad)0.003360.003400.00343Table2 and Figure 5 indicate that the AFUKF algorithm has higher estimation accuracy compared with the CKF and the UKF algorithms, for the vehicle estimation problem under the same simulation conditions.


## 4.2. Experimental Verification

In order to further verify the effectiveness of the proposed AFUKF algorithm in vehicle state estimation, a real-vehicle test of a certain type of vehicle is carried out on a double lane change road field. The test is carried out in accordance with ISO/TR3888-2004, and the test vehicle speed is 80 km/h (±3 km/h).A gyroscope (Figure6(a)) is installed on the vehicle to collect the yaw rate and lateral acceleration of the vehicle in real time. A GPSSD-20 speed instrument (Figure 6(b)) is used to measure the longitudinal speed of the vehicle. In addition, a steering torque/angle tester (Figure 6(c)) is used to measure the steering wheel.Figure 6
Measurement equipment: (a) gyroscope, (b) GPSSD-20 speed instrument, and (c) steering torque/angle tester.
(a)(b)(c)Figure7 shows the comparison result of the estimated value obtained by the AFUKF method and the actual vehicle test value of the three state variables.Figure 7
Comparison result of the estimated and test values. (a) Test and estimated values of the longitudinal velocity. (b) Test and estimated values of the lateral acceleration. (c) Test and estimated values of the yaw rate.
(a)(b)(c)From the comparison, it can be seen that there is a certain error. This is because the adopted Pacejka tire model still has a certain deviation when simulating the mechanical properties of the real vehicle tires. At the same time, the measurement error as well as installation position of the sensor are also important reasons for the deviation between the estimated value and the test value. In addition, this parameter uses the historical value of the last moment in the discretization process, which causes the cumulative effect of errors.However, the estimated value basically keeps in line with the experimental value in trend indicating the correction of the proposed AFUKF method for solving the vehicle state estimation problem.


## 5. Conclusions

We proposed a novel algorithm to research the problem of vehicle state estimation. Accordingly, a 7-DOF nonlinear vehicle dynamics model combined with the Pacejka tire model was used to describe the motion of the vehicle. Then, the problem of vehicle state estimation was solved by the proposed algorithm at double lane change road condition. Compared with the traditional CKF and UKF, the simulated results, which were verified with a real vehicle test, showed that the calculation accuracy and the convergence speed of AFUKF algorithm were higher. The results of virtual test was based on ADAMS/Car, and real vehicle test showed that the AFUKF algorithm had good state estimation accuracy and model parameter correction ability when the initial parameters of the vehicle model were inaccurate and the time-varying statistical characteristic noise was included. Under the environment of adopting inaccurate initial model parameters and time-varying noise system, the state estimation accuracy of AFUKF algorithm was significantly higher than that of the UKF algorithm. Because the UKF algorithm did not have a parameter correction estimator, it caused serious distortion when estimating the yaw rate state quantity.It is believed that the AFUKF algorithm has theoretical guiding significance for the design of the estimator in the automotive dynamic control system and can also provide a theoretical reference for the research on the key state and parameter test methods of the vehicle.



---

*Source: 7355110-2022-04-26.xml*