Vehicle state and parameter estimation based on adaptive anti-outlier unscented Kalman filter and GA-BPNN method

2.1. 3-DOF vehicle model

Based on reasonable assumptions and simplification, this paper obtains a nonlinear vehicle dynamics model including the longitudinal and lateral yaw motion of the vehicle body as shown in Fig. 1:

where $m$ is the vehicle mass; $I_z$ is the moment of inertia around the $z$ axis; $\phi$, $\dot{\phi }$ and $\ddot{\phi }$ are the yaw, yaw rate and yaw angular acceleration of the vehicle respectively; $a$ and $b$ are the distances of front and rear axles from the center of gravity; $F_{yf}$ and $F_{yr}$ are the lateral forces of front and rear tires; $F_T$ is the longitudinal force of tire; $\dot{x}$ and $\dot{y}$ are the speed in vehicle coordinate system; $\ddot{x}$ and $\ddot{y}$ are the accelerations in the vehicle coordinate system.

The motion of a vehicle in the global coordinate system can be represented as:

where $\dot{X}$ and $\dot{Y}$ are velocities of the lateral and longitudinal motions of vehicles in the global coordinate system.

Fig. 13-DOF vehicle model

2.2. Tire model

The semi empirical tire model composed of the Magic formula (MF) reflects the dynamic response between the tire and the ground, and has high fitting accuracy when the vehicle has small lateral acceleration and tire slip angle. However, this model cannot reflect the differences in adhesion characteristics between different road surfaces. The expression is as follows:

where $F_{z(f,r)}$ is the vertical load on front and rear wheels; ${\alpha }_{(f,r)}$ is the sideslip angle of the front and rear wheels; $B$ is the stiffness factor; $C$ is the shape factor; $D$ is the peaking factor; $E$ is the curvature factor.

The calculation expressions for each parameter are: $C=a_0$; $D=a_1{F}_{z(f,r)}^2+a_2{F}_{z(f,r)}$; $B=a_3\mathrm{s}\mathrm{i}\mathrm{n}\left(2\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\right(F_{z(f,r)}/a_4\left)\right)/CD$; $E=a_5{F}_{z(f,r)}+a_6$; ${\alpha }_f={\delta }_f-(\dot{y}+a\dot{\phi })/\dot{x}$; ${\alpha }_r=(b\dot{\phi }-\dot{y})/\dot{x}$, where $a_i\mathrm{}(i=\mathrm{1,2},\cdots ,6)$ are the fitted MF parameters based on experimental data; ${\delta }_f$ is the front steering angle. According to [22], the related parameters are set as: $B=$0.237, $C=$1.65, $D=$3610.5, $E=$0.707.

The vertical load of the front and rear wheels is:

2.3. Nonlinear vehicle system

The state vector of the nonlinear vehicle system is set as $\mathbf{x}=[v_x,v_y,r{]}^T$, the system input is $\mathbf{u}=[a_x{]}^T$, and the observation vector is $\mathbf{y}=[a_y,r]$.

3.1. Design of BP network algorithm based on genetic optimization

According to the design idea of the BP neural network algorithm optimized by the genetic algorithm, the initial weights and thresholds are optimized in the network. The fitness function is determined as:

where $T_i$ represents the actual output value of the $i$th training sample; $Y_i$ represents the expected output value of the $i$th training sample.

When the BP algorithm is optimized with the genetic algorithm, the roulette wheel gambling method is usually selected. In general, some individuals with excessive fitness will appear during population evolution, which are likely to determine the selection process reducing population diversity. Therefore, based on the GA algorithm used in reference, a new roulette selection method is proposed. A new roulette selection method is proposed, that is, each selected individual is removed from the total selection sequence and will not participate in the next selection process. The specific selection steps are:

Step 1: The individuals are arranged according to their fitness values, and the fitness values obtained by substituting them one by one are accumulated, and the sum is recorded as $S$.

Step 2: The random number $M$ is generated, and introduced in the formula: $M\in (0,S)$.

Step 3: The fitness function value of the first individual is taken and summed with the fitness function value of the following individuals in turn. If the cumulative value exceeds $M$, the calculation stops. The last individual whose fitness function value is accumulated is the selected parent.

Step 4: These selected individuals are put out separately and Steps 2 and 3 are repeated until a sufficient number of parents is accumulated.

According to the steps of the improved roulette selection algorithm, $N$/2 individuals are selected as the parents. This effectively prevents those individuals with abnormal fitness values from being selected for many times, enriching the diversity of the population, and improving the possible convergence of the algorithm to the local optimum.

3.2. GA-BPNN-Optimized UKF

In the paper, the improved BP neural network is used to optimize the UKF algorithm. The BP neural network is trained according to the input samples saving the trained weights and thresholds. When the trajectory parameters are estimated by the UKF, the parameters that affect the trajectory error are used as the input data for the improved BP network. So the global error can be adjusted to correct the UKF output results, thereby improving the trajectory measurement accuracy. The specific training steps are as follows:

Step 1: The error between the one-step state prediction value and the state estimation value is taken as the input sample.

Step 2: The error between the real value and the state estimate is taken as the output sample.

Step 3: The mapping relationship between UKF prediction and actual error is studied.

Step 4: The error $B_{perr}$ between the filtered and the actual value is outputted.

Firstly, the measurement information is inputted to the UKF filter to get the filtering results. And then the ${\widehat{X}}_{k+1\left|k+1\right.}-{\widehat{X}}_{k+1\left|k\right.}$ is inputted to the trained improved BP neural network. Finally, the error between the filter and actual values is output.

Finally, based on the GA-BPNN UKF algorithm the optimal estimation obtained through the UKF and the output value of the improved BP network are added to obtain the optimal estimation:

where ${\widehat{X}}_{k+1\left|k+1\right.}^{/}$ is the output value of the UKF algorithm based on the GA-BPNN; $B_{perr}$ is the output value of the improved BP network; ${\widehat{X}}_{k+1\left|k+1\right.}$ is the state estimate of the UKF.

3.3. Anti-outlier UKF based on GA-BPNN

The improved UKF algorithm mentioned above can effectively improve the filtering accuracy, but for the data gross error, the algorithm lacks the anti-interference ability. And when the measuring equipment suddenly fails, it means that its fault tolerance ability is also poor. In order to solve the above problems, a fault tolerant identification algorithm is proposed. The algorithm combines the above improved UKF algorithm with outlier elimination, assesses the innovation sequence and processes outlier points, adjusts the filter gain in real time and calculates outliers, and uses this technology to remove and repair dynamic data streams with speckled outliers or isolated outliers.

It is set that the innovation property is:

When the filter works stably, the standard deviation of innovation is $\sigma$ and $\sigma =\sqrt{\frac{1}{2n}{\omega }_i^{\left(c\right)}\left[\right(\xi {(}_{k+1\left|k\right.}^{\left(i\right)}-{\widehat{\mathrm{Z}}}_{k+1\left|k\right.}\left)\right]\left[\right(\xi {(}_{k+1\left|k\right.}^{i)}-{\widehat{\mathrm{Z}}}_{k+1\left|k\right.}){]}^T+R_{k+1}}$.

A definition and identification method is applied to assess whether each component of the observed value $Z_{k+1}$ is an outlier. The identification formula is:

where $(i,i)$ represents the $i$th element on the diagonal of innovation standard deviation; $(e_{k+1}{)}_i$ represents the $i$th component of $e_{k+1}$; and C represents a constant.

If the above identification formula is valid, then $(Z_{k+1}{)}_i$ is the normal observation. If the identification formula is not satisfied, then $(Z_{k+1}{)}_i$ is an outlier. And $(Z_{k+1}{)}_i$ represents the $i$th component of $Z_{k+1}$. In the improved UKF algorithm, since there is not only one type of outliers, it is necessary to distinguish them all and remove one by one.

For the removal of isolated outliers, according to the recursive formula of UKF, the state estimation value ${\widehat{X}}_{k+1\left|k+1\right.}$ can be obtained by modifying the prediction value ${\widehat{Z}}_{k+1\left|k\right.}$. In this case, the gain matrix $K_{k+1}$ determines the influence strength of $Z_{k+1}$ on ${\widehat{X}}_{k+1\left|k+1\right.}$. Therefore, if you want to get the correct ${\widehat{X}}_{k+1\left|k+1\right.}$, then $Z_{k+1}$ cannot be distorted. If $Z_{k+1}$ is distorted, it is necessary to adjust $K_{k+1}$ to obtain accurate ${\widehat{X}}_{k+1\left|k+1\right.}$.

If the $i$th component $(Z_{k+1}{)}_i$ of $Z_{k+1}$ does not meet the identification formula conditions, then $(Z_{k+1}{)}_i$ is an outlier. After obtaining $K_{k+1}$, let $K_{k+1}=m{K}_{k+1}(0\le m\le 1)$, where $m$ is similar to a weight coefficient, and its value depends on the size of the innovation value. When the innovation value is high, it is necessary to reduce the gain $K_{k+1}$, so that $m$ can effectively adjust it by selecting the small value between (0, 1). If the innovation value is very high, it is necessary to set $K_{k+1}$ to zero, and the value of $m$ at this time shall be 0. The adaptive control of the filter is realized. Then ${\widehat{X}}_{k+1\left|k+1\right.}$ and filtering error covariance $P_{k+1\left|k+1\right.}$ can be obtained, thus the problem of outlier point interference can be solved by obtaining the target state parameters.

For a speckled outlier, the removal steps are as follows:

Step 1: Firstly, the values ${\widehat{X}}_{k+1\left|k+1\right.}$ and $Z_{k+1}$ are calculated by using the improved UKF, and the data are saved at the same time.

Step 2: the identification formula is used to identify whether $Z_{k+1}$ is an abnormal value.

Step 3: Then Step 2 is repeated and the point sequence k of each outlier is saved, while the identification, and recording of the number of outliers continue.

Step 4: The predicted values are used instead of outliers.

Step 5: Filtering is continued until the end.

The flowchart of the adaptive Anti-Outlier UKF and GA-BPNN is shown in Fig. 2.

4.1. Numerical simulation

Carsim and Matlab/Simulink software are used to conduct co-simulation experiments to verify the estimation algorithm. Carsim is a professional vehicle dynamics simulation software developed by the Mechanical Simulation Corporation (MSC) for feature oriented parametric modeling. It is aimed at making algorithm research and development in the automotive field, testing and shortening the vehicle cycle. Scholars or engineers can obtain operational results that can highly simulate the response of actual vehicles through Carsim, and conduct more in-depth analysis on them.

Fig. 2Flowchart of adaptive Anti-Outlier UKF and GA-BPNN

The main interface of Carsim consists of three parts:

Database. The database includes databases of the entire vehicle model, driver model, and external environment (road surface information, environmental wind information, etc.). These databases comprehensively consider the human vehicle road factors of the automotive operating environment. During the operation, you can either default to an already existing database or create a new database.

Model solver. This section is the core part of Carsim operation solving, where users can set simulation termination conditions, simulation step size, and other information. It can also be easily connected to Matlab/Simulink, C language, and VB language.

Post-processing. Carsim software has powerful post-processing capabilities. During its operation, the user can choose to analyze quantitatively the curve of a specific characteristic changing over time or other parameters, or observe the response of the vehicle visually and vividly through simulation animations. In general, Carsim is easy to use, its graphical interface is easy to operate, and data visualization is powerful. The accuracy of its mathematical model has been recognized by academia and the industry, because the software has excellent scalability, which can be easily integrated into Matlab/Simulink, C language, VB and other simulation environments.

The key parameters of the vehicle are shown in Table 1.

The computing platform comprises a MacBook Air (Intel Core i5-5250U 1.6 GHz, Windows 10 Enterprise Edition) with MATLABR2009a. The BPNN is set as: the structure has 2 nodes in the input layer; 5 nodes in the hidden layer, and 1 node in the output layer, with a total of 15 nodes. And the running interval is 1 s.

Fig. 3Longitudinal velocity simulation

Fig. 3 compares and analyzes the estimated longitudinal velocity using the traditional EKF algorithm and the GA-BPNN algorithm proposed in this paper with the Carsim output parameter values. From Fig. 3, it can be observed that the estimation performance of the GA-BPNN algorithm is significantly better than that of EKF, with the maximum error of only 0.1 km/h. The numerical fluctuations can be stable and close to the actual values. The maximum error of EKF method reaches 0.113 km/h. Its estimated values are always near the actual values with very small errors, while the traditional EKF algorithm exhibits divergence at certain times.

Fig. 4Yaw rate simulation

Fig. 4 compares and analyzes the estimated yaw rate using the traditional EKF algorithm and the GA-BPNN algorithm proposed in this paper with the Carsim output parameter values. From Fig. 4, it can be observed that the estimation performance of the GA-BPNN algorithm is significantly better than that of EKF. Traditional EKF exhibits more than one divergence phenomenon in 10 seconds, while the GA-BPNN algorithm maintains good convergence, with its estimated value located near the actual value, with the maximum error of only 0.1 (°/s), so the numerical fluctuation can be in a stable state.

Fig. 5Side slip angle simulation

Fig. 5 compares and analyzes the estimated side slip angle using the traditional EKF algorithm and the GA-BPNN algorithm proposed in this paper with the Carsim output parameter values. From Fig. 5, it can be observed that the estimation performance of the GA-BPNN algorithm is significantly better than that of EKF. According to the simulation results, it can be concluded that the GA-BPNN method can correct the noise covariance through the involvement of parameter optimization estimation methods. Therefore, the GA-BPNN algorithm has excellent estimation accuracy and good convergence, which can effectively solve the problem of vehicle driving state estimation when the noise covariance is not accurately obtained.

The mean absolute error (MAE) and root mean square error (RMSE) are considered to verify the estimation accuracy of the proposed algorithm.

From Table 2, it can be seen more intuitively that the estimation accuracy of the GA-BPNN algorithm is significantly higher than that of the EKF method.

4.2. Experimental verification

According to BS ISO 3888-2002, a double lane change test is carried out to verify the algorithm effectiveness. The main measurement devices are shown in Fig. 6. And the real test vehicle is shown in Fig. 7.

Fig. 6Measurement devices

a) GPSSD-20 speed instrument

b) Steering torque/angle tester

c) AM-2800 vehicle comprehensive performance test system

Fig. 7Real test vehicle

Fig. 8 depicts a comparison of the estimated and test values.

It can be seen from Fig. 8 that due to the existence of the vehicle dynamics model, tire model and sensor measurement errors, the errors of the longitudinal velocity, yaw rate and side slip angle appear as estimated. But the overall estimation effect is good, and it maintains a good consistency with the actual measured value indicating the good estimation accuracy and robustness of the proposed algorithm.