Moving horizon estimation of vehicle state and parameters

. For the active safety control of the vehicle, it is extremely important to estimate the vehicle state in real-time and accurately during the driving process. A joint state and parameter estimation method based on QR decomposition and receding horizon estimation (RHE) is proposed. Firstly, by introducing the receding horizon strategy, the authors optimized the state and parameter estimation with a fixed number of variables, which can better deal with the estimation problem of time-varying parameters. Then, based on the principle of forward dynamic programming, the calculation of arrival cost is transformed into a least square equation, which is solved by QR decomposition. At the same time, an update method of arrival cost based on QR decomposition is given. In this way, the whole receding horizon estimation method is based on the optimization, and the feedback mechanism is introduced to improve the estimation accuracy and speed. The simulation results show that the accuracy of receding horizon estimation is obviously better than that of unscented Kalman filter (UKF), and the arrival cost update method based on QR decomposition is more convenient than the traditional arrival cost update method based on error covariance estimation.


Introduction
The vehicle active safety system can greatly improve the risk avoidance ability of the vehicle and effectively reduce the accident rate. The premise of its role is to obtain accurately and timely the current operation state, such as side slip angle, yaw rate and road adhesion, and then control the vehicle to maximize the driving safety by assessing the current vehicle state information. Although with the development of sensor technology, the vehicle status information can be obtained by adding corresponding sensors, it will also increase the manufacturing cost of vehicles, which is not conducive to the mass production of vehicles. The premise for the automobile active safety system to play its role is to obtain accurately and timely the current vehicle driving state information and road adhesion rate, and then to design a more rigorous control strategy to ensure the driving safety of the automobile through more complex logical relationship assessment. In recent years, the sensor technology, as an indispensable part of the control system, has developed rapidly. Although the current running state information of the vehicle can also be obtained by adding corresponding sensors, namely by adding a speedometer to obtain the speed information. However, it will also increase the corresponding production cost, which is not conducive to the mass production of the vehicle. At the same time, the estimation accuracy of some sensors, such as optical road adhesion coefficient sensor, is greatly affected by the environment and is prone to miscalculation. Based on the sensor information of the existing vehicle configuration, the method of using the corresponding algorithm to estimate the important parameters of vehicle can both reduce the production cost, and can have no constraints from operating environment. Moreover, even if the corresponding sensor is added to obtain the vehicle running state information, the results estimated by the algorithm can also provide the reference information for the control system or signalize to replace the sensor when it fails. Therefore, it has good economic benefits and engineering application value [1].
Although the existing research results have basically provided a ground for estimating the vehicle driving state and road adhesion rate, the estimation accuracy and efficiency of the algorithm may be further improved. For the vehicle running state estimation, the widely used method is based on the model that can describe the vehicle dynamic characteristics and Kalman filter or its improved algorithm. The estimation accuracy of this method is closely related to the accuracy of the dynamic model. But most results usually ignore the time-varying characteristics of the lateral stiffness in the vehicle driving state estimation model, resulting in the possibility of further improving the estimation accuracy. During the actual driving process of the vehicle, especially when the vehicle is in dangerous operating conditions such as sharp acceleration and sharp turning, the dynamic load of the wheel will change greatly due to the influence of body roll, acceleration and deceleration. And the tire cornering stiffness is more sensitive to the vertical load, which will make the tire cornering stiffness in the estimated model be greatly different from the initial value. So that the vehicle dynamics model calibrated according to the initial state may deviate greatly from the actual state, affecting the accuracy of vehicle driving state estimation. Therefore, the influence of wheel dynamic load shall be considered to improve the accuracy of the estimation model. This will make it possible to improve the total estimation accuracy. However, two methods commonly used in dealing with the estimation of pavement peak adhesion coefficient, namely, the estimation method based on single wheel dynamics model and the estimation method based on vehicle dynamics model, have some limitations. The first estimation method is usually based on a vehicle dynamics model with addition of the maximum adhesion coefficient, which can be transformed into an explicit tire model. And the maximum adhesion coefficient is estimated by using relevant algorithms such as Kalman filter algorithm. The estimation method based on single wheel dynamics model usually estimates the tire force firstly, and then estimates the adhesion coefficient by using the relationship between the existing mature tire model and tire force. The Dugoff tire model commonly used by the two methods cannot accurately describe the dynamic characteristics of the nonlinear region of the tire, so the estimation accuracy is low under the condition of high slip rate. Moreover, the estimation method based on the vehicle dynamics model has the problems of huge adjustment workload of the initial value of the model, which is unable to adapt to complex and changeable conditions and slow operation efficiency [2].
The problem of vehicle state estimation has been widely studied. A brief review is presented in what follows.
Wang et al. proposed a scheme of new Kalman filter to improve the estimation effect of noise interference in engineering practice [3]. Using an error-state extended Kalman filter, Liang et al. presented a scalable concept for vehicle state estimation [4]. The UKF algorithm introduces the idea of lossless transformation, which effectively overcomes the problems of low estimation accuracy and poor stability of EKF. The accuracy of the vehicle parameter estimation results will drop significantly when the system is strongly nonlinear [5]. Based on fusion of the machine learning model and the vehicle dynamics model, Yu et al. proposed a vehicle mass estimation method for intelligent vehicles [6]. Zhang et al. proposed a novel modified UKF state estimation methodology for the stability control of an electric vehicle [7]. To improve the safety and stability of land vehicles, Song et al. explored the estimation problem for different vehicle states [8]. Zhang et al. proposed a distributed bearing-based formation control scheme to extend the application domain for unmanned aerial vehicle swarm via global orientation estimation [9]. In order to improve the filtering accuracy of the filter algorithm, Liu et al. used an unscented Kalman filter and a genetic-particle swarm algorithm to estimate several vehicle key states [10,11]. Yang et al. proposed the recursive least squares method to identify parameters of the battery [12]. Shereena et al. [13] proposed a novel method to deal with the simultaneous identification of road roughness. The idea of particle filter (PF) is to use a particle set to represent the probability which value is expressed using the random state particles extracted from the posterior probability. This filtering method has strong nonlinear adaptability and multi-modal processing ability. However, this method will cause the loss of sample validity and diversity in the resampling stage, resulting in the phenomenon of sample depletion. The particle swarm optimization particle filter algorithm is used to estimate the state of the vehicle, and achieve good results. However, the algorithm has the problems of large amount of calculation and great difficulty in engineering implementation [14]. Liu et al. proposed a modular integrated estimation algorithm to develop those intelligent driving systems for the preceding vehicle longitudinal and lateral states [15]. Based on the piecewise affine identification method, Sun et al. presented a novel approach to model the tire cornering characteristics [16]. Gao et al. proposed a new methodology to address the problem of tightly coupled GNSS/INS integration [17,18]. Guo et al. proposed an avoidance method for mobile robots in dynamic environments with dynamic obstacle avoidance risk region [19]. Korayem et al. proposed a new approach in estimating the lateral tire forces and hitch-forces of a vehicle-trailer system [20]. In order to estimate the vehicle states and the tire-road peak adhesion coefficient sequentially for 4WIDEV, based on adaptive-square-root-cubature-Kalman-filter (ASRCKF) and partitioned similarity-principle (SP), Chen et al. proposed a longitudinal-lateral cooperative estimation algorithm [21].
As a model-based recursive filter, the EKF uses the first-order linearization method to extend the Kalman filter. It is a relatively mature and widely used state and parameter estimation method. Although the EKF can estimate the parameters by expanding the parameters into state variables, it is difficult to estimate the time-varying parameters due to the noise model. In addition, the EKF method cannot handle the constraint of states. The receding horizon estimation method starts from the perspective of optimal control problem, and introduces the receding horizon strategy, which realizes the estimation of state and parameters by solving the optimization problem. Therefore, receding horizon estimation can better deal with the constrained estimation problem and the joint state and parameter estimation problem. Therefore, based on the existing vehicle state parameter estimation results, this paper further improves the estimation accuracy, robustness and calculation efficiency of the algorithm, which is of great significance to improve the performance of vehicle active safety system.

3-DOF vehicle model
In order to facilitate the real-time estimation of the vehicle state, a vehicle dynamics model (shown in Fig. 1) is established. The model ignores the effects of suspension dynamics and wheel camber. Moreover, it is assumed that the roll angle and slope angle of the road are both zero. Among them, the -axis represents the longitudinal direction, and the driving direction of the vehicle is positive; the -axis represents the lateral direction, and the left side is positive. The meanings of the symbols in the figure are as follows: is the front wheel angle; and are the longitudinal and lateral speeds; is the side slip angle of the center of mass; is the yaw rate; is the wheelbase; is the distance from the axle to the center of mass; and are the longitudinal and lateral forces of the tire. = , represents front and rear directions; = , represents left and right directions. According to D'Alembert's principle, the body dynamics equations representing longitudinal and lateral motions as well as yaw motion are as follows: = arctan ( ), where and are the longitudinal and lateral accelerations; is the moment of inertia of the vehicle around the -axis. Among them, the vehicle accelerations are as follows: where is the vehicle mass; = 2 2 ⁄ is the air resistance; is the air density; is the air resistance coefficient; is the frontal windward area of the vehicle. The vehicle state estimation model is established based on a 3-DOF vehicle model shown in Fig. 1.

Tire dynamic model
The brush tire model is different from the complex magic formula tire model. It has few calculation parameters and can accurately describe the nonlinear relationship between tire longitudinal and lateral force, slip angle, slip rate, tire vertical load and road adhesion coefficient, etc. The combined model of longitudinal and lateral forces is defined as: The longitudinal and lateral forces of the tire can be expressed as: where is the half contact length; is the half contact width; is the tread distribution stiffness; is the total slip rate; is the longitudinal slip rate; is the lateral slip rate; is the vertical load of the tire; is the road adhesion coefficient. Among them, the longitudinal and lateral slip rates of each tire can be expressed as: where is the center speed of wheel; is the slip angle of tire which calculation formula is: { , = − arctan ( The formula for calculating the vertical load of each tire is: where ℎ is the height of the center of mass of the vehicle.

Nonlinear vehicle system
The state vector of the nonlinear vehicle system is set as: The system input is = [ , ] , and the observation vector is:

Problem description
For the continuous system shown in Eq. (18), it can be transformed into the discrete system shown in Eq. (19) by means of finite difference method, direct multiple shooting method and other methods [22]: where and are the system noise sequence and measurement noise sequence respectively.
Generally, it is assumed that they obey zero mean Gaussian distribution: where and are the covariance matrices of system noise and measurement noise respectively. The state estimation problem of system Eq. (19) can be transformed into the following optimization problem: where: where is the current time, ̂0 is the estimated value of the initial value of the system state; 0 is the covariance matrix of ̂0; { } =0 −1 represents the noise sequence from time 0 to time − 1. Eq. (21) of the optimization problem uses all the measured data, so it is called full information estimation. The amount of computation of full information estimation will rapidly increase to an unacceptable level with the growth of . Therefore, the receding horizon strategy is introduced to limit the dimension of full information estimation and form a receding horizon estimation method. The objective function of receding horizon estimation method Φ becomes: where − ( − ) is the arrival cost which is defined as follows according to the principle of forward dynamic programming: where ( , 0 , { }) indicates the value of the state at time when the initial value is 0 and subjected to the noise sequence { }. It can be seen from Eq. (24) that the arrival cost is a function with very complex expression. Therefore, the arrival cost is generally expressed by the following quadratic function: where Φ − * is the optimal value of optimization index at − time; ̅ − is the state estimation at time − ; − is the weight matrix of the estimation error.
Due to that Φ − * is a constant, the arrival cost equation presumes calculating ̅ − and − . At present, most researches often use the Kalman filter and its error covariance matrix updating formula to calculate the arrival cost, but this method does not involve the results of receding horizon estimation, which is not conducive to the improvement of estimation accuracy and speed. From the perspective of solving optimization problems, this paper presents a calculation method of arrival cost based on QR decomposition. Since Φ − * is a constant, it has no effect on the solution of the RHE problem, so it is actually important to update the arrival cost to calculate ̅ − and − according to the estimation results. The widely used arrival cost updating algorithm is used together with the error covariance matrix updating method of EKF to calculate − :

Arrival cost calculation method of based on QR decomposition
At time + 1:   (31) In order to obtain the analytical solution of the optimization problem shown in Eq. (31), the nonlinear functions and are linearized at the optimal estimation − * obtained by receding horizon estimation: It is set that: The arrival cost calculation problem shown in Eq. (36) is transformed into the least square problem as follows: In order to solve the least square problem shown in Eq. (38), is decomposed by QR decomposition: where is an orthogonal matrix; 1 and 2 are upper triangular matrices; 12 is the lower matrix.
The variable in Eq. (41) is only − , so the solution of the least squares problem is: To sum up, the update calculation method of arrival cost is as follows.
Step 2: Functions and are linearized: Step 3: Matrices and are constructed: Step 4: decomposition for and calculation for is done: ].
Step 5: ̅ − +1 and − +1 are calculated: Then the updating of arrival cost is completed.

Solution of rolling horizon estimation problem
It can be seen from Sections 3.1 and 3.2 that the state estimation problem of vehicle is transformed into a fixed dimension optimization problem. However, it needs to sample the system state and input. Therefore, for the continuous system of vehicle, it is necessary to choose an appropriate method to complete the discretization of the system equation. Considering the strong nonlinearity and constant sampling rate of the system, the direct multiple shooting method with constant discrete node spacing and high accuracy is selected. This method discretizes the state and control variables on discrete nodes with equal spacing. It is assumed that the control variable between adjacent nodes is constant. And the constraint of equal state variables at nodes is added to realize the discretization of continuous system. Finally, the state estimation problem is transformed into a nonlinear programming problem shown in Eq.
In this paper, a more mature sequential quadratic programming method is used to solve the nonlinear programming problem in Eq. (50).
Step 1: Initialization. 0 , −1 , −1 and the initial state estimation ̅ 0 as well as the window length N of the receding horizon are set.
Step 2: When < , the state estimation value ̅ and covariance matrix are calculated using EKF updating formula.
Step 3: When ≥ , the sequential quadratic programming method is used to solve the nonlinear programming problem Eq. (50).
Step 4: ̅ − and − are calculated using the arrival cost update strategy as described in Section 3.2.
The flow of the RHE-QR algorithm is shown in Fig. 2.
Proof. The minimum sequence Φ * obtained by the arrival cost updating algorithm is a non-decreasing sequence. It indicates that the arrival cost value grows over time.
It is set that − +1 = ̅ − +1 , then Eqs. (51)-(52) can be obtained according to Eq. (41): The arrival cost given by the arrival cost updating algorithm is bounded, indicating that the influence of historical data on the estimation results is limited and will not increase indefinitely. That is, for any vector , there is: where max is the largest eigenvalue of the matrix −1 . Eq. (54) can be obtained according to Eq. (37): The arrival cost algorithm can guarantee the stability of MHE.

Numerical simulation
To verify the performance of the proposed algorithm, a certain type of vehicle is verified by a simulation test in the ADAMS software. Considering the difference between the test vehicle and the vehicle model parameters in the ADAMS software, the specific parameters of the vehicle model in ADAMS (wheelbase, wheelbase, vehicle mass, wheel radius, etc.) are adjusted to be consistent with the test vehicle. The simulation parameters are shown in Table 1. ADAMS software uses interactive graphic environment, parts library, constraint library and force library to create a fully parameterized geometric model of mechanical system. Its solver adopts the Lagrange equation method in the multi-rigid-body system dynamics theory to establish a system dynamics equation, carry out static, kinematic and dynamic analysis of virtual mechanical system, and output displacement, velocity, acceleration and reaction force curves. The simulation of ADAMS software can be used to predict the performance of mechanical system, motion range, collision detection, peak load and calculate the input load of a finite element.
On the one hand, ADAMS is an application software for virtual prototype analysis. Users can use the software to create easily statics, kinematics and dynamic analysis on virtual mechanical systems. On the other hand, it is a virtual prototype analysis and development tool. Its open program structure and various interfaces can become a secondary development tool platform for users in special industries to conduct special types of virtual prototype analysis.

Chassis model
Chassis connects other systems of the whole vehicle, ignoring the specific structure of the power system and the body. The transmission system and the braking system which are simplified as rigid bodies are located on the chassis. And the center of mass is also located on the chassis. In the paper, a spherical mass block represents the concentrated mass. The size and position of the spherical mass block are not important. As long as being a part of the steering system, suspension and other components are connected to the chassis. The mass and moment of inertia of the whole vehicle can be modified by adjusting the properties of the sphere. At the same time, several hard points are defined to determine the positions of the wheels and the body.

Body system model
The vehicle body condition is directly related to the driver's riding comfort. The measurement of the vertical acceleration of the vehicle body, which is the evaluation index of ride comfort, needs to be carried out on the vehicle body. And the adjustment of the position and the center of mass of the whole vehicle is also realized by changing the vehicle body configuration. So, the vehicle body modeling is very important.

Powertrain system model
The powertrain system in ADAMS/Car can create a wheel driving torque in real time, so there is no need to model the actual structure. Instead, the three structures of the engine, clutch and transmission are integrated, and function templates are used to simulate their functions. Since engine excitation is also one of the excitation sources that cause automobile vibration, in order to reduce the impact of engine imbalance on vehicle body vibration, the engine powertrain of the automobile is always installed on the subframe through several elastic supports.

Steering system
The steering system is mainly composed of the following rigid bodies: steering wheel, steering column, intermediate shaft, steering shaft, steering rack, steering gear sleeve, etc. The steering wheel is connected to the chassis system through a rotating pair, and is connected to the steering column through a cylinder pair. The steering column is connected to the intermediate shaft through a constant speed pair. The intermediate shaft is connected to the steering shaft through a constant speed pair. The steering shaft is connected to the steering gear sleeve through a rotating pair. The steering gear sleeve is connected with the steering rack through the sliding pair. The rotary motion of the steering wheel is converted into a linear motion through the steering rack. And the rack drives the tie rod to reciprocate and rotate with the steering knuckle to realize car steering.

Suspension system
The vehicle suspension system studied in this paper uses a coil spring McPherson suspension. In the virtual prototype model, the suspension system is simplified into the following six parts: steering knuckle, swing arm, coil spring, air spring, shock absorber strut assembly, steering tie rod. The inner end of the swing arm is connected to the body through a rotating pair. And the outer end is connected to the lower end of the steering knuckle through a spherical pair. The upper end of the steering knuckle is connected to the lower end of the shock absorber strut assembly through a prism pair. And the middle part is connected to the wheel hub through a rotating pair and a spherical pair respectively. The wheel hub is connected with the steering tie rod. The shock absorber piston rod is connected to the body through a spherical pair. The shock absorber piston rod and the shock absorber cylinder are connected through a cylindrical pair. The coil spring and the shock absorber are strung together. And the middle part also includes some bushing connections used to make the virtual model more realistic.
By adding rubber bushings and other restraining elements, the above subsystems can be assembled to establish a vehicle model suitable for simulation shown in Fig. 3. Combining with the lateral acceleration response curve in Fig. 4(a), it can be seen that when the vehicle is driving at the turning point under the double lane change condition, with the sudden increase of the steering wheel angle, the lateral acceleration increases significantly, and there is a risk of losing the stability. It can be seen from Fig. 4(b) that the RHE-QR algorithm can better track the reference value of the yaw rate output by the virtual experiment. It can be seen from Fig. 4(c) that in the initial stage and the end stage of the double lane change condition, the proposed estimation algorithm can achieve a good estimation result. But when the vehicle is running at turning of the double lane change road, due to a sudden change of steering wheel angle at this time, the driving state of the vehicle tends to be unstable, but the RHE-QR algorithm can still better track the reference value of the side slip angle. Fig. 5 depicts the comparison results of the yaw rate and the side slip angle studied with the different methods (RHE-QR and UKF) for passing a double lane change road maneuver.
From Fig. 5, it can be seen that the accuracy of the RHE-QR estimation method is significantly higher than that of the UKF method. Compared with the traditional UKF updating method, the updating arrival cost with QR decomposition has a higher accuracy while the simulation condition is similar. This is due to the fact that the updating arrival cost with the strategy of QR decomposition utilizes the results of RHE-QR estimation method to form a feedback mechanism,

Experimental verification
According to BS ISO 3888-2002, a double lane change test is carried out to verify the effectiveness of the algorithm collecting the yaw rate and lateral acceleration as well as the side slip angle of the vehicle. It is very dangerous to establish a real vehicle test on a double lane change road with a higher speed than 45 km/h. This speed is set up as the optimal in terms of the driver's safety. A block diagram of real vehicle test system is shown in Fig. 6  And the main measurement devices are shown in Fig. 7. And the real test vehicle which photos are taken on the test grounds of Weifang University is shown in Fig. 8. The results of the double lane change test are shown in Fig. 9. It can be seen from Fig. 9(a) that the vehicle speed decreased during the double lane change operation, and the vehicle speed is 34 km·h -1 when the test is completed. It can be seen from Figs. 9(c-d) that the overall error of the yaw rate obtained based on the RHE-QR estimation algorithm proposed in this paper and measured by gyroscope is small. Combining with the lateral acceleration response curve in Fig. 9(b), it can be seen that when the vehicle is driving through a turning point under the double lane change condition, with the sudden increase of the steering angle, the lateral acceleration increases significantly, greater than 0.4 g which indicates that the vehicle is in nonlinear state having the risk of losing stability. Due to the existence of the vehicle dynamics model, tire model and sensor measurement errors, the estimated error of the yaw rate is relatively increased, but the error returns to a smaller range thereafter. The overall estimation effect is good, and it maintains a good consistency with the actual measured value. In conclusion, the RHE-QR estimation algorithm proposed in this paper has good estimation accuracy and robustness.

Conclusions
In this paper, a vehicle state and parameter estimation method are proposed. Firstly, based on the 3-DOF vehicle model and the magic formula tire model, aiming at the characteristics of nonlinearity, uncertainty and rigid body/elasticity coupling of vehicle, a joint state and parameter estimation method based on QR decomposition and receding horizon estimation is proposed. Based on the MATLAB simulation platform, the state and parameter estimation algorithm proposed in this paper is verified in the double lane change condition, and compared with the UKF estimation algorithm. The simulation results show that the maximum errors between the estimated values of the side slip angle and yaw rate obtained by the estimation algorithm proposed in this paper and the reference values taken from a virtual experiment are smaller than the estimated values of the UKF algorithm. Especially when the vehicle is in a strong nonlinear state, the reference value can still be tracked well, which provides a guarantee for vehicle stability control under extreme conditions. The test results show that the estimated yaw rate value and the actual measured value can maintain a good consistency indicating good estimation accuracy. This paper only estimates the lateral acceleration and the side slip angle as well as the yaw rate, but there are many factors that affect the driving stability of the vehicle, such as tire lateral force, longitudinal force and so on, which were ignored in this paper. That is why, due to the limitation of test conditions, only the accuracy of yaw rate estimation can be verified. Therefore, the estimation of tire lateral force, longitudinal force and other state variables as well as real vehicle test will be the focus of future research.